removing wrong landmarks and bug fixed in merge landmarks

src/landmark/landmark_polyline_2D.cpp

@@ -304,9 +304,9 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
     }
 
     WOLF_DEBUG("tryClose landmark ",id()," point ids: ");
-    for (auto id : getValidIds())
-        std::cout << id << " ";
-    std::cout << std::endl;
+    //for (auto id : getValidIds())
+    //    std::cout << id << " ";
+    //std::cout << std::endl;
 
     std::vector<std::pair<int, int> > overlapped_ids;
 
@@ -424,7 +424,8 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
                 WOLF_DEBUG("The not defined extremes are overlapped",
                            "\n\tlast_proj_2_first_def:  ", last_proj_2_first_def,
                            "\n\tfirst_proj_2_first_def: ", first_proj_2_first_def);
-                overlapped_ids.push_back(std::pair<int,int>(getFirstId(), getLastId()));
+                overlapped_ids.push_back(std::pair<int,int>(getFirstId(), getPrevValidId(getLastId())));
+                overlapped_ids.push_back(std::pair<int,int>(getLastId(), getNextValidId(getFirstId())));
             }
         }
     }

src/processor/polyline_2D_utils.cpp

@@ -707,7 +707,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
 
             // Store matches ordered by the squared norm of the transformation (I know, mixing mts and rads..)
             matches.push_back(match);
-            std::cout << "match added to list of matches!\n";
+            //std::cout << "match added to list of matches!\n";
         }
     }
 

src/processor/processor_tracker_feature_polyline_2D.cpp

@@ -1049,6 +1049,22 @@ void ProcessorTrackerFeaturePolyline2D::postProcess()
                                               params_tracker_feature_polyline_->match_landmark_overlap_n_min,
                                               params_tracker_feature_polyline_->match_landmark_overlap_n_defined_min);
     }
+
+    // Remove "wrong" landmarks
+    LandmarkBasePtrList lmk_list = getProblem()->getMap()->getLandmarkList();
+    for (auto lmk_it = lmk_list.begin(); lmk_it != lmk_list.end(); lmk_it++)
+        if ((*lmk_it)->getType() == "POLYLINE 2D")
+        {
+            auto points_map = std::static_pointer_cast<LandmarkPolyline2D>(*lmk_it)->getPointStatePtrMap();
+
+            for (auto point_pair = points_map.begin(); point_pair != points_map.end(); point_pair++)
+                if (point_pair->second->getState().maxCoeff() > 1e6 || point_pair->second->getState().minCoeff() < -1e6)
+                {
+                    (*lmk_it)->remove();
+                    break;
+                }
+        }
+
     //WOLF_DEBUG("PTF ", getName(), ": ", "untracked_features_last_ has ", untracked_features_last_.size() , " features");
     //WOLF_DEBUG_COND(last_ptr_!=nullptr,"PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
 }
@@ -1267,9 +1283,12 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
         assert(lmk_match->feature_from_id_ == 0 || lmk_match->feature_to_id_ == _feat_ptr->getNPoints()-1);
 
         // store in list
-        lmk_matches[lmk_match->normalized_score_]= lmk_match;
-        WOLF_DEBUG("match stored!");
-        assert(lmk_match->check() && "tryMatchWithLandmark: wrong match");
+        if (lmk_match->check())
+        {
+            lmk_matches[lmk_match->normalized_score_]= lmk_match;
+            WOLF_DEBUG("match stored!");
+            assert(lmk_match->check() && "tryMatchWithLandmark: wrong match");
+        }
     }
 }
 
@@ -1396,6 +1415,8 @@ LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline2D::concatenateFeature
 bool ProcessorTrackerFeaturePolyline2D::tryUpdateMatchTransformation(LandmarkMatchPolyline2DPtr& lmk_match) const
 {
     //WOLF_DEBUG("PTFP ", getName(), "::tryUpdateMatchTransformation: ");
+    if (!lmk_match->check())
+        return false;
 
     assert(lmk_match->check());