Merge branch '15-follow-core-313' into 'devel'

Resolve "follow core:#313"

Closes #15

See merge request !21

src/processor/processor_closeloop_icp.cpp

@@ -145,13 +145,12 @@ FrameBasePtrList ProcessorCloseloopIcp::selectCandidates(FrameBasePtr _keyframe_
     //Consider only key_frames from 1 to n - match_past_key_frame_
     // std::copy_if(frames.begin(), frames.end(), std::back_inserter(candidates), [&](FrameBasePtr _frame) { if(_frame->isKey()) {key_frames_counter++; return (key_frames_counter % match_past_key_frame_ == 0);
     //                                                                                                                     }else{ return false;};});
-        for(auto it=trajectory->rbegin(); it != trajectory->rend(); it++){
-            if((*it)->isKey()){
-                // WOLF_DEBUG("TIMESTAMP KEY FRAME ", (*it)->id(), " ", (*it)->getTimeStamp());
-                key_frames_counter++;
-                if (key_frames_counter > recent_key_frames_ignored_
-                    and (_keyframe_ptr->getP()->getState() - (*it)->getP()->getState()).norm() < laser_scan_params_.range_max_) candidates.push_back(*it);
-            }
+    for(auto it=trajectory->rbegin(); it != trajectory->rend(); it++)
+    {
+        // WOLF_DEBUG("TIMESTAMP KEY FRAME ", (*it)->id(), " ", (*it)->getTimeStamp());
+        key_frames_counter++;
+        if (key_frames_counter > recent_key_frames_ignored_
+                and (_keyframe_ptr->getP()->getState() - (*it)->getP()->getState()).norm() < laser_scan_params_.range_max_) candidates.push_back(*it);
     }
     WOLF_DEBUG("%%%%%%%%%%%%%%%%%% CANDIDATES SIZE ", candidates.size());
     return candidates;

test/gtest_processor_odom_icp.cpp

@@ -160,20 +160,14 @@ TEST_F(ProcessorOdomIcp_Test, solve)
     }
 
     for (auto F : *problem->getTrajectory())
-    {
-        if (F->isKey())
-            F->perturb(0.5);
-    }
+        F->perturb(0.5);
 
     std::string report =    solver->solve(SolverManager::ReportVerbosity::BRIEF);
     WOLF_TRACE(report);
 
     for (auto F : *problem->getTrajectory())
     {
-        if (F->isKey())
-        {
-            ASSERT_MATRIX_APPROX(F->getState().vector("PO") , x0.vector("PO") , 1e-6);
-        }
+        ASSERT_MATRIX_APPROX(F->getState().vector("PO") , x0.vector("PO") , 1e-6);
     }
 }