implemented and tested ignoring frames for loop closure

include/laser/processor/processor_loop_closure_icp_new.h

@@ -24,7 +24,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosureIcpNew);
 
 struct ParamsProcessorLoopClosureIcpNew : public ParamsProcessorLoopClosure
 {
-    int recent_key_frames_ignored;
+    int recent_frames_ignored, frames_ignored_after_loop;
     double max_error_threshold;
     double min_points_percent;
 
@@ -35,7 +35,8 @@ struct ParamsProcessorLoopClosureIcpNew : public ParamsProcessorLoopClosure
     ParamsProcessorLoopClosureIcpNew(std::string _unique_name, const ParamsServer& _server):
         ParamsProcessorLoopClosure(_unique_name, _server)
     {
-        recent_key_frames_ignored = _server.getParam<int>    (prefix + _unique_name + "/recent_key_frames_ignored");
+        recent_frames_ignored     = _server.getParam<int>    (prefix + _unique_name + "/recent_frames_ignored");
+        frames_ignored_after_loop = _server.getParam<int>    (prefix + _unique_name + "/frames_ignored_after_loop");
         max_error_threshold       = _server.getParam<double> (prefix + _unique_name + "/max_error_threshold");
         min_points_percent        = _server.getParam<double> (prefix + _unique_name + "/min_points_percent");
 
@@ -75,7 +76,8 @@ struct ParamsProcessorLoopClosureIcpNew : public ParamsProcessorLoopClosure
     std::string print() const override
     {
         return "\n" + ParamsProcessorBase::print()
-            + "recent_key_frames_ignored: " + std::to_string(recent_key_frames_ignored) + "\n"
+            + "recent_frames_ignored: " + std::to_string(recent_frames_ignored) + "\n"
+            + "frames_ignored_after_loop: " + std::to_string(frames_ignored_after_loop) + "\n"
             + "max_error_threshold: " + std::to_string(max_error_threshold) + "\n"
             + "min_points_percent: " + std::to_string(min_points_percent) + "\n";
     }
@@ -104,19 +106,23 @@ class ProcessorLoopClosureIcpNew : public ProcessorLoopClosure
         // ICP algorithm
         std::shared_ptr<laserscanutils::ICP> icp_tools_ptr_;
 
+        MatchLoopClosurePtr last_loop_closure_;
+
     public:
         ProcessorLoopClosureIcpNew(ParamsProcessorLoopClosureIcpNewPtr _params);
 
         WOLF_PROCESSOR_CREATE(ProcessorLoopClosureIcpNew, ParamsProcessorLoopClosureIcpNew);
         void configure(SensorBasePtr _sensor) override;
 
+        ParamsProcessorLoopClosureIcpNewPtr getParams() const;
+
     protected:
 
         /** \brief Returns if findLoopClosures() has to be called for the given capture
          *
-         * Always true in this case
+         * This case avoids searching if a recent loop closure
          */
-        bool voteFindLoopClosures(CaptureBasePtr cap) override {return true;};
+        bool voteFindLoopClosures(CaptureBasePtr cap) override;
 
         /** \brief detects and emplaces all features of the given capture
          *

src/processor/processor_loop_closure_icp_new.cpp

@@ -9,7 +9,8 @@ namespace wolf{
 // Constructor
 ProcessorLoopClosureIcpNew::ProcessorLoopClosureIcpNew(ParamsProcessorLoopClosureIcpNewPtr _params) :
     ProcessorLoopClosure("ProcessorLoopClosureIcpNew", 2, _params),
-    params_loop_closure_icp_(_params)
+    params_loop_closure_icp_(_params),
+    last_loop_closure_(nullptr)
 {
     icp_tools_ptr_ = std::make_shared<ICP>();
 }
@@ -20,6 +21,32 @@ void ProcessorLoopClosureIcpNew::configure(SensorBasePtr _sensor)
     laser_scan_params_  = sensor_laser_->getScanParams();
 }
 
+ParamsProcessorLoopClosureIcpNewPtr ProcessorLoopClosureIcpNew::getParams() const
+{
+    return params_loop_closure_icp_;
+}
+
+bool ProcessorLoopClosureIcpNew::voteFindLoopClosures(CaptureBasePtr cap)
+{
+    if (not last_loop_closure_)
+        return true;
+
+    if (params_loop_closure_icp_->frames_ignored_after_loop <= 0)
+        return true;
+
+    auto last_frm = last_loop_closure_->capture_target->getFrame();
+    for (auto i = 1; i <= params_loop_closure_icp_->frames_ignored_after_loop; i++)
+    {
+        last_frm = last_frm->getNextFrame();
+        if (not last_frm or last_frm == cap->getFrame())
+        {
+            return false;
+        }
+    }
+
+    return true;
+}
+
 std::map<double,MatchLoopClosurePtr> ProcessorLoopClosureIcpNew::findLoopClosures(CaptureBasePtr _capture)
 {
     std::map<double,MatchLoopClosurePtr> match_lc_map;
@@ -27,7 +54,7 @@ std::map<double,MatchLoopClosurePtr> ProcessorLoopClosureIcpNew::findLoopClosure
     auto cap_laser = std::dynamic_pointer_cast<CaptureLaser2d>(_capture);
     assert(cap_laser != nullptr);
 
-    //Consider only key_frames from 1 to n - match_past_key_frame_
+    // Ignore the most recent 'recent_frames_ignored' frames
     const auto &trajectory = getProblem()->getTrajectory();
     int n_ignored = 0;
     for(auto frm_it  = trajectory->rbegin();
@@ -37,7 +64,7 @@ std::map<double,MatchLoopClosurePtr> ProcessorLoopClosureIcpNew::findLoopClosure
         if (frm_it->second == _capture->getFrame())
             continue;
 
-        if (n_ignored < params_loop_closure_icp_->recent_key_frames_ignored)
+        if (n_ignored < params_loop_closure_icp_->recent_frames_ignored)
         {
             n_ignored++;
             continue;
@@ -172,6 +199,9 @@ void ProcessorLoopClosureIcpNew::emplaceFactors(MatchLoopClosurePtr match)
                                                             shared_from_this(),
                                                             params_->apply_loss_function,
                                                             TOP_LOOP);
+
+    // store last frame
+    last_loop_closure_ = match;
 }
 
 }

test/gtest_processor_loop_closure_icp.cpp

@@ -27,9 +27,10 @@ class ProcessorLoopClosureIcp_Test : public testing::Test
                                                                                          laser_root_dir + "/test/yaml/sensor_laser_2d.yaml"));
             auto params = std::make_shared<ParamsProcessorLoopClosureIcpNew>();
 
-            params->recent_key_frames_ignored = 0;
-            params->max_error_threshold       = 0.1;
-            params->min_points_percent        = 50;
+            params->recent_frames_ignored                   = 0;
+            params->frames_ignored_after_loop               = 0;
+            params->max_error_threshold                     = 0.1;
+            params->min_points_percent                      = 50;
 
             params->icp_params.use_point_to_line_distance   = 1;
             params->icp_params.max_correspondence_dist      = 1e9;
@@ -153,6 +154,68 @@ TEST_F(ProcessorLoopClosureIcp_Test, loop_closure)
     }
 }
 
+TEST_F(ProcessorLoopClosureIcp_Test, loop_closure_ignore_previous)
+{
+    processor->getParams()->recent_frames_ignored = 3;
+
+    TimeStamp t(0.0);
+
+    for (int i = 0; i < 6; i++)
+    {
+        auto frm = emplaceFrame(t, Vector3d::Zero());
+        auto cap = emplaceCaptureLaser2d(frm, generateRanges(i));
+
+        processor->keyFrameCallback(frm, 0.1);
+        if (i > 5)
+        {
+            EXPECT_EQ(cap->getFeatureList().size(), 1);
+            EXPECT_EQ(cap->getFeatureList().front()->getFactorList().size(), 1);
+            EXPECT_EQ(cap->getFeatureList().front()->getFactorList().front()->getType(), "FactorRelativePose2dWithExtrinsics");
+            EXPECT_FALSE(cap->getFeatureList().front()->getFactorList().front()->getFrameOther() == nullptr);
+            EXPECT_EQ(cap->getFeatureList().front()->getFactorList().front()->getFrameOther()->getTimeStamp(), TimeStamp(i%3));
+            problem->print(4,1,1,1);
+        }
+        else
+        {
+            EXPECT_TRUE(cap->getFeatureList().empty());
+            EXPECT_TRUE(frm->getConstrainedByList().empty());
+            problem->print(4,1,1,1);
+        }
+        t += 1;
+    }
+}
+
+TEST_F(ProcessorLoopClosureIcp_Test, loop_closure_frames_ignored_after_loop)
+{
+    processor->getParams()->frames_ignored_after_loop = 2;
+
+    TimeStamp t(0.0);
+
+    for (int i = 0; i < 6; i++)
+    {
+        auto frm = emplaceFrame(t, Vector3d::Zero());
+        auto cap = emplaceCaptureLaser2d(frm, generateRanges(i));
+
+        processor->keyFrameCallback(frm, 0.1);
+        if (i == 3)
+        {
+            EXPECT_EQ(cap->getFeatureList().size(), 1);
+            EXPECT_EQ(cap->getFeatureList().front()->getFactorList().size(), 1);
+            EXPECT_EQ(cap->getFeatureList().front()->getFactorList().front()->getType(), "FactorRelativePose2dWithExtrinsics");
+            EXPECT_FALSE(cap->getFeatureList().front()->getFactorList().front()->getFrameOther() == nullptr);
+            EXPECT_EQ(cap->getFeatureList().front()->getFactorList().front()->getFrameOther()->getTimeStamp(), TimeStamp(i%3));
+            problem->print(4,1,1,1);
+        }
+        else
+        {
+            EXPECT_TRUE(cap->getFeatureList().empty());
+            EXPECT_TRUE(frm->getConstrainedByList().empty());
+            problem->print(4,1,1,1);
+        }
+        t += 1;
+    }
+}
+
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);