still debuging nullptr in correctFeatureDrift

include/laser/processor/polyline_2D_utils.h

@@ -71,8 +71,8 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
                                      const Eigen::MatrixXs& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
                                      const Scalar& max_sq_error,
                                      const Scalar& min_overlapping_dist,
-                                     const int& min_N_overlapped,
-                                     const int& min_N_defined_overlapped,
+                                     int min_N_overlapped,
+                                     int min_N_defined_overlapped,
                                      bool both_exceeding_A_allowed_ = true,
                                      bool both_exceeding_B_allowed_ = true);
 

include/laser/processor/processor_tracker_feature_polyline_2D.h

@@ -252,6 +252,16 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
         {
             return untracked_features_last_;
         }
+
+        const FeatureBasePtrList& getIncomingNewFeatures() const
+        {
+            return untracked_features_incoming_;
+        }
+
+        const FeatureBasePtrList& getIncomingKnownFeatures() const
+        {
+            return known_features_incoming_;
+        }
 };
 
 } /* namespace wolf */

src/landmark/landmark_match_polyline_2D.cpp

@@ -172,6 +172,7 @@ bool LandmarkMatchPolyline2D::check(bool check_partial_match) const
 
 Eigen::Array<Scalar,Eigen::Dynamic,1> LandmarkMatchPolyline2D::computeSqDistArray() const
 {
+	std::cout << "LandmarkMatchPolyline2D::computeSqDistArray\n";
     assert(pl_feature_ != nullptr);
     assert(pl_landmark_ != nullptr);
 

src/processor/polyline_2D_utils.cpp

@@ -168,13 +168,13 @@ Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_au
 MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
                                      const Eigen::MatrixXs& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
                                      const Scalar& sq_error_max,
-                                     const Scalar& overlap_dist_min,
-                                     const int& overlap_N_min,
-                                     const int& overlap_N_defined_min,
+                                     const Scalar& overlap_dist_ratio_min,
+                                     int overlap_N_min,
+                                     int overlap_N_defined_min,
                                      bool both_exceeding_A_allowed_,
                                      bool both_exceeding_B_allowed_)
 {
-    bool print = false;
+    bool print = true;
     if (print)
     {
         std::cout << "computeBestSqDist...\n";
@@ -185,11 +185,22 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
         std::cout << "\tpoints_A:\n" << _points_A.topRows(2) << std::endl;
         std::cout << "\tpoints_B:\n" << _points_B.topRows(2) << std::endl;
         std::cout << "\tsq_error_max: " << sq_error_max << std::endl;
-        std::cout << "\toverlap_dist_min: " << overlap_dist_min << std::endl;
+        std::cout << "\toverlap_dist_ratio_min: " << overlap_dist_ratio_min << std::endl;
         std::cout << "\toverlap_N_min: " << overlap_N_min << std::endl;
         std::cout << "\toverlap_N_defined_min: " << overlap_N_defined_min << std::endl;
     }
 
+    if (overlap_N_min < 2)
+    {
+    	WOLF_WARN("overlap_N_min should be at least 2! It is a sufficient condition (if enough overlapping distance ratio)");
+    	overlap_N_min = 2;
+    }
+    if (overlap_N_defined_min < 1)
+    {
+    	WOLF_WARN("overlap_N_defined_min should be at least 1! It is a sufficient condition");
+    	overlap_N_defined_min = 1;
+    }
+
     // ASSERTIONS if closed -> all points defined
     assert((_closed_A &&_first_defined_A && _last_defined_A) || !_closed_A);
     assert((_closed_B &&_first_defined_B && _last_defined_B) || !_closed_B);
@@ -215,10 +226,10 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
     {
         //std::cout << "Auto-calling switching A<->B...\n";
 
-        MatchPolyline2DPtr best_match = computeBestSqDist(_points_B, _first_defined_B, _last_defined_B, _closed_B,
+        best_match = computeBestSqDist(_points_B, _first_defined_B, _last_defined_B, _closed_B,
                                                           _points_A, _first_defined_A, _last_defined_A, _closed_A,
                                                           sq_error_max,
-                                                          overlap_dist_min,
+                                                          overlap_dist_ratio_min,
                                                           overlap_N_min,
                                                           overlap_N_defined_min,
                                                           both_exceeding_B_allowed_,
@@ -309,54 +320,58 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
         bool to_B_defined   = (to_B != 0 || _first_defined_B) && (to_B != last_B || _last_defined_B);       //((to_B   != 0 && to_B   != last_B) || _last_defined_B);
 
         // SUFFICIENT CONDITIONS
-        //      COND 1: sufficient overlapping points
-        //      COND 2: sufficient overlapping defined points
-        //      COND 3: sufficient overlapping distance
+        //      COND 1: sufficient overlapping defined points
+        //      COND 2: sufficient overlapping points (if no defined points, min overlapping distance)
         int N_defined_overlap = std::max(0,N_overlap - (from_A_defined && from_B_defined ? 0 : 1) - (to_A_defined && to_B_defined ? 0 : 1));
-        Scalar overlap_dist;
-        if (N_overlap <= 1)
-            overlap_dist = 0;
-        else
-        {
-            Eigen::Vector2s p_from_A = _points_A.col(from_A).head(2);
-            Eigen::Vector2s p_to_A   = _points_A.col(to_A).head(2);
-            Eigen::Vector2s p_from_B = _points_B.col(from_B).head(2);
-            Eigen::Vector2s p_to_B   = _points_B.col(to_B).head(2);
-            Eigen::Vector2s vA = p_to_A - p_from_A;
-            Eigen::Vector2s vB = p_to_B - p_from_B;
-
-            // THE MINIMUM OF ALL 6 possible overlap distances
-            Eigen::Vector6s overlap_dists;
-            // 1: fromA - toA
-            overlap_dists(0) = vA.norm();
-            // 2: fromB - toB
-            overlap_dists(1) = vB.norm();
-            // 3: fromA - projected(toB)
-            overlap_dists(2) = (vA.normalized()).dot(p_to_B - p_from_A);
-            // 4: fromB - projected(toA)
-            overlap_dists(3) = (vB.normalized()).dot(p_to_A - p_from_B);
-            // 5: projected(fromB) - toA
-            overlap_dists(4) = (-vA.normalized()).dot(p_from_B - p_to_A);
-            // 6: projected(fromA) - toB
-            overlap_dists(5) = (-vB.normalized()).dot(p_from_A - p_to_B);
-
-            //std::cout << "\tdist2.mean() = " << dist2.mean() << std::endl;
-            //std::cout << "\tmax_sq_error - dist2.mean() = " << max_sq_error - dist2.mean() << std::endl;
-            if (print)
-                std::cout << "\toverlap_dists = " << overlap_dists.transpose() << std::endl;
-            overlap_dist = overlap_dists.minCoeff();
-        }
-
-        // if none of the sufficient conditions hold, continue
-        if ( N_overlap < overlap_N_min &&
-             N_defined_overlap < overlap_N_defined_min &&
-             N_defined_overlap <= 0 && N_overlap < 2 &&
-             overlap_dist < overlap_dist_min)
-        {
-            if (print)
-                std::cout  << "\t\tnone of the sufficient conditions hold\n";
-            continue;
-        }
+		if ( N_defined_overlap < overlap_N_defined_min &&
+			 N_overlap < overlap_N_min )
+		{
+			if (print)
+				std::cout  << "\t\tnone of the sufficient conditions hold\n";
+			continue;
+		}
+		// additional overlapping distance contidion for 2 not defined points
+		if (N_defined_overlap == 0)
+		{
+			Scalar overlap_dist;
+			WOLF_WARN_COND(N_overlap != 2, "THIS SHOULDN'T HAPPEN. \nN_defined_overlap: ", N_defined_overlap, "\nN_overlap: ", N_overlap, "\noverlap_N_defined_min: ", overlap_N_defined_min, "\noverlap_N_min: ", overlap_N_min)
+			assert(N_overlap == 2);
+			Eigen::Vector2s p_from_A = _points_A.col(from_A).head(2);
+			Eigen::Vector2s p_to_A   = _points_A.col(to_A).head(2);
+			Eigen::Vector2s p_from_B = _points_B.col(from_B).head(2);
+			Eigen::Vector2s p_to_B   = _points_B.col(to_B).head(2);
+			Eigen::Vector2s vA = p_to_A - p_from_A;
+			Eigen::Vector2s vB = p_to_B - p_from_B;
+
+			// THE MINIMUM OF ALL 6 possible overlap distances
+			Eigen::Vector6s overlap_dists;
+			// 1: fromA - toA
+			overlap_dists(0) = vA.norm();
+			// 2: fromB - toB
+			overlap_dists(1) = vB.norm();
+			// 3: fromA - projected(toB)
+			overlap_dists(2) = std::abs((vA.normalized()).dot(p_to_B - p_from_A));
+			// 4: fromB - projected(toA)
+			overlap_dists(3) = std::abs((vB.normalized()).dot(p_to_A - p_from_B));
+			// 5: projected(fromB) - toA
+			overlap_dists(4) = std::abs((-vA.normalized()).dot(p_from_B - p_to_A));
+			// 6: projected(fromA) - toB
+			overlap_dists(5) = std::abs((-vB.normalized()).dot(p_from_A - p_to_B));
+
+			//std::cout << "\tdist2.mean() = " << dist2.mean() << std::endl;
+			//std::cout << "\tmax_sq_error - dist2.mean() = " << max_sq_error - dist2.mean() << std::endl;
+			if (print)
+				std::cout << "\toverlap_dists ratios = " << (overlap_dists/std::min(vA.norm(),vB.norm())).transpose() << std::endl;
+			overlap_dist = overlap_dists.minCoeff();
+
+			// if necessary overlapping condition does not hold, continue
+			if (overlap_dist/std::min(vA.norm(),vB.norm()) < overlap_dist_ratio_min)
+			{
+				if (print)
+					std::cout  << "\t\toverlapping distance ratio condition does not hold with 0 defined points overlapped\n";
+				continue;
+			}
+		}
 
         assert(N_overlap <= _points_B.cols());
         assert(N_overlap <= _points_A.cols() || (_closed_A && N_defined_B < _points_B.cols())); // N_overlap can be > _points_A if all defined points of B match with A points and the not defined extremes should match too
@@ -407,13 +422,14 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
             best_match->from_B_id_= from_B;
             best_match->to_B_id_= to_B;
             best_match->normalized_score_ = normalized_score;
-            best_match->overlap_dist_ = overlap_dist;
+            best_match->overlap_dist_ = 0;//overlap_dist;
 
             if (print)
                 best_match->print();
         }
     }
 
+    std::cout << "returning...\n";
     return best_match;
 }
 
@@ -421,6 +437,7 @@ Eigen::Array<Scalar,Eigen::Dynamic,1> computeSqDist(const Eigen::MatrixXs& _poin
                                                     const Eigen::MatrixXs& _points_B, const int& from_B, const int& to_B, bool from_B_defined, bool to_B_defined,
                                                     const int& last_A, const int& N_overlap )
 {
+	std::cout << "computeSqDist\n";
     // POINT-POINT DISTANCES for all overlap points (in case of closed A in two parts)
     // Fill with B points
     Eigen::ArrayXXd d(_points_B.block(0,from_B, 2, N_overlap).array());
@@ -468,7 +485,7 @@ Eigen::Array<Scalar,Eigen::Dynamic,1> computeSqDist(const Eigen::MatrixXs& _poin
                                                   to_A_defined,
                                                   to_B_defined);
     }
-    //std::cout << "\t\tsquared distances = " << dist2.transpose() << std::endl;
+    std::cout << "\t\tsquared distances = " << dist2.transpose() << std::endl;
 
     return dist2;
 }