add loop use threshold

Author: PeiliangLi

VINS_ios/ViewController.mm

@@ -746,6 +746,12 @@ -(void)process
                     if(vins.Headers[i] == vins.front_pose.header)
                     {
                         KeyFrame* cur_kf = keyframe_database.getKeyframe(vins.front_pose.cur_index);
+                        if (abs(vins.front_pose.relative_yaw) > 30.0 || vins.front_pose.relative_t.norm() > 10.0)
+                        {
+                            printf("Wrong loop\n");
+                            cur_kf->removeLoop();
+                            break;
+                        }
                         cur_kf->updateLoopConnection(vins.front_pose.relative_t,
                                                      vins.front_pose.relative_q,
                                                      vins.front_pose.relative_yaw);

VINS_ios/loop/keyframe.cpp

@@ -359,6 +359,11 @@ void KeyFrame::detectLoop(int index)
     loop_index = index;
 }
 
+void KeyFrame::removeLoop()
+{
+    has_loop = false;
+}
+
 int KeyFrame::HammingDis(const BRIEF::bitset &a, const BRIEF::bitset &b)
 {
     BRIEF::bitset xor_of_bitset = a ^ b;

VINS_ios/loop/keyframe.h

@@ -86,6 +86,8 @@ class KeyFrame
 
     void detectLoop(int index);
 
+    void removeLoop();
+    
     int HammingDis(const BRIEF::bitset &a, const BRIEF::bitset &b);
 
 	// data