WIP

include/base/processor/polyline_2D_utils.h

@@ -18,6 +18,8 @@
 namespace wolf {
 
 WOLF_STRUCT_PTR_TYPEDEFS(MatchPolyline2D);
+typedef std::map<Scalar,MatchPolyline2DPtr>    MatchPolyline2DMap;
+typedef std::list<MatchPolyline2DPtr>          MatchPolyline2DList;
 
 /** \brief Match between a two polylines (not specialized to landmark nor feature)
  *
@@ -31,9 +33,12 @@ struct MatchPolyline2D
     int from_B_id_;
     int to_B_id_;
     int normalized_score_;
+    Eigen::Matrix3s T_A_B_;
 };
 
-Eigen::Matrix3s transMatrix2D(const Eigen::Vector2s& t, const Scalar& theta);
+Eigen::Matrix3s pose2T2D(const Eigen::Vector2s& t, const Scalar& theta);
+Eigen::Matrix3s pose2T2D(const Eigen::Vector3s& pose);
+Eigen::Vector3s T2pose2D(const Eigen::Matrix3s& T);
 
 Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_incoming, const Eigen::MatrixXs& _points_last);
 
@@ -48,6 +53,10 @@ 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 );
 
+MatchPolyline2DList computeBestRigidTrans(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& max_sq_error );
+
 bool isProjectedOverSegment(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux, const Eigen::Vector3s& _B);
 
 Eigen::VectorXs computeSquaredMahalanobisDistances(const Eigen::Vector2s& _feature,

include/base/processor/processor_tracker_feature_polyline.h

@@ -28,23 +28,25 @@ namespace wolf
 {
 
 WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeaturePolyline);
-WOLF_STRUCT_PTR_TYPEDEFS(FeatureMatchPolyline2D);
 WOLF_PTR_TYPEDEFS(ProcessorTrackerFeaturePolyline);
+typedef std::list<FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DList;
+typedef std::list<LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DList;
 
 struct ProcessorParamsTrackerFeaturePolyline : public ProcessorParamsTrackerFeature
 {
         laserscanutils::LineFinderIterativeParams line_finder_params;
-        Scalar match_position_error_th;
+        Scalar match_alignment_position_sq_norm_th;
+        Scalar match_pose_sq_norm_th;
         Scalar class_position_error_th;
         unsigned int new_features_th;
         Scalar loop_time_th;
         std::vector<PolylineRectangularClass> polyline_classes;
 
         // These values below are constant and defined within the class -- provide a setter or accept them at construction time if you need to configure them
-        Scalar aperture_error_th_;// = 20.0 * M_PI / 180.; //20 degrees
-        Scalar angular_error_th_;// = 10.0 * M_PI / 180.; //10 degrees;
-        Scalar position_error_th_;// = 1;
-        Scalar min_features_ratio_th_;// = 0.5;
+        //Scalar aperture_error_th_;// = 20.0 * M_PI / 180.; //20 degrees
+        //Scalar angular_error_th_;// = 10.0 * M_PI / 180.; //10 degrees;
+        //Scalar position_error_th_;// = 1;
+        //Scalar min_features_ratio_th_;// = 0.5;
 };
 
 class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
@@ -166,16 +168,16 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
          */
         virtual void postProcess() override;
 
-        FeatureMatchPolyline2DPtr tryMatchWithFeature(const FeaturePolyline2DPtr& _ftr_last, const FeaturePolyline2DPtr& _ftr_incoming) const;
+        FeatureMatchPolyline2DList tryMatchWithFeature(const FeaturePolyline2DPtr& _ftr_last, const FeaturePolyline2DPtr& _ftr_incoming) const;
 
-        FeatureMatchPolyline2DPtr bestFeatureMatch(const FeatureMatchPolyline2DPtr& _match_A,
-                                                   const FeatureMatchPolyline2DPtr& _match_B) const;
+//        FeatureMatchPolyline2DPtr bestFeatureMatch(const FeatureMatchPolyline2DPtr& _match_A,
+//                                                   const FeatureMatchPolyline2DPtr& _match_B) const;
 
-        LandmarkMatchPolyline2DPtr tryMatchWithLandmark(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const;
+        LandmarkMatchPolyline2DList tryMatchWithLandmark(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const;
 
-        LandmarkMatchPolyline2DPtr tryMatchWithLandmark(const Eigen::MatrixXs& _lmk_expected_feature_points, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const;
+        bool updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr, const Eigen::Matrix3s& T_landmark_feature_prior);
 
-        void computeIncomingTransformations(const TimeStamp& _ts);
+        void computeTransformations();
 
         void expectedFeatureLast(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_, Eigen::MatrixXs& expected_feature_cov_) const;
 

src/processor/polyline_2D_utils.cpp

@@ -2,7 +2,7 @@
 
 namespace wolf {
 
-Eigen::Matrix3s transMatrix2D(const Eigen::Vector2s& t, const Scalar& theta)
+Eigen::Matrix3s pose2T2D(const Eigen::Vector2s& t, const Scalar& theta)
 {
     Eigen::Matrix3s T(Eigen::Matrix3s::Identity());
 
@@ -12,33 +12,51 @@ Eigen::Matrix3s transMatrix2D(const Eigen::Vector2s& t, const Scalar& theta)
     return T;
 }
 
-Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_incoming, const Eigen::MatrixXs& _points_last)
+Eigen::Matrix3s pose2T2D(const Eigen::Vector3s& pose)
 {
-    assert(_points_incoming.cols() == _points_last.cols());
-    assert(_points_incoming.rows() >= 2);
-    assert(_points_last.rows() >= 2);
+    Eigen::Matrix3s T(Eigen::Matrix3s::Identity());
+
+    T.topLeftCorner(2,2) = Eigen::Rotation2D<Scalar>(pose(2)).matrix();
+    T.topRightCorner(2,1) = pose.head(2);
+
+    return T;
+}
+
+Eigen::Vector3s T2pose2D(const Eigen::Matrix3s& T)
+{
+    Eigen::Vector3s pose;
+    pose.head(2) = T.topRightCorner(2,1);
+    pose(2) = atan2(T(0,0),T(1,0));
+
+    return pose;
+}
 
-    Eigen::Vector3s t_incoming_last;
+Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_A, const Eigen::MatrixXs& _points_B)
+{
+    assert(_points_A.cols() == _points_B.cols());
+    assert(_points_A.rows() >= 2);
+    assert(_points_B.rows() >= 2);
+
+    Eigen::Vector3s t_A_B;
 
     // Rotation
-    Eigen::VectorXs angles_incoming_last(_points_incoming.cols());
-    Eigen::MatrixXs points_incoming_mean = _points_incoming.colwise()-_points_incoming.rowwise().mean();
-    Eigen::MatrixXs points_last_mean = _points_last.colwise()-_points_last.rowwise().mean();
+    Eigen::VectorXs angles_A_B(_points_A.cols());
+    Eigen::MatrixXs points_A_mean = _points_A.colwise()-_points_A.rowwise().mean();
+    Eigen::MatrixXs points_B_mean = _points_B.colwise()-_points_B.rowwise().mean();
 
-    for (auto i = 0; i<_points_incoming.cols(); i++)
-        angles_incoming_last(i) = pi2pi(atan2(points_incoming_mean(1,i),points_incoming_mean(0,i)) - atan2(points_last_mean(1,i),points_last_mean(0,i)));
+    for (auto i = 0; i<_points_A.cols(); i++)
+        angles_A_B(i) = pi2pi(atan2(points_A_mean(1,i),points_A_mean(0,i)) - atan2(points_B_mean(1,i),points_B_mean(0,i)));
 
     // fix 2nd&3rd quadrant angles bad resulting mean
-    if (angles_incoming_last.maxCoeff() > M_PI / 2 && angles_incoming_last.minCoeff() < -M_PI / 2)
-        angles_incoming_last = (angles_incoming_last.array() > 0).select(angles_incoming_last, angles_incoming_last.array()+2*M_PI);
+    if (angles_A_B.maxCoeff() > M_PI / 2 && angles_A_B.minCoeff() < -M_PI / 2)
+        angles_A_B = (angles_A_B.array() > 0).select(angles_A_B, angles_A_B.array()+2*M_PI);
 
-    t_incoming_last(2) = angles_incoming_last.mean();
+    t_A_B(2) = angles_A_B.mean();
 
     // Translation
-    t_incoming_last.head<2>() = _points_incoming.topRows<2>().rowwise().mean() - Eigen::Rotation2D<Scalar>(t_incoming_last(2))*_points_last.topRows<2>().rowwise().mean();
-
+    t_A_B.head<2>() = _points_A.topRows<2>().rowwise().mean() - Eigen::Rotation2D<Scalar>(t_A_B(2))*_points_B.topRows<2>().rowwise().mean();
 
-    return t_incoming_last;
+    return t_A_B;
 }
 
 /** \brief Computes the squared distance from a point to a segment defined by two points
@@ -338,6 +356,218 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
     return best_match;
 }
 
+
+MatchPolyline2DList computeBestRigidTrans(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& max_sq_error )
+{
+    //std::cout << "computeBestRigidTrans...\n";
+    //std::cout << "\tA is " << (_closed_A ? "CLOSED" : "OPEN") << " has "<< _points_A.cols() << " points\n";
+    //std::cout << "\tB is " << (_closed_B ? "CLOSED" : "OPEN") << " has "<< _points_B.cols() << " points\n";
+    //std::cout << "\tpoints_A:\n" << _points_A.topRows(2) << std::endl;
+    //std::cout << "\tpoints_B:\n" << _points_B.topRows(2) << std::endl;
+
+    // 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);
+
+    /* ASSUMPTIONS:
+     *
+     *      IF BOTH NOT CLOSED:
+     *          (a) - A has equal or more points than B         --> Otherwise: Call the function switching A<->B
+     *
+     *      IF ONE CLOSED:
+     *          (b) - Should be A closed (not B)                --> Otherwise: Call the function switching A<->B
+     *          (c) - B.defined_points <= A.(defined_)points    --> Otherwise: No matching
+     *
+     *      IF BOTH CLOSED:
+     *          (d) - B.(defined_)points == A.(defined_)points  --> Otherwise: No matching
+     */
+
+    MatchPolyline2DList matches;
+
+    // ----------------- Call switching A<->B
+    if ( (!_closed_A && !_closed_B && _points_A.cols() < _points_B.cols()) || // (a)
+         (!_closed_A &&  _closed_B) )                                         // (b)
+    {
+        //std::cout << "Auto-calling switching A<->B...\n";
+
+        matches = computeBestRigidTrans(_points_B, _first_defined_B, _last_defined_B, _closed_B,
+                                             _points_A, _first_defined_A, _last_defined_A, _closed_A, max_sq_error);
+        // undo switching
+        for (auto match : matches)
+        {
+            std::swap(match->from_A_id_, match->from_B_id_);
+            std::swap(match->to_A_id_,   match->to_B_id_);
+            match->T_A_B_ = match->T_A_B_.inverse();
+        }
+        return matches;
+    }
+
+    // ------------------ No matching (c) & (d) return empty list
+    int N_defined_B = _points_B.cols() - (_first_defined_B ? 0 : 1) - (_last_defined_B ? 0 : 1);
+
+    if ((_closed_A && !_closed_B && _points_A.cols() < N_defined_B) ||     // (c)
+        (_closed_A &&  _closed_B && _points_A.cols() != _points_B.cols())) // (d)
+        return matches;
+
+    /* ------------------ Normal function call:
+     * Search along all possible overlapping configurations between A&B points
+     *      We define offset as the correspondence (if exists) of the first point_A to B's.
+     *      Offset is within [min_offset, max_offset], we distinguish between 3 cases:
+     *          1. None closed:  [-last_A, last_B]
+     *          2. A closed:     [-last_A, 0]
+     */
+
+    int last_A, last_B, offset, max_offset, min_offset, from_A, to_A, from_B, to_B, N_overlapped;
+    last_A = _points_A.cols() - 1;
+    last_B = _points_B.cols() - 1;
+
+    if (!_closed_A)
+    {
+        min_offset = -last_A;
+        max_offset = last_B;
+    }
+    else
+    {
+        min_offset = -last_A;
+        max_offset = 0;
+    }
+
+    // Check all overlapping configurations
+    for (offset = min_offset; offset <= max_offset; offset++)
+    {
+        from_A = std::max(0,-offset);
+        from_B = std::max(0, offset);
+
+        if (!_closed_A && !_closed_B)
+            N_overlapped = std::min(last_B - from_B, last_A - from_A)+1;
+        else if(_closed_A)
+            N_overlapped = _points_B.cols();
+        else
+            std::runtime_error("Impossible state. Function should auto-called reversing A&B");
+
+        to_A = from_A+N_overlapped-1;
+        to_B = from_B+N_overlapped-1;
+
+        while (to_A > last_A && _closed_A)
+            to_A -= _points_A.cols();
+
+        //std::cout << "\toffset " << offset << std::endl;
+        //std::cout << "\t\tfrom_A " << from_A << std::endl;
+        //std::cout << "\t\tto_A   " << to_A << std::endl;
+        //std::cout << "\t\tfrom_B " << from_B << std::endl;
+        //std::cout << "\t\tto_B   " << to_B << std::endl;
+        //std::cout << "\t\tlast_A  " << last_A << std::endl;
+        //std::cout << "\t\tlast_B  " << last_B << std::endl;
+        //std::cout << "\t\tN_overlapped " << N_overlapped << std::endl;
+
+        bool from_A_defined = ((from_A != 0 && from_A != last_A) || _first_defined_A);
+        bool from_B_defined = ((from_B != 0 && from_B != last_B) || _first_defined_B);
+        bool to_A_defined   = ((to_A   != 0 && to_A   != last_A) || _last_defined_A);
+        bool to_B_defined   = ((to_B   != 0 && to_B   != last_B) || _last_defined_B);
+
+        //std::cout << "\t\tfrom_A_defined " << from_A_defined << (from_A == 0) << (from_A == last_A) << _first_defined_A << std::endl;
+        //std::cout << "\t\tfrom_B_defined " << from_B_defined << (from_B == 0) << (from_B == last_B) << _first_defined_B << std::endl;
+        //std::cout << "\t\tto_A_defined   " << to_A_defined   << (to_A == 0)   << (to_A == last_A)   << _last_defined_A << std::endl;
+        //std::cout << "\t\tto_B_defined   " << to_B_defined   << (to_B == 0)   << (to_B == last_B)   << _last_defined_B << std::endl;
+
+        // If only one overlapped point, both should be defined
+        if ( N_overlapped== 1 &&
+             (!from_A_defined || !from_B_defined || !to_A_defined || !to_B_defined ) )
+            continue;
+
+        assert(N_overlapped <= _points_B.cols());
+        assert(N_overlapped <= _points_A.cols() || (_closed_A && N_defined_B < _points_B.cols())); // N_overlapped can be > _points_A if all defined points of B match with A points and the not defined extremes should match too
+        assert(from_A >= 0);
+        assert(from_B >= 0);
+        assert(from_B == 0 || !_closed_A);
+        assert(to_B == _points_B.cols()-1 || !_closed_A);
+        assert(to_A <= last_A);
+        assert(to_B <= last_B);
+        assert(to_A < _points_A.cols());
+        assert(to_B < _points_B.cols());
+
+        // RIGID TRANSFORMATION FOR DEFINED POINTS
+        int from_B_def = from_B_defined ? from_B : from_B+1;
+        int from_A_def = from_A_defined ? from_A : from_A+1;
+        int to_A_def   = to_A_defined ? to_A : to_A+1;
+        int N_overlapped_def = N_overlapped - (from_B_defined && from_A_defined ? 0 : 1) - (to_B_defined && to_A_defined  ? 0 : 1);
+        Eigen::MatrixXs points_B_def = _points_B.middleCols(from_B_def, N_overlapped_def);
+        Eigen::MatrixXs points_A_def(_points_A.rows(), N_overlapped_def);
+        if (to_A >= from_A)
+            points_A_def = _points_A.middleCols(from_A_def, N_overlapped_def);
+        else
+        {
+            points_A_def.leftCols(_points_A.cols()-from_A_def) = _points_A.middleCols(from_A_def, _points_A.cols()-from_A_def);
+            points_A_def.rightCols(to_A_def+1)                 = _points_A.middleCols(0, to_A_def+1);
+        }
+
+        Eigen::Vector3s pose_A_B = computeRigidTrans(points_A_def, points_B_def);
+        Eigen::Matrix3s T_A_B = pose2T2D(pose_A_B.head(2), pose_A_B(2));
+
+        // POINT-POINT DISTANCES of the matching of defined points
+        Eigen::MatrixXs d = (points_A_def - T_A_B * points_B_def).topRows(2);
+        Eigen::ArrayXd dist2(N_overlapped);
+        dist2.middleRows(from_B_defined && from_A_defined ? 0 : 1, N_overlapped_def) = d.colwise().squaredNorm().transpose().array();
+
+        // POINT-LINE DISTANCES for not defined extremes
+        // First
+        if (!from_B_defined || !from_A_defined)
+        {
+            // take care of closed A: next of from_A
+            int next_from_A = (from_A+1 > last_A ? 0 : from_A+1);
+            //std::cout << "\t\t\tFirst feature not defined distance to line" << std::endl;
+
+            // std::cout << "\t\t\tA   " << _points_A.col(from_A).transpose() << std::endl;
+            // std::cout << "\t\t\tAaux" << _points_A.col(next_from_A).transpose() << std::endl;
+            // std::cout << "\t\t\tB   " << _points_B.col(from_B).transpose() << std::endl;
+            dist2(0) = sqDistPointToLine(_points_A.col(from_A),
+                                         _points_A.col(next_from_A),
+                                         T_A_B * _points_B.col(from_B),
+                                         from_A_defined,
+                                         from_B_defined);
+            assert(N_overlapped > 1);
+        }
+        // Last
+        if (!to_B_defined || !to_A_defined)
+        {
+            // take care of closed A: next of to_A
+            int prev_to_A = (to_A == 0 ? last_A : to_A-1);
+            //std::cout << "\t\t\tLast feature not defined distance to line" << std::endl;
+
+            // std::cout << "\t\t\tA   " << _points_A.col(to_A).transpose() << std::endl;
+            // std::cout << "\t\t\tAaux" << _points_A.col(prev_to_A).transpose() << std::endl;
+            // std::cout << "\t\t\tB   " << _points_B.col(to_B).transpose() << std::endl;
+            dist2(N_overlapped-1) = sqDistPointToLine(_points_A.col(to_A),
+                                                      _points_A.col(prev_to_A),
+                                                      T_A_B * _points_B.col(to_B),
+                                                      to_A_defined,
+                                                      to_B_defined);
+            assert(N_overlapped > 1);
+        }
+        //std::cout << "\t\tsquared distances = " << dist2.transpose() << std::endl;
+
+        // All squared distances should be within a threshold
+        if ((dist2 < max_sq_error).all())
+        {
+            auto match = std::make_shared<MatchPolyline2D>();
+
+            match->from_A_id_= from_A;
+            match->to_A_id_= to_A;
+            match->from_B_id_= from_B;
+            match->to_B_id_= to_B;
+            match->normalized_score_ = (max_sq_error - dist2.mean()) / max_sq_error;
+            match->T_A_B_ = T_A_B;
+
+            // Store matches ordered by the squared norm of the transformation (I know, mixing mts and rads..)
+            matches.push_back(match);
+        }
+    }
+
+    return matches;
+}
+
 bool isProjectedOverSegment(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux, const Eigen::Vector3s& _B)
 {
     /* The orthogonal projection of B over the line A-Aaux is in the segment [A,Aaux].

test/gtest_polyline_2D.cpp

@@ -10,7 +10,7 @@ TEST(Polyline2DTest, RigidTransformation)
     // Random points around random position
     MatrixXs points_last = MatrixXs::Random(3,8);
     points_last.bottomRows(1) = MatrixXs::Ones(1,8);
-    MatrixXs T_random = transMatrix2D(Vector2s::Random(), (rand() % 2000*M_PI)*0.001);
+    MatrixXs T_random = pose2T2D(Vector2s::Random(), (rand() % 2000*M_PI)*0.001);
     points_last = T_random * points_last;
 
     // For different orientation changes between last and incomming
@@ -23,7 +23,7 @@ TEST(Polyline2DTest, RigidTransformation)
         // inverse transformation
         Scalar theta_incoming_last = -theta_last_incoming;
         Vector2s t_incoming_last = -R_last_incoming.transpose() * t_last_incoming;
-        Matrix3s T_incoming_last = transMatrix2D(t_incoming_last, theta_incoming_last);
+        Matrix3s T_incoming_last = pose2T2D(t_incoming_last, theta_incoming_last);
 
         // rotate points
         MatrixXs points_incoming = T_incoming_last * points_last;