diff --git a/CMakeLists.txt b/CMakeLists.txt
index d76d259b303c034477695255eaea250a96b4b845..b6539a1bab14ae553a09626abd5611f802d157d3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -372,6 +372,7 @@ SET(HDRS_FEATURE
   include/base/feature/feature_polyline_2D.h
   include/base/feature/feature_base.h
   include/base/feature/feature_match.h
+  include/base/feature/feature_match_polyline_2D.h
   include/base/feature/feature_pose.h
   include/base/feature/feature_gnss_fix.h
   include/base/feature/feature_gnss_single_diff.h
@@ -626,6 +627,9 @@ IF (laser_scan_utils_FOUND)
   SET(HDRS_SENSOR ${HDRS_SENSOR}
     include/base/sensor/sensor_laser_2D.h
     )
+  SET(SRCS_CAPTURE ${SRCS_CAPTURE}
+    src/capture/capture_laser_2D.cpp
+    )
   SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
     src/processor/processor_polyline.cpp
     src/processor/processor_tracker_feature_corner.cpp
diff --git a/include/base/landmark/landmark_polyline_2D.h b/include/base/landmark/landmark_polyline_2D.h
index db93b3319a929aff2b6addfcbade9640690521f7..68f5d421fd99a19f695427d7bd68ca513000e2bf 100644
--- a/include/base/landmark/landmark_polyline_2D.h
+++ b/include/base/landmark/landmark_polyline_2D.h
@@ -253,7 +253,7 @@ inline bool LandmarkPolyline2D::isClosed() const
 inline LandmarkPolyline2DPtr LandmarkPolyline2D::getMergedInLandmark() const
 {
     // recursive call
-    if (merged_in_lmk_->getMergedInLandmark() != nullptr)
+    if (merged_in_lmk_ != nullptr && merged_in_lmk_->getMergedInLandmark() != nullptr)
         return merged_in_lmk_->getMergedInLandmark();
     return merged_in_lmk_;
 }
diff --git a/include/base/processor/processor_tracker_feature_polyline.h b/include/base/processor/processor_tracker_feature_polyline.h
index f5059b7e0f6c5b86a8746e6492733b4822a785bc..9d4e04c541c5f7533465b25df2151abaa9772966 100644
--- a/include/base/processor/processor_tracker_feature_polyline.h
+++ b/include/base/processor/processor_tracker_feature_polyline.h
@@ -171,27 +171,25 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
          */
         virtual void postProcess() override;
 
-        FeatureMatchPolyline2DList tryMatchWithFeature(const FeaturePolyline2DPtr& _ftr_last, const FeaturePolyline2DPtr& _ftr_incoming) const;
         void tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches, const FeaturePolyline2DPtr& _ftr_last, const FeaturePolyline2DPtr& _ftr_incoming, const Eigen::Matrix3s& _T_last_incoming_prior) const;
 
-//        FeatureMatchPolyline2DPtr bestFeatureMatch(const FeatureMatchPolyline2DPtr& _match_A,
-//                                                   const FeatureMatchPolyline2DPtr& _match_B) const;
-
-        LandmarkMatchPolyline2DList tryMatchWithLandmark(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const;
         void tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr, const Eigen::Matrix3s& _T_feature_landmark_prior) const;
 
         bool updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr);
 
         void computeTransformations();
 
-        //void expectedFeatureLast(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_, Eigen::MatrixXs& expected_feature_cov_) const;
-
     public:
 
         /// @brief Factory method
         static ProcessorBasePtr create(const std::string& _unique_name,
                                        const ProcessorParamsBasePtr _params,
                                        const SensorBasePtr _sensor_ptr);
+
+        const FeatureBaseList& getLastNewFeatures() const
+        {
+            return all_features_last_;
+        }
 };
 
 } /* namespace wolf */
diff --git a/src/processor/polyline_2D_utils.cpp b/src/processor/polyline_2D_utils.cpp
index 0b8aad1fc492fac0461cbe94d42953298ecfb7ae..e6f7c9a7a5572b0c1e2deb4bab5c8addcfdf3ff6 100644
--- a/src/processor/polyline_2D_utils.cpp
+++ b/src/processor/polyline_2D_utils.cpp
@@ -26,7 +26,7 @@ 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));
+    pose(2) = atan2(T(1,0),T(0,0));
 
     return pose;
 }
@@ -36,6 +36,7 @@ Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_A, const Eigen:
     assert(_points_A.cols() == _points_B.cols());
     assert(_points_A.rows() >= 2);
     assert(_points_B.rows() >= 2);
+    assert(_points_A.cols() >= 2);
 
     Eigen::Vector3s t_A_B;
 
@@ -362,8 +363,8 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
                                           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 << "\tA is " << (_closed_A ? "CLOSED" : "NOT CLOSED") << " has "<< _points_A.cols() << " points\n";
+    //std::cout << "\tB is " << (_closed_B ? "CLOSED" : "NOT CLOSED") << " 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;
 
@@ -393,7 +394,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
         //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);
+                                        _points_A, _first_defined_A, _last_defined_A, _closed_A, max_sq_error);
         // undo switching
         for (auto match : matches)
         {
@@ -472,8 +473,11 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
         //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 &&
+        // If only one overlapped point, no rigid transformation possible
+        if ( N_overlapped == 1 )
+            continue;
+        // If only two overlapped points, all should be defined
+        if ( N_overlapped == 2 &&
              (!from_A_defined || !from_B_defined || !to_A_defined || !to_B_defined ) )
             continue;
 
@@ -489,6 +493,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
         assert(to_B < _points_B.cols());
 
         // RIGID TRANSFORMATION FOR DEFINED POINTS
+        //std::cout << "\tcomputing rigid transformation...\n";
         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;
@@ -502,7 +507,9 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
             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);
         }
+        //std::cout << "\tpoints filled...\n";
 
+        //std::cout << "\tcomputeRigidTrans\n";
         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));
 
@@ -512,6 +519,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
         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
+        //std::cout << "\tPOINT-LINE DISTANCES\n";
         // First
         if (!from_B_defined || !from_A_defined)
         {
@@ -546,7 +554,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
                                                       to_B_defined);
             assert(N_overlapped > 1);
         }
-        //std::cout << "\t\tsquared distances = " << dist2.transpose() << std::endl;
+        //std::cout << "\t\tsquared distances = " << dist2.transpose() << "( should be < " << max_sq_error << ")" << std::endl;
 
         // All squared distances should be within a threshold
         if ((dist2 < max_sq_error).all())
@@ -562,9 +570,11 @@ 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 << matches.size() << " rigid transformations found\n";
     return matches;
 }
 
diff --git a/src/processor/processor_tracker_feature_polyline.cpp b/src/processor/processor_tracker_feature_polyline.cpp
index a2198720c41eb5cfabcf12465a6e43568918b330..29c688b9333f8484a1c1e61708a87bb3518d9252 100644
--- a/src/processor/processor_tracker_feature_polyline.cpp
+++ b/src/processor/processor_tracker_feature_polyline.cpp
@@ -27,6 +27,11 @@ unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseLis
                                                             FeatureBaseList& _features_incoming_out,
                                                             FeatureMatchMap& _feature_correspondences)
 {
+    WOLF_DEBUG("PTF ", getName(), "::trackFeatures", _features_last_in.size());
+
+    if (_features_last_in.empty())
+        return 0;
+
     // prior transformations
     Eigen::Matrix3s T_last_incoming_prior(Eigen::Matrix3s::Identity());
     T_last_incoming_prior.topLeftCorner(2,2)  = R_last_incoming_;
@@ -42,58 +47,74 @@ unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseLis
         auto pl_incoming = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
         FeatureMatchPolyline2DScalarMap best_matches;
 
+        std::cout << "\tmatching feature incoming " << pl_incoming->id() << "\n";
+
         // Check matching with all features in last
         // Store all matches consistent with T_last_incoming_prior in best_matches sorted by difference from T_last_incoming_prior
         for (auto ftr_last_ : _features_last_in)
             tryMatchWithFeature(best_matches, std::static_pointer_cast<FeaturePolyline2D>(ftr_last_), pl_incoming, T_last_incoming_prior);
 
+        std::cout << "\t" << best_matches.size() << " matches with features last found\n";
+
         // Try the match for all candidates (sorted by difference from movement prior)
         for (auto best_ftr_match_pair : best_matches)
         {
             auto best_ftr_match = best_ftr_match_pair.second;
             auto pl_last = std::static_pointer_cast<FeaturePolyline2D>(best_ftr_match->feature_ptr_);
-            auto pl_lmk  = std::static_pointer_cast<LandmarkPolyline2D>(landmark_match_map_[pl_last]->landmark_ptr_);
-
-            // the match with this last_ feature was removed after any updateMatchWithLandmark
-            if(landmark_match_map_.find(pl_last) == landmark_match_map_.end())
-                continue;
 
-            auto lmk_match_last = landmark_match_map_[pl_last];
+            std::cout << "\t\tconfirming match with feature last: " << pl_last->id() << "\n";
 
-            // Landmark changed or was merged -> Redo match with last feature
-            if (lmk_match_last->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr)
+            // There is a landmark_match (i.e. it has not been recently created in processNew)
+            if(find(new_features_last_.begin(), new_features_last_.end(), pl_last) == new_features_last_.end())
             {
-                if (!updateMatchWithLandmark(lmk_match_last,pl_lmk,pl_last))
+                // the match with this last_ feature was removed after any updateMatchWithLandmark
+                if(landmark_match_map_.find(pl_last) == landmark_match_map_.end())
+                {
+                    std::cout << "\t\tremoved feature last\n";
+                    continue;
+                }
+
+                auto lmk_match_last = landmark_match_map_[pl_last];
+                auto pl_lmk  = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match_last->landmark_ptr_);
+
+                // Landmark changed or was merged -> Redo match with last feature
+                if (lmk_match_last->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr)
+                {
+                    std::cout << "\t\tupdating match\n";
+                    if (!updateMatchWithLandmark(lmk_match_last,pl_lmk,pl_last))
+                        continue;
+                }
+                // removed -> not valid match with landmark
+                else if (pl_lmk->isRemoving())
                     continue;
+
+                // Add the match to landmark_match_map
+                auto lmk_match_incoming = std::make_shared<LandmarkMatchPolyline2D>();
+
+                lmk_match_incoming->feature_from_id_  = best_ftr_match->feature_incoming_from_id_;
+                lmk_match_incoming->feature_to_id_    = best_ftr_match->feature_incoming_to_id_;
+                // Not adding new points nor checks (done in correctFeatureDrift)
+                lmk_match_incoming->landmark_from_id_ = lmk_match_last->landmark_from_id_ + best_ftr_match->feature_last_from_id_ - lmk_match_last->feature_from_id_;
+                lmk_match_incoming->landmark_to_id_   = lmk_match_last->landmark_to_id_   + best_ftr_match->feature_last_to_id_   - lmk_match_last->feature_to_id_;
+                lmk_match_incoming->landmark_ptr_     = pl_lmk;
+                lmk_match_incoming->landmark_version_ = pl_lmk->getVersion();
+                lmk_match_incoming->normalized_score_ = best_ftr_match->normalized_score_;
+                lmk_match_incoming->T_feature_landmark_ = best_ftr_match->T_incoming_last_ * lmk_match_last->T_feature_landmark_;
+
+                landmark_match_map_[pl_incoming] = lmk_match_incoming;
             }
-            // removed -> not valid match with landmark
-            else if (pl_lmk->isRemoving())
-                continue;
 
-            // TRACK still valid
+            // TRACK valid
+            std::cout << "\t\tvalid match, storing...\n";
             // add best match to match_map _feature_correspondences[feature_out_ptr] = feature_in_ptr
             _feature_correspondences[pl_incoming] = best_ftr_match;
 
             // add feature to list of tracked features
             _features_incoming_out.push_back(pl_incoming);
 
-            // Add the match to landmark_match_map
-            auto lmk_match_incoming = std::make_shared<LandmarkMatchPolyline2D>();
-
-            lmk_match_incoming->feature_from_id_  = best_ftr_match->feature_incoming_from_id_;
-            lmk_match_incoming->feature_to_id_    = best_ftr_match->feature_incoming_to_id_;
-            // Not adding new points nor checks (done in correctFeatureDrift)
-            lmk_match_incoming->landmark_from_id_ = lmk_match_last->landmark_from_id_ + best_ftr_match->feature_last_from_id_ - lmk_match_last->feature_from_id_;
-            lmk_match_incoming->landmark_to_id_   = lmk_match_last->landmark_to_id_   + best_ftr_match->feature_last_to_id_   - lmk_match_last->feature_to_id_;
-            lmk_match_incoming->landmark_ptr_     = pl_lmk;
-            lmk_match_incoming->landmark_version_ = pl_lmk->getVersion();
-            lmk_match_incoming->normalized_score_ = best_ftr_match->normalized_score_;
-            lmk_match_incoming->T_feature_landmark_ = best_ftr_match->T_incoming_last_ * lmk_match_last->T_feature_landmark_;
-
-            landmark_match_map_[pl_incoming] = lmk_match_incoming;
-
             // match for this feature has been found
             matched = true;
+            std::cout << "\t\tmatch found\n";
             break;
         }
 
@@ -103,6 +124,8 @@ unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseLis
         else
             ftr_it++;
     }
+
+    WOLF_DEBUG(_feature_correspondences.size(), " features tracked");
     return _feature_correspondences.size();
 }
 
@@ -110,6 +133,7 @@ bool ProcessorTrackerFeaturePolyline::correctFeatureDrift(const FeatureBasePtr _
                                                           const FeatureBasePtr _last_feature,
                                                           FeatureBasePtr _incoming_feature)
 {
+    WOLF_DEBUG("PTF ", getName(), "::correctFeatureDrift: ");
     // If incoming observed new landmark points & landmark has points
     // Check if match is still valid and update the match
     // TODO
@@ -123,6 +147,7 @@ bool ProcessorTrackerFeaturePolyline::correctFeatureDrift(const FeatureBasePtr _
 
 bool ProcessorTrackerFeaturePolyline::voteForKeyFrame()
 {
+    WOLF_DEBUG("PTF ", getName(), "::voteForKeyFrame: ");
     // TODO
     /* IDEAS:
      *   - Any feature incoming derived too much from projected Landmark
@@ -135,8 +160,11 @@ bool ProcessorTrackerFeaturePolyline::voteForKeyFrame()
 
 unsigned int ProcessorTrackerFeaturePolyline::processNew(const unsigned int& _max_features)
 {
+    WOLF_DEBUG("PTF ", getName(), "::processNew: ");
     unsigned int n = ProcessorTrackerFeature::processNew(_max_features);
 
+    WOLF_DEBUG("Processing ", n, " new last features");
+
     // prior transformations
     Eigen::Matrix3s T_sensor_world_last(Eigen::Matrix3s::Identity());
     T_sensor_world_last.topLeftCorner(2,2)  = R_sensor_world_last_;
@@ -149,6 +177,7 @@ unsigned int ProcessorTrackerFeaturePolyline::processNew(const unsigned int& _ma
     {
         auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
         LandmarkMatchPolyline2DScalarMap best_lmk_matches;
+        WOLF_DEBUG("Processing feature: ", pl_ftr->id());
 
         // Check matching with all existing polyline landmarks
         // Store all matches consistent with T_last_incoming_prior in best_matches sorted by difference from T_last_incoming_prior
@@ -202,19 +231,26 @@ unsigned int ProcessorTrackerFeaturePolyline::processNew(const unsigned int& _ma
 unsigned int ProcessorTrackerFeaturePolyline::detectNewFeatures(const unsigned int& _max_new_features,
                                                                 FeatureBaseList& _features_last_out)
 {
+    WOLF_DEBUG("PTF ", getName(), "::detectNewFeatures (", all_features_last_.size(), " in all_features_last_)");
     int prev_size = _features_last_out.size();
-    int N_new_features = new_features_last_.size();
 
     if (_features_last_out == new_features_last_)
     {
+        WOLF_DEBUG("_features_last_out:", _features_last_out.size(), " before splice");
+        WOLF_DEBUG("all_features_last_:", all_features_last_.size(), " before splice");
         _features_last_out.splice(_features_last_out.end(), all_features_last_);
+        WOLF_DEBUG("_features_last_out:", _features_last_out.size(), " after splice");
+        WOLF_DEBUG("all_features_last_:", all_features_last_.size(), " after splice");
 
-        if (_features_last_out.size() > prev_size + _max_new_features)
+        // 0 means unlimited new features
+        if (_max_new_features != 0 && _features_last_out.size() > prev_size + _max_new_features)
         {
             _features_last_out.resize(prev_size + _max_new_features);
+            WOLF_DEBUG(_max_new_features, " were provided (max new features)");
             return _max_new_features;
         }
-        return N_new_features;
+        WOLF_DEBUG(_features_last_out.size()-prev_size, " were provided");
+        return _features_last_out.size()-prev_size;
     }
     else
         std::runtime_error("asking for new features not from last");
@@ -224,23 +260,29 @@ unsigned int ProcessorTrackerFeaturePolyline::detectNewFeatures(const unsigned i
 
 void ProcessorTrackerFeaturePolyline::establishConstraints()
 {
+    WOLF_DEBUG("PTF ", getName(), "::establishConstraints: ");
     unsigned int N_constraints = 0;
 
     // Create a constraint for each match in last features
     auto ftr_it = last_ptr_->getFeatureList().begin();
     while (ftr_it != last_ptr_->getFeatureList().end())
     {
+        WOLF_DEBUG("\tLast feature: ", (*ftr_it)->id());
         assert(landmark_match_map_.find(*ftr_it) != landmark_match_map_.end() && "feature without landmark match in last features");
-        auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(landmark_match_map_[*ftr_it]->landmark_ptr_);
-        auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
         auto lmk_match = landmark_match_map_[*ftr_it];
+        auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match->landmark_ptr_);
+        auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
+        WOLF_DEBUG("\tLandmark: ", pl_lmk->id());
 
         // LANDMARK CHANGED: update match
+        WOLF_DEBUG("\tLandmark changed?");
         if (lmk_match->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr)
         {
+            WOLF_DEBUG("\tLandmark changed");
             // not successful update
             if (!updateMatchWithLandmark(lmk_match,pl_lmk,pl_ftr))
             {
+                WOLF_DEBUG("\tNot successful update");
                 // remove from match map
                 landmark_match_map_.erase(*ftr_it);
                 // remove from last feature list
@@ -250,22 +292,29 @@ void ProcessorTrackerFeaturePolyline::establishConstraints()
         }
 
         // LANDMARK REMOVED: remove match and feature
-        else if (pl_lmk->isRemoving())
+        else
         {
-            // remove from match map
-            landmark_match_map_.erase(*ftr_it);
-            // remove from last feature list
-            ftr_it = last_ptr_->getFeatureList().erase(ftr_it);
-            continue;
+            WOLF_DEBUG("\tLandmark removed?");
+            if (pl_lmk->isRemoving())
+            {
+                WOLF_DEBUG("\tLandmark was removed");
+                // remove from match map
+                landmark_match_map_.erase(*ftr_it);
+                // remove from last feature list
+                ftr_it = last_ptr_->getFeatureList().erase(ftr_it);
+                continue;
+            }
         }
 
         // MODIFY LANDMARK (only for not closed and not recently created)
+        WOLF_DEBUG("\tModifying..");
         if (!pl_lmk->isClosed() && pl_lmk->getHits() > 0)
             // If modified, add lmk to modified_lmks_ list
             if (modifyLandmarkAndMatch(lmk_match, pl_ftr))
                 modified_lmks_.push_back(pl_lmk);
 
         // ESTABLISH CONSTRAINTS
+        WOLF_DEBUG("\tEstablishing constraints..");
         assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
         assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
         assert((pl_lmk->getNPoints() >= pl_ftr->getNPoints() + lmk_match->landmark_from_id_ || pl_lmk->isClosed()) && "Landmark didn't grow properly");
@@ -281,46 +330,46 @@ void ProcessorTrackerFeaturePolyline::establishConstraints()
         int lmk_point_id = lmk_match->landmark_from_id_;
         for (int ftr_point_id = 0; ftr_point_id < pl_ftr->getNPoints(); ftr_point_id++)
         {
-            //std::cout << "feature point " << ftr_point_id << std::endl;
-            //std::cout << "landmark point " << lmk_point_id << std::endl;
+            std::cout << "feature point " << ftr_point_id << std::endl;
+            std::cout << "landmark point " << lmk_point_id << std::endl;
 
             // First not defined point
             if (ftr_point_id == 0 && !pl_ftr->isFirstDefined())
                 // first point to line constraint
             {
-                //std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getNextValidId(lmk_point_id) << std::endl;
+                std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getNextValidId(lmk_point_id) << std::endl;
                 assert(!(lmk_point_id == pl_lmk->getLastId() && !pl_lmk->isClosed()) && "Trying to establish a constraint for first not defined point of a feature with the last landmark point without being closed. A landmark didn't grow correctly.");
 
                 // emplace constraint
                 emplaceConstraintPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getNextValidId(lmk_point_id));
                 N_constraints++;
-                //std::cout << "constraint added" << std::endl;
+                std::cout << "constraint added" << std::endl;
             }
 
             // Last not defined point
             else if (ftr_point_id == pl_ftr->getNPoints()-1 && !pl_ftr->isLastDefined())
                 // last point to line constraint
             {
-                //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_prev_point_id << std::endl;
+                std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getPrevValidId(lmk_point_id) << std::endl;
                 assert(!(lmk_point_id == pl_lmk->getFirstId() && !pl_lmk->isClosed()) && "Trying to establish a constraint for last not defined point of a feature with the first landmark point without being closed. A landmark didn't grow correctly.");
 
                 // emplace constraint
                 emplaceConstraintPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getPrevValidId(lmk_point_id));
                 N_constraints++;
-                //std::cout << "constraint added" << std::endl;
+                std::cout << "constraint added" << std::endl;
             }
 
             // Defined point
             else
                 // point to point constraint
             {
-                //std::cout << "point-point: landmark point " << lmk_point_id << std::endl;
-                //std::cout << "landmark first id:" << pl_lmk->getFirstId() << std::endl;
-                //std::cout << "landmark last id:" << pl_lmk->getLastId() << std::endl;
-                //std::cout << "landmark n points:" << pl_lmk->getNPoints() << std::endl;
+                std::cout << "point-point: landmark point " << lmk_point_id << std::endl;
+                std::cout << "landmark first id:" << pl_lmk->getFirstId() << std::endl;
+                std::cout << "landmark last id:" << pl_lmk->getLastId() << std::endl;
+                std::cout << "landmark n points:" << pl_lmk->getNPoints() << std::endl;
                 emplaceConstraintPoint(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id);
                 N_constraints++;
-                //std::cout << "constraint added" << std::endl;
+                std::cout << "constraint added" << std::endl;
             }
 
             // Next lmk point
@@ -330,7 +379,7 @@ void ProcessorTrackerFeaturePolyline::establishConstraints()
         // next ftr
         ftr_it++;
     }
-    //std::cout << N_constraints << " constraints established, trying to close and classify " << modified_lmks.size() << std::endl;
+    std::cout << N_constraints << " constraints established" << std::endl;
 }
 
 void ProcessorTrackerFeaturePolyline::emplaceConstraintPointToLine(FeaturePolyline2DPtr _polyline_feature,
@@ -364,11 +413,11 @@ void ProcessorTrackerFeaturePolyline::emplaceConstraintPoint(FeaturePolyline2DPt
 
 LandmarkBasePtr ProcessorTrackerFeaturePolyline::createLandmark(FeatureBasePtr _feature_ptr)
 {
-    //std::cout << "ProcessorTrackerFeaturePolyline::createLandmark" << std::endl;
+    WOLF_DEBUG("PTF ", getName(), "::createLandmark: ");
     //std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl;
     //std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl;
     assert(_feature_ptr->getType() == "POLYLINE 2D");
-    assert(std::find(new_features_last_.begin(),new_features_last_.end(),_feature_ptr) != new_features_last_.end() && "creating a landmark for a feature which is not in last capture");
+    assert(std::find(last_ptr_->getFeatureList().begin(),last_ptr_->getFeatureList().end(),_feature_ptr) != last_ptr_->getFeatureList().end() && "creating a landmark for a feature which is not in last capture");
 
 
     FeaturePolyline2DPtr polyline_ptr = std::static_pointer_cast<FeaturePolyline2D>(_feature_ptr);
@@ -391,6 +440,7 @@ LandmarkBasePtr ProcessorTrackerFeaturePolyline::createLandmark(FeatureBasePtr _
 
 bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match, FeaturePolyline2DPtr& pl_ftr)
 {
+    WOLF_DEBUG("PTF ", getName(), "::modifyLandmarkAndMatch: ");
     auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match->landmark_ptr_);
 
     bool print = false;
@@ -537,43 +587,51 @@ bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyli
 
 void ProcessorTrackerFeaturePolyline::advanceDerived()
 {
+    WOLF_DEBUG("PTF ", getName(), "::advanceDerived: ");
     // remove last features from landmark_match_map_
-    for (auto ftr : last_ptr_->getFeatureList())
-    {
-        assert(landmark_match_map_.find(ftr) != landmark_match_map_.end());
-        landmark_match_map_.erase(ftr);
-    }
+    if (last_ptr_ != nullptr)
+        for (auto ftr : last_ptr_->getFeatureList())
+        {
+            assert(landmark_match_map_.find(ftr) != landmark_match_map_.end());
+            landmark_match_map_.erase(ftr);
+        }
 
+    WOLF_DEBUG("removing ", all_features_last_.size() , " features in last");
     all_features_last_.clear();
     all_features_last_.splice(all_features_last_.end(),all_features_incoming_);
+    WOLF_DEBUG("last has ", all_features_last_.size() , " features (prev. incoming)");
 
     ProcessorTrackerFeature::advanceDerived();
 }
 
 void ProcessorTrackerFeaturePolyline::resetDerived()
 {
+    WOLF_DEBUG("PTF ", getName(), "::resetDerived: ");
     // remove last features from landmark_match_map_
-    for (auto ftr : last_ptr_->getFeatureList())
-    {
-        assert(landmark_match_map_.find(ftr) != landmark_match_map_.end());
-        landmark_match_map_.erase(ftr);
-    }
+    if (last_ptr_ != nullptr)
+        for (auto ftr : last_ptr_->getFeatureList())
+        {
+            assert(landmark_match_map_.find(ftr) != landmark_match_map_.end());
+            landmark_match_map_.erase(ftr);
+        }
 
+    WOLF_DEBUG("removing ", all_features_last_.size() , " features in last");
     all_features_last_.clear();
     all_features_last_.splice(all_features_last_.end(),all_features_incoming_);
+    WOLF_DEBUG("last has ", all_features_last_.size() , " features (prev. incoming)");
 
     ProcessorTrackerFeature::resetDerived();
 }
 
 void ProcessorTrackerFeaturePolyline::preProcess()
 {
+    WOLF_DEBUG("PTF ", getName(), "::extractPolylines: ");
     // Extract all polylines from incoming_
-    //WOLF_DEBUG("ProcessorTrackerFeaturePolyline::extractPolylines: ");
     // TODO: sort corners by bearing
     std::list<laserscanutils::Polyline> polylines;
 
     line_finder_.findPolylines(std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_)->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensorPtr()))->getScanParams(), polylines);
-    //WOLF_DEBUG("Polylines extracted: ", polylines.size());
+    WOLF_DEBUG("Polylines extracted: ", polylines.size());
 
     for (auto&& pl : polylines)
     {
@@ -584,11 +642,13 @@ void ProcessorTrackerFeaturePolyline::preProcess()
     }
 
     // compute transformations
-    computeTransformations();
+    if (last_ptr_ != nullptr && incoming_ptr_ != nullptr)
+        computeTransformations();
 }
 
 void ProcessorTrackerFeaturePolyline::postProcess()
 {
+    WOLF_DEBUG("PTF ", getName(), "::postProcess: ");
     // Try to close & classify modified landmarks
     while (!modified_lmks_.empty())
     {
@@ -629,178 +689,12 @@ void ProcessorTrackerFeaturePolyline::postProcess()
     }
 }
 
-FeatureMatchPolyline2DList ProcessorTrackerFeaturePolyline::tryMatchWithFeature(const FeaturePolyline2DPtr& _ftr_L, const FeaturePolyline2DPtr& _ftr_I) const
-{
-    FeatureMatchPolyline2DList ftr_matches;
-
-    // compute best sq distance matching
-    MatchPolyline2DList matches = computeBestRigidTrans(_ftr_I->getPoints(),       // <--feature incoming points
-                                                        _ftr_I->isFirstDefined(),
-                                                        _ftr_I->isLastDefined(),
-                                                        false,                     // <--feature is not closed for sure
-                                                        _ftr_L->getPoints(),       // <--feature last points
-                                                        _ftr_L->isFirstDefined(),
-                                                        _ftr_L->isLastDefined(),
-                                                        false,                     // <--feature is not closed for sure
-                                                        params_tracker_feature_polyline_->match_alignment_position_sq_norm_th);
-    // valid match
-    for (auto match : matches)
-    {
-        auto ftr_match = std::make_shared<FeatureMatchPolyline2D>();
-        // feature incoming point id's
-        ftr_match->feature_incoming_from_id_ = match->from_A_id_;
-        ftr_match->feature_incoming_to_id_   = match->to_A_id_;
-        // feature last point id's
-        ftr_match->feature_last_from_id_ = match->from_B_id_;
-        ftr_match->feature_last_to_id_   = match->to_B_id_;
-        // incoming last transformation
-        ftr_match->T_incoming_last_ = match->T_A_B_;
-        // score
-        ftr_match->normalized_score_ = match->normalized_score_;
-        // feature correspondence
-        ftr_match->feature_ptr_ = _ftr_L;
-        // store in list
-        ftr_matches.push_back(ftr_match);
-    }
-
-    return ftr_matches;
-
-    /*
-    //Search along all possible overlapping configurations between A&B points
-    //     We define offset as the correspondence (if exists) of the first point_L to B's.
-    //     Offset is within [-last_I, last_L]
-
-    bool print = true;
-
-    int from_L, to_L, from_I, to_I, N_overlapped;
-    int first_L = 0;//(_ftr_L->isFirstDefined() ? 0 : 1);
-    int first_I = 0;//(_ftr_I->isFirstDefined() ? 0 : 1);
-    int last_L = _ftr_L->getNPoints() - 1;//(_ftr_L->isLastDefined() ? _ftr_L->getNPoints() - 1 : _ftr_L->getNPoints() - 2);
-    int last_I = _ftr_I->getNPoints() - 1;//(_ftr_I->isLastDefined() ? _ftr_I->getNPoints() - 1 : _ftr_I->getNPoints() - 2);
-
-    Eigen::MatrixXs points_L = _ftr_L->getPoints();
-    Eigen::MatrixXs points_I = _ftr_I->getPoints();
-
-    if (print)
-    {
-        std::cout << "ProcessorTrackerFeaturePolyline::computeBestMatch...\n";
-        std::cout << "\tdefined points_L:\n" << points_L.topRows(2) << std::endl;
-        std::cout << "\tdefined points_I:\n" << points_I.topRows(2) << std::endl;
-    }
-
-    FeatureMatchPolyline2DPtr best_match = nullptr;
-
-    // Check all overlapping configurations
-    int min_offset = -last_L+first_I;
-    int max_offset = last_I-first_L;
-    for (int offset = min_offset; offset <= max_offset; offset++)
-    {
-        from_L = std::max(first_L,-offset);
-        from_I = std::max(first_I, offset);
-
-        N_overlapped = std::min(last_I - from_I, last_L - from_L)+1;
-
-        to_L = from_L+N_overlapped-1;
-        to_I = from_I+N_overlapped-1;
-
-        if (print)
-        {
-            std::cout << "\toffset   " << offset << std::endl;
-            std::cout << "\t\tfrom_L " << from_L << std::endl;
-            std::cout << "\t\tto_L   " << to_L << std::endl;
-            std::cout << "\t\tfrom_I " << from_I << std::endl;
-            std::cout << "\t\tto_I   " << to_I << std::endl;
-            std::cout << "\t\tlast_L " << last_L << std::endl;
-            std::cout << "\t\tlast_I " << last_I << std::endl;
-            std::cout << "\t\tN_overlapped " << N_overlapped << std::endl;
-        }
-        bool from_L_defined = ((from_L != 0 && from_L != last_L) || _ftr_L->isFirstDefined());
-        bool from_I_defined = ((from_I != 0 && from_I != last_I) || _ftr_I->isFirstDefined());
-        bool to_L_defined   = ((to_L   != 0 && to_L   != last_L) || _ftr_L->isLastDefined());
-        bool to_I_defined   = ((to_I   != 0 && to_I   != last_I) || _ftr_I->isLastDefined());
-
-        // If only one overlapped point, both should be defined
-        if ( N_overlapped== 1 &&
-             (!from_L_defined || !from_I_defined || !to_L_defined || !to_I_defined ) )
-            continue;
-
-        assert(N_overlapped <= points_I.cols());
-        assert(N_overlapped <= points_L.cols());
-        assert(N_overlapped > 0);
-        assert(from_L >= 0);
-        assert(from_I >= 0);
-        assert(to_L <= last_L);
-        assert(to_I <= last_I);
-        assert(to_L < points_L.cols());
-        assert(to_I < points_I.cols());
-        assert(to_I == last_I || to_L == last_L);
-        assert(from_I == first_I || from_L == first_L);
-
-        // POINT-POINT DISTANCES for all overlapped points
-        Eigen::ArrayXXd d = (points_I.block(0,from_I, 2, N_overlapped) - points_L.block(0,from_L, 2, N_overlapped)).array();
-        Eigen::ArrayXd dist2 = d.row(0).pow(2) + d.row(1).pow(2);
-
-        // POINT-LINE DISTANCES for not defined extremes
-        // First
-        if (!from_I_defined || !from_L_defined)
-        {
-            if (print)
-                std::cout << "\t\t\tFirst feature not defined distance to line" << std::endl;
-
-            // std::cout << "\t\t\tL   " << points_L.col(from_L).transpose() << std::endl;
-            // std::cout << "\t\t\tLaux" << points_L.col(from_L+1).transpose() << std::endl;
-            // std::cout << "\t\t\tI   " << points_I.col(from_I).transpose() << std::endl;
-            dist2(0) = sqDistPointToLine(points_L.col(from_L),
-                                         points_L.col(from_L+1),
-                                         points_I.col(from_I),
-                                         from_L_defined,
-                                         from_I_defined);
-            assert(N_overlapped > 1);
-        }
-        // Last
-        if (!to_I_defined || !to_L_defined)
-        {
-            if (print)
-                std::cout << "\t\t\tLast feature not defined distance to line" << std::endl;
-
-            // std::cout << "\t\t\tL   " << points_L.col(to_L).transpose() << std::endl;
-            // std::cout << "\t\t\tLaux" << points_L.col(to_L-1).transpose() << std::endl;
-            // std::cout << "\t\t\tI   " << points_I.col(to_I).transpose() << std::endl;
-            dist2(N_overlapped-1) = sqDistPointToLine(points_L.col(to_L),
-                                                      points_L.col(to_L-1),
-                                                      points_I.col(to_I),
-                                                      to_L_defined,
-                                                      to_I_defined);
-            assert(N_overlapped > 1);
-        }
-        if (print)
-            std::cout << "\t\tsquared distances = " << dist2.transpose() << std::endl;
-
-        // All squared distances should be within a threshold
-        if ((dist2 < params_tracker_feature_polyline_->match_position_error_th*params_tracker_feature_polyline_->match_position_error_th).all())
-        {
-            FeatureMatchPolyline2DPtr current_match;
-            current_match->feature_incoming_from_id_= from_I;
-            current_match->feature_incoming_to_id_= to_I;
-            current_match->feature_last_from_id_= from_L;
-            current_match->feature_last_to_id_= to_L;
-            current_match->feature_ptr_= _ftr_L;
-            current_match->normalized_score_ = dist2.mean();
-
-            // Choose the most overlapped one
-            best_match = bestFeatureMatch(best_match,current_match);
-
-            if (print && best_match == current_match)
-                std::cout << "\tBEST MATCH" << std::endl;
-        }
-    }
-
-    return best_match;
-    */
-}
-
 void ProcessorTrackerFeaturePolyline::tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches, const FeaturePolyline2DPtr& _ftr_L, const FeaturePolyline2DPtr& _ftr_I, const Eigen::Matrix3s& _T_last_incoming_prior) const
 {
+    WOLF_DEBUG("PTF ", getName(), "::tryMatchWithFeature: ");
+    WOLF_DEBUG("last incomming pose ", T2pose2D(_T_last_incoming_prior).transpose());
+    WOLF_DEBUG("incomming last pose ", T2pose2D(_T_last_incoming_prior.inverse()).transpose());
+
     // compute best sq distance matching
     MatchPolyline2DList matches = computeBestRigidTrans(_ftr_I->getPoints(),       // <--feature incoming points
                                                         _ftr_I->isFirstDefined(),
@@ -814,10 +708,15 @@ void ProcessorTrackerFeaturePolyline::tryMatchWithFeature(FeatureMatchPolyline2D
     // valid match
     for (auto match : matches)
     {
+        WOLF_DEBUG("match with pose ", T2pose2D(match->T_A_B_).transpose());
         Scalar sq_norm = T2pose2D(_T_last_incoming_prior * match->T_A_B_).squaredNorm();
 
         if (sq_norm > params_tracker_feature_polyline_->match_feature_pose_sq_norm_th)
+        {
+            WOLF_DEBUG("match with squared norm ", sq_norm, " discarded");
             continue;
+        }
+        WOLF_DEBUG("match with squared norm ", sq_norm, " stored!");
 
         auto ftr_match = std::make_shared<FeatureMatchPolyline2D>();
         // feature incoming point id's
@@ -837,72 +736,12 @@ void ProcessorTrackerFeaturePolyline::tryMatchWithFeature(FeatureMatchPolyline2D
     }
 }
 
-//wolf::FeatureMatchPolyline2DPtr ProcessorTrackerFeaturePolyline::bestFeatureMatch(const FeatureMatchPolyline2DPtr& _match_A, const FeatureMatchPolyline2DPtr& _match_B) const
-//{
-//    assert(_match_A != nullptr || _match_B != nullptr);
-//    if (_match_A == nullptr)
-//        return _match_B;
-//
-//    if (_match_B == nullptr)
-//        return _match_A;
-//
-//    // Choose the most overlapped match, if equally overlapped: the best score
-//    int overlapping_A = _match_A->feature_incoming_to_id_ - _match_A->feature_incoming_from_id_;
-//    int overlapping_B = _match_B->feature_incoming_to_id_ - _match_B->feature_incoming_from_id_;
-//    if (overlapping_A > overlapping_B ||
-//        (overlapping_A == overlapping_B && _match_A->normalized_score_ > _match_B->normalized_score_))
-//        return _match_A;
-//
-//    return _match_B;
-//}
-
-
-LandmarkMatchPolyline2DList ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const
-{
-    LandmarkMatchPolyline2DList lmk_matches;
-
-    // compute best sq distance matching
-    MatchPolyline2DList matches = computeBestRigidTrans(_lmk_ptr->computePointsGlobal(),  // <--landmark points in local coordinates
-                                                        _lmk_ptr->isFirstDefined(),
-                                                        _lmk_ptr->isLastDefined(),
-                                                        _lmk_ptr->isClosed(),
-                                                        _feat_ptr->getPoints(),        // <--feature points
-                                                        _feat_ptr->isFirstDefined(),
-                                                        _feat_ptr->isLastDefined(),
-                                                        false,                      // <--feature is not closed for sure
-                                                        params_tracker_feature_polyline_->match_alignment_position_sq_norm_th);
-    // valid match
-    for (auto match : matches)
-    {
-        auto lmk_match = std::make_shared<LandmarkMatchPolyline2D>();
-
-        // landmark
-        lmk_match->landmark_ptr_=_lmk_ptr;
-        lmk_match->landmark_version_=_lmk_ptr->getVersion();
-        // landmark point id's
-        std::vector<int> lmk_valid_ids = _lmk_ptr->getValidIds();
-        lmk_match->landmark_from_id_ = lmk_valid_ids[match->from_A_id_];
-        lmk_match->landmark_to_id_   = lmk_valid_ids[match->to_A_id_];
-        // feature point id's
-        lmk_match->feature_from_id_ = match->from_B_id_;
-        lmk_match->feature_to_id_   = match->to_B_id_;
-
-        // incoming last transformation
-        lmk_match->T_feature_landmark_ = match->T_A_B_.inverse();
-        // score
-        lmk_match->normalized_score_ = match->normalized_score_;
-
-        assert(lmk_match->feature_from_id_ == 0 || !_lmk_ptr->isClosed());
-        assert(lmk_match->feature_to_id_ == _feat_ptr->getNPoints()-1 || !_lmk_ptr->isClosed());
-
-        // store in list
-        lmk_matches.push_back(lmk_match);
-    }
-    return lmk_matches;
-}
-
-void ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr, const Eigen::Matrix3s& _T_feature_landmark_prior) const
+void ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches,
+                                                           const LandmarkPolyline2DPtr& _lmk_ptr,
+                                                           const FeaturePolyline2DPtr& _feat_ptr,
+                                                           const Eigen::Matrix3s& _T_feature_landmark_prior) const
 {
+    WOLF_DEBUG("PTF ", getName(), "::tryMatchWithLandmark: ");
     // compute best sq distance matching
     MatchPolyline2DList matches = computeBestRigidTrans(_lmk_ptr->computePointsGlobal(),  // <--landmark points in local coordinates
                                                         _lmk_ptr->isFirstDefined(),
@@ -949,6 +788,7 @@ void ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(LandmarkMatchPolyline
 
 bool ProcessorTrackerFeaturePolyline::updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr)
 {
+    WOLF_DEBUG("PTF ", getName(), "::updateMatchWithLandmark: ");
     assert(_lmk_ptr->id() == _lmk_match->landmark_ptr_->id());
 
     // merged: update the pl_lmk pointer
@@ -959,55 +799,29 @@ bool ProcessorTrackerFeaturePolyline::updateMatchWithLandmark(LandmarkMatchPolyl
     }
 
     // try match with _feat_ptr again
-    auto new_lmk_matches = tryMatchWithLandmark(_lmk_ptr,_feat_ptr);
-
-    // choose the one with smallest difference from movement of the old match
-    LandmarkMatchPolyline2DPtr best_lmk_match = nullptr;
-    Scalar best_sq_norm = params_tracker_feature_polyline_->match_landmark_pose_sq_norm_th;
-    for (auto new_lmk_match : new_lmk_matches)
-    {
-        Scalar sq_norm = T2pose2D(_lmk_match->T_feature_landmark_.inverse() * new_lmk_match->T_feature_landmark_).squaredNorm();
-        if (sq_norm < best_sq_norm)
-        {
-            best_lmk_match = new_lmk_match;
-            best_sq_norm = sq_norm;
-        }
-    }
-
-    _lmk_match = best_lmk_match;
+    LandmarkMatchPolyline2DScalarMap new_lmk_matches;
+    tryMatchWithLandmark(new_lmk_matches,_lmk_ptr,_feat_ptr, _lmk_match->T_feature_landmark_);
 
     // not valid match
-    if (_lmk_match == nullptr)
+    if (new_lmk_matches.empty())
     {
+        _lmk_match = nullptr;
         landmark_match_map_.erase(_feat_ptr);
         return false;
     }
 
     // valid match: update landmark match of last
-    landmark_match_map_[_feat_ptr] = best_lmk_match;
+    _lmk_match = new_lmk_matches.begin()->second; // the frist is the one with smallest difference from movement of the old match
+    landmark_match_map_[_feat_ptr] = _lmk_match;
     return true;
 }
 
 // MATH ///////////////////////////////////////////////////////////////////////////////////////////////////////////
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
-/*void ProcessorTrackerFeaturePolyline::expectedFeatureLast(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_, Eigen::MatrixXs& expected_feature_cov_) const
-{
-    assert(_landmark_ptr->getType() == "POLYLINE 2D" && "ProcessorTrackerFeaturePolyline::expectedFeature: Bad landmark type");
-    LandmarkPolyline2DPtr pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(_landmark_ptr);
-
-    //std::cout << "ProcessorPolyline::expectedFeature" << std::endl;
-    //std::cout << "t_world_sensor_last_: " << t_world_sensor_last_.transpose() << std::endl;
-    //std::cout << "R_sensor_world_last_: "  << std::endl << R_sensor_world_last_ << std::endl;
-
-    expected_feature_ = pl_lmk->computePointsGlobal();
-    expected_feature_.topRows(2) = R_sensor_world_last_ * (expected_feature_.topRows(2).colwise() - t_world_sensor_last_);
-    expected_feature_cov_ = pl_lmk->computePointsCovGlobal();
-    // TODO Rotate covariances
-}*/
-
 void ProcessorTrackerFeaturePolyline::computeTransformations()
 {
+    //WOLF_DEBUG("ProcessorTrackerFeaturePolyline::computeTransformations: ");
     Eigen::Vector3s vehicle_pose_incoming = getProblem()->getState(incoming_ptr_->getTimeStamp());
     Eigen::Vector3s vehicle_pose_last     = getProblem()->getState(last_ptr_->getTimeStamp());
 
@@ -1052,13 +866,16 @@ void ProcessorTrackerFeaturePolyline::computeTransformations()
     R_last_incoming_ = R_world_sensor_last_.transpose() * R_world_sensor_incoming_;
     t_last_incoming_ = t_sensor_world_last_ + R_sensor_world_last_ * t_world_sensor_incoming_;
 
-    //std::cout << "t_world_robot_incoming_ " << t_world_robot_.transpose() << std::endl;
-    //std::cout << "t_robot_sensor_incoming_ " << t_robot_sensor_.transpose() << std::endl;
-    //std::cout << "R_robot_sensor_incoming_ " << std::endl << R_robot_sensor_ << std::endl;
-    //std::cout << "t_world_sensor_incoming_ " << t_world_sensor_.transpose() << std::endl;
-    //std::cout << "R_world_sensor_incoming_ " << std::endl << R_world_sensor_ << std::endl;
-    //std::cout << "t_sensor_world_incoming_ " << t_sensor_world_.transpose() << std::endl;
-    //std::cout << "R_sensor_world_incoming_ " << std::endl << R_sensor_world_ << std::endl;
+    //std::cout << "t_robot_sensor_ " << t_robot_sensor_.transpose() << std::endl;
+    //std::cout << "R_robot_sensor_ " << std::endl << R_robot_sensor_ << std::endl;
+    //std::cout << "t_world_sensor_incoming_ " << t_world_sensor_incoming_.transpose() << std::endl;
+    //std::cout << "R_world_sensor_incoming_ " << std::endl << R_world_sensor_incoming_ << std::endl;
+    //std::cout << "t_sensor_world_incoming_ " << t_sensor_world_incoming_.transpose() << std::endl;
+    //std::cout << "R_sensor_world_incoming_ " << std::endl << R_sensor_world_incoming_ << std::endl;
+    //std::cout << "t_world_sensor_last_ " << t_world_sensor_last_.transpose() << std::endl;
+    //std::cout << "R_world_sensor_last_ " << std::endl << R_world_sensor_last_ << std::endl;
+    //std::cout << "t_sensor_world_last_ " << t_sensor_world_last_.transpose() << std::endl;
+    //std::cout << "R_sensor_world_last_ " << std::endl << R_sensor_world_last_ << std::endl;
     //std::cout << "t_incoming_last_ " << t_incoming_last_.transpose() << std::endl;
     //std::cout << "R_incoming_last_ " << std::endl << R_incoming_last_ << std::endl;
     //std::cout << "t_last_incoming_ " << t_last_incoming_.transpose() << std::endl;
diff --git a/test/gtest_polyline_2D.cpp b/test/gtest_polyline_2D.cpp
index d3c96443811118c8e3e882f2d6eb7473bb25bb52..d8c50e392e2551fd72323d8cb9c259e954ee9c4a 100644
--- a/test/gtest_polyline_2D.cpp
+++ b/test/gtest_polyline_2D.cpp
@@ -5,6 +5,13 @@
 using namespace wolf;
 using namespace Eigen;
 
+TEST(Polyline2DTest, Transformations)
+{
+    ASSERT_MATRIX_APPROX(Matrix3s::Identity(), pose2T2D(Vector2s::Zero(),0), 1e-12);
+    ASSERT_MATRIX_APPROX(Matrix3s::Identity(), pose2T2D(Vector3s::Zero()), 1e-12);
+    ASSERT_MATRIX_APPROX(Vector3s::Zero(), T2pose2D(Matrix3s::Identity()), 1e-12);
+}
+
 TEST(Polyline2DTest, RigidTransformation)
 {
     // Random points around random position