From aed1fbf5ee288de3fb7047393decb2b00a5648d0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Wed, 3 Apr 2019 14:27:55 +0200
Subject: [PATCH] WIP polylines..

---
 CMakeLists.txt                                |    5 +-
 .../landmark/landmark_match_polyline_2D.h     |    2 +
 include/base/processor/polyline_2D_utils.h    |    2 +-
 ...> processor_tracker_feature_polyline_2D.h} |   22 +-
 src/landmark/landmark_match_polyline_2D.cpp   |   66 +
 src/processor/polyline_2D_utils.cpp           |   37 +-
 ...processor_tracker_feature_polyline_2D.cpp} |  298 +++--
 test/gtest_polyline_2D.cpp                    |  161 ++-
 ..._processor_tracker_feature_polyline_2D.cpp | 1183 +++++++++++++++++
 9 files changed, 1667 insertions(+), 109 deletions(-)
 rename include/base/processor/{processor_tracker_feature_polyline.h => processor_tracker_feature_polyline_2D.h} (92%)
 create mode 100644 src/landmark/landmark_match_polyline_2D.cpp
 rename src/processor/{processor_tracker_feature_polyline.cpp => processor_tracker_feature_polyline_2D.cpp} (76%)
 create mode 100644 test/gtest_processor_tracker_feature_polyline_2D.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index be9884623..0728998b8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -555,6 +555,7 @@ SET(SRCS_LANDMARK
   src/landmark/landmark_container.cpp
   src/landmark/landmark_line_2D.cpp
   src/landmark/landmark_polyline_2D.cpp
+  src/landmark/landmark_match_polyline_2D.cpp
   )
 SET(SRCS_PROCESSOR
   src/processor/processor_frame_nearest_neighbor_filter.cpp
@@ -619,7 +620,7 @@ IF (laser_scan_utils_FOUND)
     include/base/processor/processor_polyline.h
     include/base/processor/processor_tracker_feature_corner.h
     include/base/processor/processor_tracker_landmark_corner.h
-  	include/base/processor/processor_tracker_feature_polyline.h
+  	include/base/processor/processor_tracker_feature_polyline_2D.h
   	include/base/processor/polyline_2D_utils.h
     )
   SET(HDRS_SENSOR ${HDRS_SENSOR}
@@ -632,7 +633,7 @@ IF (laser_scan_utils_FOUND)
     src/processor/processor_polyline.cpp
     src/processor/processor_tracker_feature_corner.cpp
     src/processor/processor_tracker_landmark_corner.cpp
-  	src/processor/processor_tracker_feature_polyline.cpp
+  	src/processor/processor_tracker_feature_polyline_2D.cpp
   	src/processor/polyline_2D_utils.cpp
     )
   SET(SRCS_SENSOR ${SRCS_SENSOR}
diff --git a/include/base/landmark/landmark_match_polyline_2D.h b/include/base/landmark/landmark_match_polyline_2D.h
index f7835b0c8..24d1eddf4 100644
--- a/include/base/landmark/landmark_match_polyline_2D.h
+++ b/include/base/landmark/landmark_match_polyline_2D.h
@@ -25,6 +25,8 @@ struct LandmarkMatchPolyline2D : public LandmarkMatch
     int feature_to_id_;
     int landmark_version_;
     Eigen::Matrix3s T_feature_landmark_;
+
+    bool check() const;
 };
 
 }//end namespace
diff --git a/include/base/processor/polyline_2D_utils.h b/include/base/processor/polyline_2D_utils.h
index 94d262eee..78d6b960a 100644
--- a/include/base/processor/polyline_2D_utils.h
+++ b/include/base/processor/polyline_2D_utils.h
@@ -40,7 +40,7 @@ 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);
+Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_incoming, const Eigen::MatrixXs& _points_last, const bool& first_defined = true, const bool& last_defined=true);
 
 /** \brief Computes the squared distance from a point to a segment defined by two points
  **/
diff --git a/include/base/processor/processor_tracker_feature_polyline.h b/include/base/processor/processor_tracker_feature_polyline_2D.h
similarity index 92%
rename from include/base/processor/processor_tracker_feature_polyline.h
rename to include/base/processor/processor_tracker_feature_polyline_2D.h
index 94ee78021..f88366880 100644
--- a/include/base/processor/processor_tracker_feature_polyline.h
+++ b/include/base/processor/processor_tracker_feature_polyline_2D.h
@@ -1,12 +1,12 @@
 /*
- * processor_tracker_feature_polyline.h
+ * processor_tracker_feature_polyline_2D.h
  *
  *  Created on: Mar 11, 2019
  *      Author: jvallve
  */
 
-#ifndef PROCESSOR_TRACKER_FEATURE_POLYLINE_H_
-#define PROCESSOR_TRACKER_FEATURE_POLYLINE_H_
+#ifndef PROCESSOR_TRACKER_FEATURE_POLYLINE_2D_H_
+#define PROCESSOR_TRACKER_FEATURE_POLYLINE_2D_H_
 
 #include "base/processor/polyline_2D_utils.h"
 #include "base/processor/processor_tracker_feature.h"
@@ -27,14 +27,14 @@
 namespace wolf
 {
 
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeaturePolyline);
-WOLF_PTR_TYPEDEFS(ProcessorTrackerFeaturePolyline);
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeaturePolyline2D);
+WOLF_PTR_TYPEDEFS(ProcessorTrackerFeaturePolyline2D);
 typedef std::list<FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DList;
 typedef std::list<LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DList;
 typedef std::map<Scalar,FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DScalarMap;
 typedef std::map<Scalar,LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DScalarMap;
 
-struct ProcessorParamsTrackerFeaturePolyline : public ProcessorParamsTrackerFeature
+struct ProcessorParamsTrackerFeaturePolyline2D : public ProcessorParamsTrackerFeature
 {
     laserscanutils::LineFinderIterativeParams line_finder_params;
     Scalar match_alignment_position_sq_norm_th;
@@ -46,10 +46,10 @@ struct ProcessorParamsTrackerFeaturePolyline : public ProcessorParamsTrackerFeat
     std::vector<PolylineRectangularClass> polyline_classes;
 };
 
-class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
+class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
 {
     protected:
-        ProcessorParamsTrackerFeaturePolylinePtr params_tracker_feature_polyline_;
+        ProcessorParamsTrackerFeaturePolyline2DPtr params_tracker_feature_polyline_;
         laserscanutils::LineFinderIterative line_finder_;
 
         FeatureBaseList all_features_incoming_, all_features_last_;
@@ -68,9 +68,9 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
         bool extrinsics_transformation_computed_;
 
     public:
-        ProcessorTrackerFeaturePolyline(ProcessorParamsTrackerFeaturePolylinePtr _params);
+        ProcessorTrackerFeaturePolyline2D(ProcessorParamsTrackerFeaturePolyline2DPtr _params);
 
-        virtual ~ProcessorTrackerFeaturePolyline();
+        virtual ~ProcessorTrackerFeaturePolyline2D();
 
         virtual void configure(SensorBasePtr _sensor){};
 
@@ -204,4 +204,4 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
 
 } /* namespace wolf */
 
-#endif /* PROCESSOR_TRACKER_FEATURE_POLYLINE_H_ */
+#endif /* PROCESSOR_TRACKER_FEATURE_POLYLINE_2D_H_ */
diff --git a/src/landmark/landmark_match_polyline_2D.cpp b/src/landmark/landmark_match_polyline_2D.cpp
new file mode 100644
index 000000000..f6028792b
--- /dev/null
+++ b/src/landmark/landmark_match_polyline_2D.cpp
@@ -0,0 +1,66 @@
+/*
+ * landmark_match_polyline_2D.cpp
+ *
+ *  Created on: Apr 2, 2019
+ *      Author: jvallve
+ */
+
+#include "base/landmark/landmark_match_polyline_2D.h"
+#include "base/landmark/landmark_polyline_2D.h"
+
+namespace wolf {
+
+bool LandmarkMatchPolyline2D::check() const
+{
+    LandmarkPolyline2DPtr pl_lmk = std::dynamic_pointer_cast<LandmarkPolyline2D>(this->landmark_ptr_);
+    if (pl_lmk == nullptr)
+    {
+        WOLF_ERROR("LandmarkMatchPolyline2D landmark is not polyline 2D");
+        return false;
+    }
+    if (!pl_lmk->isClosed())
+    {
+        if (landmark_from_id_ > landmark_to_id_)
+        {
+            WOLF_ERROR("LandmarkMatchPolyline2D lmk_from > lmk_to with a not closed lmk");
+            return false;
+        }
+        if (landmark_from_id_ < pl_lmk->getFirstId())
+        {
+            WOLF_ERROR("LandmarkMatchPolyline2D lmk_from < lmk_first with a not closed lmk");
+            return false;
+        }
+        if (landmark_to_id_ > pl_lmk->getLastId())
+        {
+            WOLF_ERROR("LandmarkMatchPolyline2D lmk_to < lmk_last with a not closed lmk");
+            return false;
+        }
+    }
+    int lmk_id = landmark_from_id_;
+    int ftr_id = feature_from_id_;
+    while (1)
+    {
+        if (lmk_id == landmark_to_id_)
+        {
+            if (ftr_id != feature_to_id_)
+            {
+                WOLF_ERROR("LandmarkMatchPolyline2D landmark_to_id_ does not correspond to feature_to_id_");
+                return false;
+            }
+            break;
+        }
+        else if (lmk_id == pl_lmk->getLastId() && !pl_lmk->isClosed())
+        {
+            WOLF_ERROR("LandmarkMatchPolyline2D lmk_id == lmk_last_id but lmk_id != landmark_to_id_ in a not closed lmk");
+            return false;
+        }
+        else
+        {
+            lmk_id = pl_lmk->getNextValidId(lmk_id);
+            ftr_id++;
+        }
+    }
+    return true;
+}
+
+} // namespace wolf
diff --git a/src/processor/polyline_2D_utils.cpp b/src/processor/polyline_2D_utils.cpp
index 25a84f85f..fc0e62dda 100644
--- a/src/processor/polyline_2D_utils.cpp
+++ b/src/processor/polyline_2D_utils.cpp
@@ -31,31 +31,46 @@ Eigen::Vector3s T2pose2D(const Eigen::Matrix3s& T)
     return pose;
 }
 
-Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_A, const Eigen::MatrixXs& _points_B)
+Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_A, const Eigen::MatrixXs& _points_B, const bool& first_defined, const bool& last_defined)
 {
     assert(_points_A.cols() == _points_B.cols());
     assert(_points_A.rows() >= 2);
     assert(_points_B.rows() >= 2);
-    assert(_points_A.cols() >= 2);
 
+
+    int N_defined_points = _points_A.cols() - (first_defined ? 0 : 1) - (last_defined ? 0 : 1);
+    bool all_defined = first_defined && last_defined;
     Eigen::Vector3s t_A_B;
+    assert(N_defined_points > 0 && _points_A.cols() > 1);
 
     // Rotation
-    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();
+    Eigen::VectorXs angles_A_B(Eigen::VectorXs::Zero(_points_A.cols()));
+    Eigen::Vector2s points_A_mean((all_defined ? _points_A.row(0).mean() : _points_A.row(0).segment(first_defined ? 0 : 1, N_defined_points).mean()),
+                                  (all_defined ? _points_A.row(1).mean() : _points_A.row(1).segment(first_defined ? 0 : 1, N_defined_points).mean()));
+    Eigen::Vector2s points_B_mean((all_defined ? _points_B.row(0).mean() : _points_B.row(0).segment(first_defined ? 0 : 1, N_defined_points).mean()),
+                                  (all_defined ? _points_B.row(1).mean() : _points_B.row(1).segment(first_defined ? 0 : 1, N_defined_points).mean()));
+    Eigen::MatrixXs points_A_centered = _points_A.topRows<2>().colwise()-points_A_mean;
+    Eigen::MatrixXs points_B_centered = _points_B.topRows<2>().colwise()-points_B_mean;
 
-    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)));
+    for (auto i = 0; i < _points_A.cols(); i++ )
+        angles_A_B(i) = pi2pi(atan2(points_A_centered(1,i),points_A_centered(0,i)) - atan2(points_B_centered(1,i),points_B_centered(0,i)));
 
     // fix 2nd&3rd quadrant angles bad resulting mean
     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_A_B(2) = angles_A_B.mean();
+        angles_A_B = (angles_A_B.array() >= 0).select(angles_A_B, angles_A_B.array()+2*M_PI);
+
+    // if only one defined point, mean of not defined points angles (mean is pivot)
+    if (N_defined_points == 1)
+        t_A_B(2) = ((first_defined ? 0 : angles_A_B(0)) + (last_defined ? 0 : angles_A_B(_points_A.cols()-1))) / ((first_defined ? 0 : 1) + (last_defined ? 0 : 1));
+    // use all points if all defined
+    else if (all_defined)
+        t_A_B(2) = angles_A_B.mean();
+    // otherwise compute using defined points
+    else
+        t_A_B(2) = angles_A_B.segment(first_defined ? 0 : 1, N_defined_points).mean();
 
     // Translation
-    t_A_B.head<2>() = _points_A.topRows<2>().rowwise().mean() - Eigen::Rotation2D<Scalar>(t_A_B(2))*_points_B.topRows<2>().rowwise().mean();
+    t_A_B.head<2>() = points_A_mean - Eigen::Rotation2D<Scalar>(t_A_B(2))*points_B_mean;
 
     return t_A_B;
 }
diff --git a/src/processor/processor_tracker_feature_polyline.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp
similarity index 76%
rename from src/processor/processor_tracker_feature_polyline.cpp
rename to src/processor/processor_tracker_feature_polyline_2D.cpp
index b230252eb..b90877de9 100644
--- a/src/processor/processor_tracker_feature_polyline.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2D.cpp
@@ -1,16 +1,16 @@
 /*
- * processor_tracker_feature_polyline.cpp
+ * processor_tracker_feature_polyline_2D.cpp
  *
  *  Created on: Mar 11, 2019
  *      Author: jvallve
  */
 
-#include "base/processor/processor_tracker_feature_polyline.h"
+#include "base/processor/processor_tracker_feature_polyline_2D.h"
 
 namespace wolf
 {
 
-ProcessorTrackerFeaturePolyline::ProcessorTrackerFeaturePolyline(ProcessorParamsTrackerFeaturePolylinePtr _params) :
+ProcessorTrackerFeaturePolyline2D::ProcessorTrackerFeaturePolyline2D(ProcessorParamsTrackerFeaturePolyline2DPtr _params) :
      ProcessorTrackerFeature("TRACKER FEATURE POLYLINE",_params),
      params_tracker_feature_polyline_(_params),
      line_finder_(_params->line_finder_params),
@@ -19,11 +19,11 @@ ProcessorTrackerFeaturePolyline::ProcessorTrackerFeaturePolyline(ProcessorParams
 
 }
 
-ProcessorTrackerFeaturePolyline::~ProcessorTrackerFeaturePolyline()
+ProcessorTrackerFeaturePolyline2D::~ProcessorTrackerFeaturePolyline2D()
 {
 }
 
-unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseList& _features_last_in,
+unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseList& _features_last_in,
                                                             FeatureBaseList& _features_incoming_out,
                                                             FeatureMatchMap& _feature_correspondences)
 {
@@ -93,8 +93,18 @@ unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseLis
                 else if (pl_lmk->isRemoving())
                     continue;
 
+                // try to concatenate landmark match
+                auto pl_lmk_match_incoming = concatenateFeatureLandmarkMatches(pl_incoming,
+                                                                               best_ftr_match,
+                                                                               landmark_match_map_[pl_last]);
                 // Add the incoming match to landmark_match_map
-                landmark_match_map_[pl_incoming] = concatenateFeatureLandmarkMatches(pl_incoming, best_ftr_match, landmark_match_map_[pl_last]);
+                if (pl_lmk_match_incoming != nullptr)
+                    landmark_match_map_[pl_incoming] = pl_lmk_match_incoming;
+                else
+                {
+                    WOLF_DEBUG("PTFP ", getName(), "::trackFeatures: incoming track lost common points with landmark");
+                    continue;
+                }
             }
 
             // TRACK valid
@@ -122,9 +132,9 @@ unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseLis
     return _feature_correspondences.size();
 }
 
-bool ProcessorTrackerFeaturePolyline::correctFeatureDrift(const FeatureBasePtr _origin_feature,
-                                                          const FeatureBasePtr _last_feature,
-                                                          FeatureBasePtr _incoming_feature)
+bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr _origin_feature,
+                                                            const FeatureBasePtr _last_feature,
+                                                            FeatureBasePtr _incoming_feature)
 {
     //WOLF_DEBUG("PTFP ", getName(), "::correctFeatureDrift: ");
     // If incoming observed new landmark points & landmark has points
@@ -138,7 +148,7 @@ bool ProcessorTrackerFeaturePolyline::correctFeatureDrift(const FeatureBasePtr _
     return true;
 }
 
-bool ProcessorTrackerFeaturePolyline::voteForKeyFrame()
+bool ProcessorTrackerFeaturePolyline2D::voteForKeyFrame()
 {
     //WOLF_DEBUG("PTFP ", getName(), "::voteForKeyFrame: ");
     // TODO
@@ -151,7 +161,7 @@ bool ProcessorTrackerFeaturePolyline::voteForKeyFrame()
     return false;
 }
 
-unsigned int ProcessorTrackerFeaturePolyline::processNew(const unsigned int& _max_features)
+unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const unsigned int& _max_features)
 {
     WOLF_DEBUG("PTFP ", getName(), "::processNew: ");
     unsigned int n = ProcessorTrackerFeature::processNew(_max_features);
@@ -224,23 +234,35 @@ unsigned int ProcessorTrackerFeaturePolyline::processNew(const unsigned int& _ma
     }
 
     // store in landmark_match_map_ the incoming features
-    for (auto ftr_it = std::prev(incoming_ptr_->getFeatureList().end(),n);
-              ftr_it != incoming_ptr_->getFeatureList().end();
-              ftr_it++)
+    auto ftr_it = std::prev(incoming_ptr_->getFeatureList().end(),n);
+    while (ftr_it != incoming_ptr_->getFeatureList().end())
     {
         auto pl_incoming = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
         assert(matches_last_from_incoming_.find(pl_incoming) != matches_last_from_incoming_.end() && "last-incoming match not found");
         auto pl_ftr_match = std::static_pointer_cast<FeatureMatchPolyline2D>(matches_last_from_incoming_[pl_incoming]);
         assert(landmark_match_map_.find(pl_ftr_match->feature_ptr_) != landmark_match_map_.end() && "last-lmk match not found");
-        landmark_match_map_[pl_incoming] = concatenateFeatureLandmarkMatches(pl_incoming,
-                                                                             pl_ftr_match,
-                                                                             landmark_match_map_[pl_ftr_match->feature_ptr_]);
+        auto pl_lmk_match_incoming = concatenateFeatureLandmarkMatches(pl_incoming,
+                                                                       pl_ftr_match,
+                                                                       landmark_match_map_[pl_ftr_match->feature_ptr_]);
+        if (pl_lmk_match_incoming != nullptr)
+        {
+            landmark_match_map_[pl_incoming] = pl_lmk_match_incoming;
+            ftr_it++;
+        }
+        else
+        {
+            WOLF_DEBUG("PTFP ", getName(), "::processNew: incoming track lost common points with landmark");
+            matches_last_from_incoming_.erase(pl_incoming);
+            // move feature from known to all features
+            all_features_incoming_.push_back(pl_incoming);
+            ftr_it = incoming_ptr_->getFeatureList().erase(ftr_it);
+        }
     }
     return n;
 }
 
-unsigned int ProcessorTrackerFeaturePolyline::detectNewFeatures(const unsigned int& _max_new_features,
-                                                                FeatureBaseList& _features_last_out)
+unsigned int ProcessorTrackerFeaturePolyline2D::detectNewFeatures(const unsigned int& _max_new_features,
+                                                                  FeatureBaseList& _features_last_out)
 {
     WOLF_DEBUG("PTFP ", getName(), "::detectNewFeatures (", all_features_last_.size(), " in all_features_last_)");
     int prev_size = _features_last_out.size();
@@ -265,7 +287,7 @@ unsigned int ProcessorTrackerFeaturePolyline::detectNewFeatures(const unsigned i
     return 0;
 }
 
-void ProcessorTrackerFeaturePolyline::establishConstraints()
+void ProcessorTrackerFeaturePolyline2D::establishConstraints()
 {
     WOLF_DEBUG("PTFP ", getName(), "::establishConstraints: ");
     unsigned int N_constraints = 0;
@@ -322,16 +344,19 @@ void ProcessorTrackerFeaturePolyline::establishConstraints()
 
         // ESTABLISH CONSTRAINTS
         WOLF_DEBUG("\tEstablishing constraints..");
+        // checks
         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");
-        //std::cout << "Establishing constraint between" << std::endl;
-        //std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapturePtr() ? pl_ftr->getCapturePtr()->id() : 9999999999999999) << " is linked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl;
-        //std::cout << "\tlandmark " << pl_lmk->id() << std::endl;
-        //std::cout << "\tmatch from feature point " << lmk_match->feature_from_id_ << std::endl;
-        //std::cout << "\tmatch to feature point " << lmk_match->feature_to_id_ << std::endl;
-        //std::cout << "\tmatch from landmark point " << lmk_match->landmark_from_id_ << std::endl;
-        //std::cout << "\tmatch to landmark point " << lmk_match->landmark_to_id_ << std::endl;
+        assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
+        assert(lmk_match->check() && "Landmark didn't grow properly");
+
+        std::cout << "Establishing constraint between" << std::endl;
+        std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapturePtr() ? pl_ftr->getCapturePtr()->id() : 9999999999999999) << " is linked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl;
+        std::cout << "\tlandmark " << pl_lmk->id() << std::endl;
+        std::cout << "\tmatch from feature point " << lmk_match->feature_from_id_ << std::endl;
+        std::cout << "\tmatch to feature point " << lmk_match->feature_to_id_ << std::endl;
+        std::cout << "\tmatch from landmark point " << lmk_match->landmark_from_id_ << std::endl;
+        std::cout << "\tmatch to landmark point " << lmk_match->landmark_to_id_ << std::endl;
 
         // Constraints for all feature points
         int lmk_point_id = lmk_match->landmark_from_id_;
@@ -389,11 +414,11 @@ void ProcessorTrackerFeaturePolyline::establishConstraints()
     std::cout << N_constraints << " constraints established" << std::endl;
 }
 
-void ProcessorTrackerFeaturePolyline::emplaceConstraintPointToLine(FeaturePolyline2DPtr _polyline_feature,
-                                                                   LandmarkPolyline2DPtr _polyline_landmark,
-                                                                   const int& _ftr_point_id,
-                                                                   const int& _lmk_point_id,
-                                                                   const int& _lmk_prev_point_id)
+void ProcessorTrackerFeaturePolyline2D::emplaceConstraintPointToLine(FeaturePolyline2DPtr _polyline_feature,
+                                                                     LandmarkPolyline2DPtr _polyline_landmark,
+                                                                     const int& _ftr_point_id,
+                                                                     const int& _lmk_point_id,
+                                                                     const int& _lmk_prev_point_id)
 {
     assert(_polyline_landmark->isValidId(_lmk_point_id) && _polyline_landmark->isValidId(_lmk_prev_point_id));
 
@@ -405,10 +430,10 @@ void ProcessorTrackerFeaturePolyline::emplaceConstraintPointToLine(FeaturePolyli
     _polyline_landmark->addConstrainedBy(new_ctr);
 }
 
-void ProcessorTrackerFeaturePolyline::emplaceConstraintPoint(FeaturePolyline2DPtr _polyline_feature,
-                                                             LandmarkPolyline2DPtr _polyline_landmark,
-                                                             const int& _ftr_point_id,
-                                                             const int& _lmk_point_id)
+void ProcessorTrackerFeaturePolyline2D::emplaceConstraintPoint(FeaturePolyline2DPtr _polyline_feature,
+                                                               LandmarkPolyline2DPtr _polyline_landmark,
+                                                               const int& _ftr_point_id,
+                                                               const int& _lmk_point_id)
 {
     // CREATE CONSTRAINT --------------------
     ConstraintBasePtr new_ctr = std::make_shared<ConstraintPoint2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id);
@@ -418,7 +443,7 @@ void ProcessorTrackerFeaturePolyline::emplaceConstraintPoint(FeaturePolyline2DPt
     _polyline_landmark->addConstrainedBy(new_ctr);
 }
 
-LandmarkBasePtr ProcessorTrackerFeaturePolyline::createLandmark(FeatureBasePtr _feature_ptr)
+LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::createLandmark(FeatureBasePtr _feature_ptr)
 {
     WOLF_DEBUG("PTFP ", getName(), "::createLandmark: ");
     //std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl;
@@ -445,12 +470,12 @@ LandmarkBasePtr ProcessorTrackerFeaturePolyline::createLandmark(FeatureBasePtr _
     //std::cout << "done" << std::endl;
 }
 
-bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match, FeaturePolyline2DPtr& pl_ftr)
+bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match, FeaturePolyline2DPtr& pl_ftr)
 {
     WOLF_DEBUG("PTFP ", getName(), "::modifyLandmarkAndMatch: ");
     auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match->landmark_ptr_);
 
-    bool print = false;
+    bool print = true;
     bool modified = false;
 
     if (print)
@@ -469,8 +494,9 @@ bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyli
         std::cout << "\tmatch from landmark point " << lmk_match->landmark_from_id_ << std::endl;
         std::cout << "\tmatch to landmark point " << lmk_match->landmark_to_id_ << std::endl;
     }
-    Eigen::MatrixXs points_global = R_world_sensor_last_ * pl_ftr->getPoints().topRows<2>() +
-                                    t_world_sensor_last_ * Eigen::VectorXs::Ones(pl_ftr->getNPoints()).transpose();
+    //Eigen::MatrixXs points_global = R_world_sensor_last_ * pl_ftr->getPoints().topRows<2>() +
+    //                                t_world_sensor_last_ * Eigen::VectorXs::Ones(pl_ftr->getNPoints()).transpose();
+    Eigen::MatrixXs points_global = lmk_match->T_feature_landmark_.inverse() * pl_ftr->getPoints();
 
     // MODIFY LANDMARK
     // -----------------GROW Front-----------------
@@ -495,10 +521,18 @@ bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyli
         lmk_match->feature_from_id_ = 0;
         lmk_match->landmark_version_ = pl_lmk->getVersion();
         modified = true;
-        //std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl;
-        //std::cout << "\tfeat to   " << lmk_match->feature_to_id_ << std::endl;
-        //std::cout << "\tland from " << lmk_match->landmark_from_id_ << std::endl;
-        //std::cout << "\tland to   " << lmk_match->landmark_to_id_ << std::endl;
+        if (print)
+        {
+            std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl;
+            std::cout << "\tfeat to   " << lmk_match->feature_to_id_ << std::endl;
+            std::cout << "\tland from " << lmk_match->landmark_from_id_ << std::endl;
+            std::cout << "\tland to   " << lmk_match->landmark_to_id_ << std::endl;
+        }
+
+        // checks
+        assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
+        assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
+        assert(lmk_match->check() && "Landmark didn't grow properly");
     }
     // -----------------CHANGE Front-----------------
     // Change first not defined point if landmark first not defined matched with first feature point that:
@@ -524,8 +558,12 @@ bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyli
             modified = true;
             lmk_match->landmark_version_ = pl_lmk->getVersion();
         }
+
+        // checks
+        assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
+        assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
+        assert(lmk_match->check() && "Landmark didn't grow properly");
     }
-    assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
     // -----------------GROW Back-----------------
     // Add new back points (if any feature point not matched)
     if (lmk_match->feature_to_id_ < pl_ftr->getNPoints()-1)
@@ -548,10 +586,18 @@ bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyli
         lmk_match->feature_to_id_ = pl_ftr->getNPoints()-1;
         lmk_match->landmark_version_ = pl_lmk->getVersion();
         modified = true;
-        //std::cout << "\tfeat from " << pl_match->feature_from_id_ << std::endl;
-        //std::cout << "\tfeat to " << pl_match->feature_to_id_ << std::endl;
-        //std::cout << "\tland from " << pl_match->landmark_from_id_ << std::endl;
-        //std::cout << "\tland to " << pl_match->landmark_to_id_ << std::endl;
+        if (print)
+        {
+            std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl;
+            std::cout << "\tfeat to   " << lmk_match->feature_to_id_ << std::endl;
+            std::cout << "\tland from " << lmk_match->landmark_from_id_ << std::endl;
+            std::cout << "\tland to   " << lmk_match->landmark_to_id_ << std::endl;
+        }
+        // checks
+        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() && "Landmark didn't grow properly");
+        assert(lmk_match->check() && "Landmark didn't grow properly");
     }
     // -----------------CHANGE Back-----------------
     // Change last not defined point if landmark last not defined matched with last feature point that:
@@ -577,6 +623,12 @@ bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyli
             lmk_match->landmark_version_ = pl_lmk->getVersion();
             modified = true;
         }
+
+        // checks
+        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() && "Landmark didn't grow properly");
+        assert(lmk_match->check() && "Landmark didn't grow properly");
     }
     assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
 
@@ -596,7 +648,7 @@ bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyli
     return modified;
 }
 
-void ProcessorTrackerFeaturePolyline::advanceDerived()
+void ProcessorTrackerFeaturePolyline2D::advanceDerived()
 {
     WOLF_DEBUG("PTFP ", getName(), "::advanceDerived: ");
     // remove last features from landmark_match_map_
@@ -616,7 +668,7 @@ void ProcessorTrackerFeaturePolyline::advanceDerived()
     ProcessorTrackerFeature::advanceDerived();
 }
 
-void ProcessorTrackerFeaturePolyline::resetDerived()
+void ProcessorTrackerFeaturePolyline2D::resetDerived()
 {
     WOLF_DEBUG("PTFP ", getName(), "::resetDerived: ");
     // remove last features from landmark_match_map_
@@ -637,7 +689,7 @@ void ProcessorTrackerFeaturePolyline::resetDerived()
     ProcessorTrackerFeature::resetDerived();
 }
 
-void ProcessorTrackerFeaturePolyline::preProcess()
+void ProcessorTrackerFeaturePolyline2D::preProcess()
 {
     WOLF_DEBUG("PTFP ", getName(), "::extractPolylines: ");
     // Extract all polylines from incoming_
@@ -664,7 +716,7 @@ void ProcessorTrackerFeaturePolyline::preProcess()
         WOLF_DEBUG("PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
 }
 
-void ProcessorTrackerFeaturePolyline::postProcess()
+void ProcessorTrackerFeaturePolyline2D::postProcess()
 {
     WOLF_DEBUG("PTFP ", getName(), "::postProcess: ");
     // Try to close & classify modified landmarks
@@ -710,10 +762,10 @@ void ProcessorTrackerFeaturePolyline::postProcess()
         WOLF_DEBUG("PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)");
 }
 
-void ProcessorTrackerFeaturePolyline::tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches,
-                                                          const FeaturePolyline2DPtr& _ftr_L,
-                                                          const FeaturePolyline2DPtr& _ftr_I,
-                                                          const Eigen::Matrix3s& _T_last_incoming_prior) const
+void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches,
+                                                            const FeaturePolyline2DPtr& _ftr_L,
+                                                            const FeaturePolyline2DPtr& _ftr_I,
+                                                            const Eigen::Matrix3s& _T_last_incoming_prior) const
 {
     //WOLF_DEBUG("PTFP ", getName(), "::tryMatchWithFeature: ");
 
@@ -740,7 +792,19 @@ void ProcessorTrackerFeaturePolyline::tryMatchWithFeature(FeatureMatchPolyline2D
         ftr_match->feature_last_from_id_ = best_match->from_B_id_;
         ftr_match->feature_last_to_id_   = best_match->to_B_id_;
         // incoming last transformation
-        //ftr_match->T_incoming_last_ = match->T_A_B_; unavailable data
+        bool from_defined = (ftr_match->feature_incoming_from_id_ != 0                      || _ftr_I->isFirstDefined()) &&
+                            (ftr_match->feature_last_from_id_     != 0                      || _ftr_L->isFirstDefined());
+        bool to_defined   = (ftr_match->feature_incoming_to_id_ != _ftr_I->getNPoints()-1   || _ftr_I->isLastDefined())  &&
+                            (ftr_match->feature_last_to_id_     != _ftr_L->getNPoints()-1   || _ftr_L->isLastDefined());
+        int N_points = ftr_match->feature_incoming_to_id_ - ftr_match->feature_incoming_from_id_;
+        int N_def_points = N_points - (from_defined ? 0:1) - (to_defined ? 0:1);
+        if (N_def_points > 0 && N_points > 2)
+            ftr_match->T_incoming_last_ = pose2T2D(computeRigidTrans(_ftr_I->getPoints().middleCols(ftr_match->feature_incoming_from_id_,N_points),
+                                                                     _ftr_L->getPoints().middleCols(ftr_match->feature_last_from_id_,N_points),
+                                                                     from_defined,
+                                                                     to_defined));
+        else
+            ftr_match->T_incoming_last_ = _T_last_incoming_prior;
         // score
         ftr_match->normalized_score_ = best_match->normalized_score_;
         // feature correspondence
@@ -793,10 +857,10 @@ void ProcessorTrackerFeaturePolyline::tryMatchWithFeature(FeatureMatchPolyline2D
 //    }
 }
 
-void ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches,
-                                                           const LandmarkPolyline2DPtr& _lmk_ptr,
-                                                           const FeaturePolyline2DPtr& _feat_ptr,
-                                                           const Eigen::Matrix3s& _T_feature_landmark_prior) const
+void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches,
+                                                             const LandmarkPolyline2DPtr& _lmk_ptr,
+                                                             const FeaturePolyline2DPtr& _feat_ptr,
+                                                             const Eigen::Matrix3s& _T_feature_landmark_prior) const
 {
     //WOLF_DEBUG("PTFP ", getName(), "::tryMatchWithLandmark: ");
 
@@ -827,8 +891,22 @@ void ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(LandmarkMatchPolyline
         lmk_match->feature_from_id_ = best_match->from_B_id_;
         lmk_match->feature_to_id_   = best_match->to_B_id_;
 
-        // incoming last transformation
+        // feature landmark transformation
         lmk_match->T_feature_landmark_ = best_match->T_A_B_.inverse().eval();
+
+        bool from_defined = (lmk_match->landmark_from_id_ != _lmk_ptr->getFirstId()    || _lmk_ptr->isFirstDefined()) &&
+                            (lmk_match->feature_from_id_  != 0                         || _feat_ptr->isFirstDefined());
+        bool to_defined   = (lmk_match->landmark_to_id_   != _lmk_ptr->getLastId()     || _lmk_ptr->isLastDefined())  &&
+                            (lmk_match->feature_to_id_    != _feat_ptr->getNPoints()-1 || _feat_ptr->isLastDefined());
+        int N_points = lmk_match->feature_to_id_ - lmk_match->feature_from_id_;
+        int N_def_points = N_points - (from_defined ? 0:1) - (to_defined ? 0:1);
+        if (N_def_points > 0 && N_points > 2)
+            lmk_match->T_feature_landmark_ = pose2T2D(computeRigidTrans(_feat_ptr->getPoints().middleCols(lmk_match->feature_from_id_,N_points),
+                                                                        _lmk_ptr->computePointsGlobal().middleCols(lmk_match->landmark_from_id_,N_points),
+                                                                        from_defined,
+                                                                        to_defined));
+        else
+            lmk_match->T_feature_landmark_ = _T_feature_landmark_prior;
         // score
         lmk_match->normalized_score_ = best_match->normalized_score_;
 
@@ -838,6 +916,7 @@ void ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(LandmarkMatchPolyline
         // store in list
         lmk_matches[lmk_match->normalized_score_]= lmk_match;
         WOLF_DEBUG("match stored!");
+        assert(lmk_match->check() && "tryMatchWithLandmark: wrong match");
     }
 
 //    // compute best rigid transformation matching
@@ -884,8 +963,11 @@ void ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(LandmarkMatchPolyline
 //    }
 }
 
-bool ProcessorTrackerFeaturePolyline::updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr) const
+bool ProcessorTrackerFeaturePolyline2D::updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match,
+                                                                LandmarkPolyline2DPtr& _lmk_ptr,
+                                                                FeaturePolyline2DPtr& _feat_ptr) const
 {
+    assert(_lmk_match->check() && "updateMatchWithLandmark: input _lmk_match wrong");
     WOLF_DEBUG("PTFP ", getName(), "::updateMatchWithLandmark: ");
     assert(_lmk_ptr->id() == _lmk_match->landmark_ptr_->id());
 
@@ -905,34 +987,88 @@ bool ProcessorTrackerFeaturePolyline::updateMatchWithLandmark(LandmarkMatchPolyl
     }
 
     // valid match: update landmark match of last
+    assert(_lmk_match->check() && "updateMatchWithLandmark: wrongly updated");
     _lmk_match = new_lmk_matches.begin()->second; // the frist is the one with smallest difference from movement of the old match
     return true;
 }
 
-LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline::concatenateFeatureLandmarkMatches(FeaturePolyline2DPtr pl_incoming,
-                                                                                              FeatureMatchPolyline2DPtr ftr_match,
-                                                                                              LandmarkMatchPolyline2DPtr lmk_match_last) const
+LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline2D::concatenateFeatureLandmarkMatches(FeaturePolyline2DPtr pl_incoming,
+                                                                                                FeatureMatchPolyline2DPtr ftr_match,
+                                                                                                LandmarkMatchPolyline2DPtr lmk_match_last) const
 {
-    // Add the match to landmark_match_map
+    assert(lmk_match_last->check() && "input lmk_match_last wrong");
+    WOLF_DEBUG("PTFP ", getName(), "::concatenateFeatureLandmarkMatches: ");
+    std::cout << "ftr_match:";
+    std::cout << "\n\tincoming_from:    " << ftr_match->feature_incoming_from_id_;
+    std::cout << "\n\tincoming_to:      " << ftr_match->feature_incoming_to_id_;
+    std::cout << "\n\tlast_from:        " << ftr_match->feature_last_from_id_;
+    std::cout << "\n\tlast_to:          " << ftr_match->feature_last_to_id_;
+    std::cout << "\nlmk_match_last:";
+    std::cout << "\n\tlast_from:        " << lmk_match_last->feature_from_id_;
+    std::cout << "\n\tlast_to:          " << lmk_match_last->feature_to_id_;
+    std::cout << "\n\tlmk_from:         " << lmk_match_last->landmark_from_id_;
+    std::cout << "\n\tlmk_to:           " << lmk_match_last->landmark_to_id_;
+
     auto lmk_match_incoming = std::make_shared<LandmarkMatchPolyline2D>();
+    auto lmk_ptr = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match_last->landmark_ptr_);
 
+    // feature correspondence from&to
     lmk_match_incoming->feature_from_id_  = ftr_match->feature_incoming_from_id_;
     lmk_match_incoming->feature_to_id_    = ftr_match->feature_incoming_to_id_;
+
+    // landmark correspondence from&to
     // Not adding new points nor checks (done in correctFeatureDrift)
-    lmk_match_incoming->landmark_from_id_ = lmk_match_last->landmark_from_id_ + ftr_match->feature_last_from_id_ - lmk_match_last->feature_from_id_;
-    lmk_match_incoming->landmark_to_id_   = lmk_match_last->landmark_to_id_   + ftr_match->feature_last_to_id_   - lmk_match_last->feature_to_id_;
-    lmk_match_incoming->landmark_ptr_     = lmk_match_last->landmark_ptr_;
-    lmk_match_incoming->landmark_version_ = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match_last->landmark_ptr_)->getVersion();
+    int from_offset = ftr_match->feature_last_from_id_ - lmk_match_last->feature_from_id_;
+    int to_offset   = ftr_match->feature_last_to_id_   - lmk_match_last->feature_to_id_;
+    std::cout << "\nfrom_offset:         " << from_offset;
+    std::cout << "\nto_offset:           " << from_offset;
+    lmk_match_incoming->landmark_from_id_ = lmk_match_last->landmark_from_id_;
+    lmk_match_incoming->landmark_to_id_   = lmk_match_last->landmark_to_id_;
+    while (from_offset > 0)
+    {
+        if (lmk_match_incoming->landmark_from_id_ == lmk_match_incoming->landmark_to_id_ && !lmk_ptr->isClosed())
+            return nullptr;
+        lmk_match_incoming->landmark_from_id_ = lmk_ptr->getNextValidId(lmk_match_incoming->landmark_from_id_);
+        from_offset--;
+    }
+    while (from_offset < 0)
+    {
+        lmk_match_incoming->feature_from_id_++;
+        from_offset++;
+    }
+    while (to_offset < 0)
+    {
+        if (lmk_match_incoming->landmark_from_id_ == lmk_match_incoming->landmark_to_id_ && !lmk_ptr->isClosed())
+            return nullptr;
+        lmk_match_incoming->landmark_to_id_ = lmk_ptr->getPrevValidId(lmk_match_incoming->landmark_to_id_);
+        to_offset++;
+    }
+    while (to_offset > 0)
+    {
+        lmk_match_incoming->feature_to_id_--;
+        to_offset--;
+    }
+    std::cout << "\nlmk_match_incoming:";
+    std::cout << "\n\tincoming_from:    " << lmk_match_incoming->feature_from_id_;
+    std::cout << "\n\tincoming_to:      " << lmk_match_incoming->feature_to_id_;
+    std::cout << "\n\tlmk_from:         " << lmk_match_incoming->landmark_from_id_;
+    std::cout << "\n\tlmk_to:           " << lmk_match_incoming->landmark_to_id_ << std::endl;
+
+    // other match attributes
+    lmk_match_incoming->landmark_ptr_     = lmk_ptr;
+    lmk_match_incoming->landmark_version_ = lmk_ptr->getVersion();
     lmk_match_incoming->normalized_score_ = ftr_match->normalized_score_;
     lmk_match_incoming->T_feature_landmark_ = ftr_match->T_incoming_last_ * lmk_match_last->T_feature_landmark_;
 
+    assert(lmk_match_incoming->check() && "lmk_match_incoming wrongly concatenated");
+
     return lmk_match_incoming;
 }
 
 // MATH ///////////////////////////////////////////////////////////////////////////////////////////////////////////
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
-void ProcessorTrackerFeaturePolyline::computeTransformations()
+void ProcessorTrackerFeaturePolyline2D::computeTransformations()
 {
     //WOLF_DEBUG("ProcessorTrackerFeaturePolyline::computeTransformations: ");
     Eigen::Vector3s vehicle_pose_incoming = getProblem()->getState(incoming_ptr_->getTimeStamp());
@@ -996,12 +1132,12 @@ void ProcessorTrackerFeaturePolyline::computeTransformations()
 
 }
 
-ProcessorBasePtr ProcessorTrackerFeaturePolyline::create(const std::string& _unique_name,
-                                                         const ProcessorParamsBasePtr _params,
-                                                         const SensorBasePtr _sensor_ptr)
+ProcessorBasePtr ProcessorTrackerFeaturePolyline2D::create(const std::string& _unique_name,
+                                                           const ProcessorParamsBasePtr _params,
+                                                           const SensorBasePtr _sensor_ptr)
 {
-    auto params = std::static_pointer_cast<ProcessorParamsTrackerFeaturePolyline>(_params);
-    auto prc_ptr = std::make_shared<ProcessorTrackerFeaturePolyline>(params);
+    auto params = std::static_pointer_cast<ProcessorParamsTrackerFeaturePolyline2D>(_params);
+    auto prc_ptr = std::make_shared<ProcessorTrackerFeaturePolyline2D>(params);
     prc_ptr->setName(_unique_name);
     prc_ptr->configure(_sensor_ptr);
     return prc_ptr;
@@ -1012,5 +1148,5 @@ ProcessorBasePtr ProcessorTrackerFeaturePolyline::create(const std::string& _uni
 // Register in the SensorFactory
 #include "base/processor/processor_factory.h"
 namespace wolf {
-WOLF_REGISTER_PROCESSOR("TRACKER FEATURE POLYLINE", ProcessorTrackerFeaturePolyline)
+WOLF_REGISTER_PROCESSOR("TRACKER FEATURE POLYLINE", ProcessorTrackerFeaturePolyline2D)
 } /* namespace wolf */
diff --git a/test/gtest_polyline_2D.cpp b/test/gtest_polyline_2D.cpp
index d8c50e392..05d2f706f 100644
--- a/test/gtest_polyline_2D.cpp
+++ b/test/gtest_polyline_2D.cpp
@@ -12,7 +12,7 @@ TEST(Polyline2DTest, Transformations)
     ASSERT_MATRIX_APPROX(Vector3s::Zero(), T2pose2D(Matrix3s::Identity()), 1e-12);
 }
 
-TEST(Polyline2DTest, RigidTransformation)
+TEST(Polyline2DTest, RigidTransformation8points)
 {
     // Random points around random position
     MatrixXs points_last = MatrixXs::Random(3,8);
@@ -21,7 +21,7 @@ TEST(Polyline2DTest, RigidTransformation)
     points_last = T_random * points_last;
 
     // For different orientation changes between last and incomming
-    for (Scalar theta_last_incoming = -M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI)
+    for (Scalar theta_last_incoming = -0.9*M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI)
     {
         // movement from last to incoming (random translation)
         Matrix2s R_last_incoming = Rotation2D<Scalar>(theta_last_incoming).matrix();
@@ -39,12 +39,167 @@ TEST(Polyline2DTest, RigidTransformation)
         Vector3s v = computeRigidTrans(points_incoming, points_last);
 
         ASSERT_MATRIX_APPROX(v.head(2), t_incoming_last, 1e-12);
-        ASSERT_DOUBLE_EQ(v(2), theta_incoming_last);
+        ASSERT_NEAR(v(2), theta_incoming_last, 1e-12);
     }
 
 //  PRINTF("All good at TestTest::DummyTestExample !\n");
 }
 
+TEST(Polyline2DTest, RigidTransformation2points2defined)
+{
+    // Random points around random position
+    MatrixXs points_last = MatrixXs::Random(3,2);
+    points_last.bottomRows(1) = MatrixXs::Ones(1,2);
+    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
+    for (Scalar theta_last_incoming = -0.9*M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI)
+    {
+        // movement from last to incoming (random translation)
+        Matrix2s R_last_incoming = Rotation2D<Scalar>(theta_last_incoming).matrix();
+        Vector2s t_last_incoming(Vector2s::Random());
+
+        // inverse transformation
+        Scalar theta_incoming_last = -theta_last_incoming;
+        Vector2s t_incoming_last = -R_last_incoming.transpose() * t_last_incoming;
+        Matrix3s T_incoming_last = pose2T2D(t_incoming_last, theta_incoming_last);
+
+        // rotate points
+        MatrixXs points_incoming = T_incoming_last * points_last;
+
+        // compute rigit transformation
+        Vector3s v_incoming_last_computed = computeRigidTrans(points_incoming, points_last);
+        Matrix3s T_incoming_last_computed = pose2T2D(v_incoming_last_computed);
+
+        ASSERT_MATRIX_APPROX(T_incoming_last_computed*points_last.rightCols(2), points_incoming.rightCols(2), 1e-12);
+        ASSERT_MATRIX_APPROX(v_incoming_last_computed.head(2), t_incoming_last, 1e-12);
+        ASSERT_NEAR(v_incoming_last_computed(2), theta_incoming_last, 1e-12);
+    }
+
+//  PRINTF("All good at TestTest::DummyTestExample !\n");
+}
+
+TEST(Polyline2DTest, RigidTransformation3points2defined)
+{
+    // Random points around random position
+    MatrixXs points_last = MatrixXs::Random(3,3);
+    points_last.bottomRows(1) = MatrixXs::Ones(1,3);
+    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
+    for (Scalar theta_last_incoming = -0.9*M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI)
+    {
+        // movement from last to incoming (random translation)
+        Matrix2s R_last_incoming = Rotation2D<Scalar>(theta_last_incoming).matrix();
+        Vector2s t_last_incoming(Vector2s::Random());
+
+        // inverse transformation
+        Scalar theta_incoming_last = -theta_last_incoming;
+        Vector2s t_incoming_last = -R_last_incoming.transpose() * t_last_incoming;
+        Matrix3s T_incoming_last = pose2T2D(t_incoming_last, theta_incoming_last);
+
+        // rotate points
+        MatrixXs points_incoming = T_incoming_last * points_last;
+
+        // change not defined point (first)
+        points_incoming.col(0) = points_incoming.col(0) + (rand() % 1000 - 500)*0.001 * (points_incoming.col(0) - points_incoming.col(1));
+        points_incoming(2,0) = 1;
+
+        // compute rigit transformation
+        Vector3s v_incoming_last_computed = computeRigidTrans(points_incoming, points_last, false, true);
+        Matrix3s T_incoming_last_computed = pose2T2D(v_incoming_last_computed);
+
+        ASSERT_MATRIX_APPROX(T_incoming_last_computed*points_last.rightCols(2), points_incoming.rightCols(2), 1e-12);
+        ASSERT_MATRIX_APPROX(v_incoming_last_computed.head(2), t_incoming_last, 1e-12);
+        ASSERT_NEAR(v_incoming_last_computed(2), theta_incoming_last, 1e-12);
+    }
+
+//  PRINTF("All good at TestTest::DummyTestExample !\n");
+}
+
+TEST(Polyline2DTest, RigidTransformation3points1defined)
+{
+    // Random points around random position
+    MatrixXs points_last = MatrixXs::Random(3,3);
+    points_last.bottomRows(1) = MatrixXs::Ones(1,3);
+    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
+    for (Scalar theta_last_incoming = -0.9*M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI)
+    {
+        // movement from last to incoming (random translation)
+        Matrix2s R_last_incoming = Rotation2D<Scalar>(theta_last_incoming).matrix();
+        Vector2s t_last_incoming(Vector2s::Random());
+
+        // inverse transformation
+        Scalar theta_incoming_last = -theta_last_incoming;
+        Vector2s t_incoming_last = -R_last_incoming.transpose() * t_last_incoming;
+        Matrix3s T_incoming_last = pose2T2D(t_incoming_last, theta_incoming_last);
+
+        // rotate points
+        MatrixXs points_incoming = T_incoming_last * points_last;
+
+        // change 2 not defined points
+        points_incoming.col(0) = points_incoming.col(0) + (rand() % 1000 - 500)*0.001 * (points_incoming.col(1) - points_incoming.col(0));
+        points_incoming(2,0) = 1;
+        points_incoming.col(2) = points_incoming.col(2) + (rand() % 1000 - 500)*0.001 * (points_incoming.col(2) - points_incoming.col(1));
+        points_incoming(2,2) = 1;
+
+        // compute rigit transformation
+        Vector3s v_incoming_last_computed = computeRigidTrans(points_incoming, points_last, false, false);
+        Matrix3s T_incoming_last_computed = pose2T2D(v_incoming_last_computed);
+
+        ASSERT_MATRIX_APPROX(T_incoming_last_computed*points_last.col(1), points_incoming.col(1), 1e-12);
+        ASSERT_MATRIX_APPROX(v_incoming_last_computed.head(2), t_incoming_last, 1e-12);
+        ASSERT_NEAR(v_incoming_last_computed(2), theta_incoming_last, 1e-12);
+    }
+
+//  PRINTF("All good at TestTest::DummyTestExample !\n");
+}
+
+TEST(Polyline2DTest, RigidTransformation2points1defined)
+{
+    // Random points around random position
+    MatrixXs points_last = MatrixXs::Random(3,2);
+    points_last.bottomRows(1) = MatrixXs::Ones(1,2);
+    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
+    for (Scalar theta_last_incoming = -0.9*M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI)
+    {
+        // movement from last to incoming (random translation)
+        Matrix2s R_last_incoming = Rotation2D<Scalar>(theta_last_incoming).matrix();
+        Vector2s t_last_incoming(Vector2s::Random());
+
+        // inverse transformation
+        Scalar theta_incoming_last = -theta_last_incoming;
+        Vector2s t_incoming_last = -R_last_incoming.transpose() * t_last_incoming;
+        Matrix3s T_incoming_last = pose2T2D(t_incoming_last, theta_incoming_last);
+
+        // rotate points
+        MatrixXs points_incoming = T_incoming_last * points_last;
+
+        // change not defined point
+        points_incoming.col(0) = points_incoming.col(0) + (rand() % 1000 - 500)*0.001 * (points_incoming.col(0) - points_incoming.col(1));
+        points_incoming(2,0) = 1;
+
+        // compute rigit transformation
+        Vector3s v_incoming_last_computed = computeRigidTrans(points_incoming, points_last, false, true);
+        Matrix3s T_incoming_last_computed = pose2T2D(v_incoming_last_computed);
+
+        ASSERT_MATRIX_APPROX(T_incoming_last_computed*points_last.col(1), points_incoming.col(1), 1e-12);
+        ASSERT_MATRIX_APPROX(v_incoming_last_computed .head(2), t_incoming_last, 1e-12);
+        ASSERT_NEAR(v_incoming_last_computed(2), theta_incoming_last, 1e-12);
+    }
+
+//  PRINTF("All good at TestTest::DummyTestExample !\n");
+}
+
+
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
diff --git a/test/gtest_processor_tracker_feature_polyline_2D.cpp b/test/gtest_processor_tracker_feature_polyline_2D.cpp
new file mode 100644
index 000000000..2c3580aed
--- /dev/null
+++ b/test/gtest_processor_tracker_feature_polyline_2D.cpp
@@ -0,0 +1,1183 @@
+/**
+ * \file gtest_processor_tracker_feature_polyline_2D.cpp
+ *
+ *  Created on: Apr 3, 2019
+ *      \author: jvallve
+ */
+
+#include "base/processor/processor_tracker_feature_polyline_2D.h"
+#include "base/wolf.h"
+
+#include "utils_gtest.h"
+#include "base/logging.h"
+
+#include "base/ceres_wrapper/ceres_manager.h"
+
+#include <cmath>
+#include <iostream>
+
+using namespace Eigen;
+
+WOLF_PTR_TYPEDEFS(ProcessorPolylinePublicMethods);
+class ProcessorPolylinePublicMethods : public ProcessorTrackerFeaturePolyline2D
+{
+    public:
+        ProcessorPolylinePublicMethods(ProcessorParamsTrackerFeaturePolyline2DPtr _params) :
+            ProcessorTrackerFeaturePolyline2D(_params)
+        {
+
+        }
+
+        unsigned int trackFeatures(const FeatureBaseList& _features_last_in,
+                                   FeatureBaseList& _features_incoming_out,
+                                   FeatureMatchMap& _feature_correspondences)
+        {
+            return ProcessorTrackerFeaturePolyline2D::trackFeatures(_features_last_in,_features_incoming_out,_feature_correspondences);
+        }
+
+        bool correctFeatureDrift(const FeatureBasePtr _origin_feature,
+                                 const FeatureBasePtr _last_feature,
+                                 FeatureBasePtr _incoming_feature)
+        {
+            return ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(_origin_feature,
+                                                                          _last_feature,
+                                                                          _incoming_feature);
+        }
+
+        bool voteForKeyFrame()
+        {
+            return ProcessorTrackerFeaturePolyline2D::voteForKeyFrame();
+        }
+
+        unsigned int processNew(const unsigned int& _max_features)
+        {
+            return ProcessorTrackerFeaturePolyline2D::processNew(_max_features);
+        }
+
+        unsigned int detectNewFeatures(const unsigned int& _max_new_features,
+                                       FeatureBaseList& _features_last_out)
+        {
+            return ProcessorTrackerFeaturePolyline2D::detectNewFeatures(_max_new_features, _features_last_out);
+        }
+
+        void establishConstraints()
+        {
+            ProcessorTrackerFeaturePolyline2D::establishConstraints();
+        }
+
+        void emplaceConstraintPointToLine(FeaturePolyline2DPtr _polyline_feature,
+                                          LandmarkPolyline2DPtr _polyline_landmark,
+                                          const int& _ftr_point_id,
+                                          const int& _lmk_point_id,
+                                          const int& _lmk_prev_point_id)
+        {
+            ProcessorTrackerFeaturePolyline2D::emplaceConstraintPointToLine(_polyline_feature,
+                                                                            _polyline_landmark,
+                                                                            _ftr_point_id,
+                                                                            _lmk_point_id,
+                                                                            _lmk_prev_point_id);
+        }
+
+        void emplaceConstraintPoint(FeaturePolyline2DPtr _polyline_feature,
+                                    LandmarkPolyline2DPtr _polyline_landmark,
+                                    const int& _ftr_point_id,
+                                    const int& _lmk_point_id)
+        {
+            ProcessorTrackerFeaturePolyline2D::emplaceConstraintPoint(_polyline_feature,
+                                                                      _polyline_landmark,
+                                                                      _ftr_point_id,
+                                                                      _lmk_point_id);
+        }
+
+        LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr)
+        {
+            return ProcessorTrackerFeaturePolyline2D::createLandmark(_feature_ptr);
+        }
+
+        bool modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match, FeaturePolyline2DPtr& pl_ftr)
+        {
+            return ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch( lmk_match, pl_ftr);
+        }
+
+        void tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches,
+                                 const FeaturePolyline2DPtr& _ftr_last,
+                                 const FeaturePolyline2DPtr& _ftr_incoming,
+                                 const Eigen::Matrix3s& _T_last_incoming_prior) const
+        {
+            ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature( ftr_matches,
+                                                                   _ftr_last,
+                                                                   _ftr_incoming,
+                                                                   _T_last_incoming_prior);
+        }
+
+        void tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches,
+                                  const LandmarkPolyline2DPtr& _lmk_ptr,
+                                  const FeaturePolyline2DPtr& _feat_ptr,
+                                  const Eigen::Matrix3s& _T_feature_landmark_prior) const
+        {
+            ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(lmk_matches,
+                                                                    _lmk_ptr,
+                                                                    _feat_ptr,
+                                                                    _T_feature_landmark_prior);
+        }
+
+        bool updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr) const
+        {
+            return ProcessorTrackerFeaturePolyline2D::updateMatchWithLandmark(_lmk_match, _lmk_ptr, _feat_ptr);
+        }
+
+        void computeTransformations()
+        {
+            ProcessorTrackerFeaturePolyline2D::computeTransformations();
+        }
+
+        LandmarkMatchPolyline2DPtr concatenateFeatureLandmarkMatches(FeaturePolyline2DPtr pl_incoming,
+                                                                     FeatureMatchPolyline2DPtr ftr_match_incoming_last,
+                                                                     LandmarkMatchPolyline2DPtr lmk_match_last) const
+        {
+            return ProcessorTrackerFeaturePolyline2D::concatenateFeatureLandmarkMatches(pl_incoming,
+                                                                                        ftr_match_incoming_last,
+                                                                                        lmk_match_last);
+        }
+};
+
+class ProcessorPolyline2Dt : public testing::Test
+{
+    public: //These can be accessed in fixtures
+        ProcessorPolylinePublicMethodsPtr processor_pl;
+        wolf::SensorBasePtr sensor_ptr;
+        wolf::TimeStamp t;
+        wolf::Scalar mti_clock, tmp;
+        wolf::Scalar dt;
+        Vector6s data;
+        Matrix6s data_cov;
+        VectorXs x0;
+        std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr;
+
+    //a new of this will be created for each new test
+    virtual void SetUp()
+    {
+        using namespace wolf;
+        using namespace Eigen;
+        using std::shared_ptr;
+        using std::make_shared;
+        using std::static_pointer_cast;
+        using namespace wolf::Constants;
+
+        std::string wolf_root = _WOLF_ROOT_DIR;
+
+        // Wolf problem
+        problem = Problem::create("POV 3D");
+        Vector7s extrinsics = (Vector7s() << 0,0,0, 0,0,0,1).finished();
+        sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics,  wolf_root + "/src/examples/sensor_imu.yaml");
+        ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml");
+
+        // Time and data variables
+        data = Vector6s::Zero();
+        data_cov = Matrix6s::Identity();
+
+        // Set the origin
+        x0.resize(10);
+
+        // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
+        cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero());
+    }
+
+    virtual void TearDown()
+    {
+        // code here will be called just after the test completes
+        // ok to through exceptions from here if need be
+        /*
+            You can do deallocation of resources in TearDown or the destructor routine. 
+                However, if you want exception handling you must do it only in the TearDown code because throwing an exception 
+                from the destructor results in undefined behavior.
+            The Google assertion macros may throw exceptions in platforms where they are enabled in future releases. 
+                Therefore, it's a good idea to use assertion macros in the TearDown code for better maintenance.
+        */
+    }
+};
+
+TEST(ProcessorIMU_constructors, ALL)
+{
+    using namespace wolf;
+
+    //constructor with ProcessorIMUParamsPtr argument only
+    ProcessorParamsIMUPtr param_ptr = std::make_shared<ProcessorParamsIMU>();
+    param_ptr->max_time_span =   2.0;
+    param_ptr->max_buff_length = 20000;
+    param_ptr->dist_traveled =   2.0;
+    param_ptr->angle_turned =    2.0;
+
+    ProcessorIMUPtr prc1 = std::make_shared<ProcessorIMU>(param_ptr);
+    ASSERT_EQ(prc1->getMaxTimeSpan(), param_ptr->max_time_span);
+    ASSERT_EQ(prc1->getMaxBuffLength(), param_ptr->max_buff_length);
+    ASSERT_EQ(prc1->getDistTraveled(), param_ptr->dist_traveled);
+    ASSERT_EQ(prc1->getAngleTurned(), param_ptr->angle_turned);
+
+    //Factory constructor without yaml
+    std::string wolf_root = _WOLF_ROOT_DIR;
+    ProblemPtr problem = Problem::create("POV 3D");
+    Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished();
+    SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
+    ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
+    ProcessorParamsIMU params_default;
+    ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(),   params_default.max_time_span);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), params_default.max_buff_length);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(),  params_default.dist_traveled);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(),   params_default.angle_turned);
+
+    //Factory constructor with yaml
+    processor_ptr = problem->installProcessor("IMU", "Sec IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml");
+    ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(),   2.0);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), 20000);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(),  2.0);
+    ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(),   0.2);
+}
+
+TEST(ProcessorIMU, voteForKeyFrame)
+{
+    using namespace wolf;
+    using namespace Eigen;
+    using std::shared_ptr;
+    using std::make_shared;
+    using std::static_pointer_cast;
+    using namespace wolf::Constants;
+
+    std::string wolf_root = _WOLF_ROOT_DIR;
+
+    // Wolf problem
+    ProblemPtr problem = Problem::create("POV 3D");
+    Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished();
+    SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics,  wolf_root + "/src/examples/sensor_imu.yaml");
+    ProcessorParamsIMUPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
+    prc_imu_params->max_time_span = 1;
+    prc_imu_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass
+    prc_imu_params->dist_traveled = 1000000000;
+    prc_imu_params->angle_turned = 1000000000;
+    prc_imu_params->voting_active = true;
+    ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", sensor_ptr, prc_imu_params);
+    
+    //setting origin
+    VectorXs x0(10);
+    TimeStamp t(0);
+    x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    MatrixXs P0(9,9); P0.setIdentity();
+    problem->setPrior(x0, P0, t, 0.01);
+
+    //data variable and covariance matrix
+    // since we integrate only a few times, give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
+    Vector6s data;
+    data << 1,0,0, 0,0,0;
+    Matrix6s data_cov(Matrix6s::Identity());
+    data_cov(0,0) = 0.5;
+
+    // Create the captureIMU to store the IMU data arriving from (sensor / callback / file / etc.)
+    std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero());
+
+    //  Time  
+    // we want more than one data to integrate otherwise covariance will be 0
+    Scalar dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() - 0.1;
+    t.set(dt);
+    cap_imu_ptr->setTimeStamp(t);
+    sensor_ptr->process(cap_imu_ptr);
+
+    dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() + 0.1;
+    t.set(dt);
+    cap_imu_ptr->setTimeStamp(t);
+    sensor_ptr->process(cap_imu_ptr);
+
+    /*There should be 3 frames :
+        - 1 KeyFrame at origin
+        - 1 keyframe created by process() in voteForKeyFrame() since conditions to create a keyframe are met
+        - 1 frame that would be used by future data
+    */
+    ASSERT_EQ(problem->getTrajectoryPtr()->getFrameList().size(),(unsigned int) 3);
+
+    /* if max_time_span == 2,  Wolf tree should be
+
+    Hardware
+        S1
+            pm5
+            o: C2 - F2
+            l: C4 - F3
+        Trajectory
+        KF1
+            Estim, ts=0,	 x = ( 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0)
+            C1 -> S1
+        KF2
+            Estim, ts=2.1,	 x = ( 2.2     0       -22     0       0       0       1       2.1     0       -21     0       0       0       0       0       0      )
+            C2 -> S1
+                f1
+                    m = ( 2.21 0   0   0   0   0   1   2.1 0   0  )
+                    c1 --> F1
+        F3
+            Estim, ts=2.1,	 x = ( . . .)
+            C4 -> S1
+    */
+    //TODO : ASSERT TESTS to make sure the constraints are correctly set + check the tree above
+
+}
+
+//replace TEST by TEST_F if SetUp() needed
+TEST_F(ProcessorIMUt, interpolate)
+{
+    using namespace wolf;
+
+    t.set(0);
+    x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    MatrixXs P0(9,9); P0.setIdentity();
+    problem->setPrior(x0, P0, t, 0.01);
+
+    data << 2, 0, 0, 0, 0, 0; // only acc_x
+    cap_imu_ptr->setData(data);
+
+    // make one step to depart from origin
+    cap_imu_ptr->setTimeStamp(0.05);
+    sensor_ptr->process(cap_imu_ptr);
+    Motion mot_ref = problem->getProcessorMotionPtr()->getMotion();
+
+    // make two steps, then pretend it's just one
+    cap_imu_ptr->setTimeStamp(0.10);
+    sensor_ptr->process(cap_imu_ptr);
+    Motion mot_int_gt = problem->getProcessorMotionPtr()->getMotion();
+
+    cap_imu_ptr->setTimeStamp(0.15);
+    sensor_ptr->process(cap_imu_ptr);
+    Motion mot_final = problem->getProcessorMotionPtr()->getMotion();
+    mot_final.delta_ = mot_final.delta_integr_;
+    Motion mot_sec = mot_final;
+
+//    problem->getProcessorMotionPtr()->getBuffer().print(0,1,1,0);
+
+class P : public wolf::ProcessorIMU
+{
+    public:
+        P() : ProcessorIMU(std::make_shared<ProcessorParamsIMU>()) {}
+        Motion interp(const Motion& ref, Motion& sec, TimeStamp ts)
+        {
+            return ProcessorIMU::interpolate(ref, sec, ts);
+        }
+} imu;
+
+Motion mot_int = imu.interp(mot_ref, mot_sec, TimeStamp(0.10));
+
+ASSERT_MATRIX_APPROX(mot_int.data_,  mot_int_gt.data_, 1e-6);
+//ASSERT_MATRIX_APPROX(mot_int.delta_, mot_int_gt.delta_, 1e-6); // FIXME: delta_p not correctly interpolated
+ASSERT_MATRIX_APPROX(mot_final.delta_integr_,  mot_sec.delta_integr_, 1e-6);
+
+}
+
+TEST_F(ProcessorIMUt, acc_x)
+{
+    t.set(0); // clock in 0,1 ms ticks
+    x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    MatrixXs P0(9,9); P0.setIdentity();
+    problem->setPrior(x0, P0, t, 0.01);
+
+    data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
+
+    cap_imu_ptr->setData(data);
+    cap_imu_ptr->setTimeStamp(0.1);
+    sensor_ptr->process(cap_imu_ptr);
+
+    // Expected state after one integration
+    VectorXs x(10);
+    x << 0.01,0,0, 0,0,0,1, 0.2,0,0; // advanced at a=2m/s2 during 0.1s ==> dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2
+    Vector6s b; b << 0,0,0, 0,0,0;
+
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
+}
+
+TEST_F(ProcessorIMUt, acc_y)
+{
+    // last part of this test fails with precision wolf::Constants::EPS_SMALL beacause error is in 1e-12
+    // difference hier is that we integrate over 1ms periods
+
+    t.set(0); // clock in 0,1 ms ticks
+    x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    MatrixXs P0(9,9); P0.setIdentity();
+    problem->setPrior(x0, P0, t, 0.01);
+
+    data << 0, 20, 9.806, 0, 0, 0; // only acc_y, but measure gravity!
+
+    cap_imu_ptr->setData(data);
+    cap_imu_ptr->setTimeStamp(0.001);
+    sensor_ptr->process(cap_imu_ptr);
+
+    // Expected state after one integration
+    VectorXs x(10);
+    x << 0,0.00001,0, 0,0,0,1, 0,0.02,0; // advanced at a=20m/s2 during 0.001s ==> dx = 0.5*20*0.001^2 = 0.00001; dvx = 20*0.001 = 0.02
+    Vector6s b; b<< 0,0,0, 0,0,0;
+
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
+
+    //do so for 5s
+    const unsigned int iter = 1000; //how many ms
+    for(unsigned int i = 1; i < iter; i++) //already did one integration above
+    {
+        cap_imu_ptr->setTimeStamp(i*0.001 + 0.001); //first one will be 0.002 and last will be 5.000
+        sensor_ptr->process(cap_imu_ptr);
+    }
+
+    // advanced at a=20m/s2 during 1s ==> dx = 0.5*20*1^2 = 10; dvx = 20*1 = 20
+    x << 0,10,0, 0,0,0,1, 0,20,0;
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS);
+}
+
+TEST_F(ProcessorIMUt, acc_z)
+{
+    t.set(0); // clock in 0,1 ms ticks
+    x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    MatrixXs P0(9,9); P0.setIdentity();
+    problem->setPrior(x0, P0, t, 0.01);
+
+    data << 0, 0, 9.806 + 2.0, 0, 0, 0; // only acc_z, but measure gravity!
+
+    cap_imu_ptr->setData(data);
+    cap_imu_ptr->setTimeStamp(0.1);
+    sensor_ptr->process(cap_imu_ptr);
+
+    // Expected state after one integration
+    VectorXs x(10);
+    x << 0,0,0.01, 0,0,0,1, 0,0,0.2; // advanced at a=2m/s2 during 0.1s ==> dz = 0.5*2*0.1^2 = 0.01; dvz = 2*0.1 = 0.2
+    Vector6s b; b<< 0,0,0, 0,0,0;
+
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
+    ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
+
+    //do so for 5s
+    const unsigned int iter = 50; //how 0.1s 
+    for(unsigned int i = 1; i < iter; i++) //already did one integration above
+    {
+        cap_imu_ptr->setTimeStamp(i*0.1 + 0.1); //first one will be 0.2 and last will be 5.0
+        sensor_ptr->process(cap_imu_ptr);
+    }
+
+    // advanced at a=2m/s2 during 5s ==> dz = 0.5*2*5^2 = 25; dvz = 2*5 = 10
+    x << 0,0,25, 0,0,0,1, 0,0,10;
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS);
+    ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS);
+}
+
+TEST_F(ProcessorIMUt, check_Covariance)
+{
+    t.set(0); // clock in 0,1 ms ticks
+    x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    MatrixXs P0(9,9); P0.setIdentity();
+    problem->setPrior(x0, P0, t, 0.01);
+
+    data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
+
+    cap_imu_ptr->setData(data);
+    cap_imu_ptr->setTimeStamp(0.1);
+    sensor_ptr->process(cap_imu_ptr);
+
+    VectorXs delta_preint(problem->getProcessorMotionPtr()->getMotion().delta_integr_);
+//    Matrix<wolf::Scalar,9,9> delta_preint_cov = problem->getProcessorMotionPtr()->getCurrentDeltaPreintCov();
+
+    ASSERT_FALSE(delta_preint.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL));
+//    ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXs::Zero(9,9), wolf::Constants::EPS_SMALL); // << "delta_preint_cov :\n" << delta_preint_cov; //covariances computed only at keyframe creation
+}
+
+TEST_F(ProcessorIMUt, gyro_x)
+{
+    wolf::Scalar dt(0.001);
+    t.set(0); // clock in 0,1 ms ticks
+    x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    MatrixXs P0(9,9); P0.setIdentity();
+    problem->setPrior(x0, P0, t, 0.01);
+
+    wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+    data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
+
+    cap_imu_ptr->setData(data);
+    cap_imu_ptr->setTimeStamp(0.001);
+    sensor_ptr->process(cap_imu_ptr);
+
+    // Expected state after one integration
+    Quaternions quat_comp(Quaternions::Identity());
+    quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+    VectorXs x(10);
+    x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
+
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
+
+    //do so for 5s
+    const unsigned int iter = 1000; //how many ms 
+    for(unsigned int i = 1; i < iter; i++) //already did one integration above
+    {
+        // quaternion composition
+        quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+        Quaternions rot(problem->getCurrentState().data()+3);
+        data.head(3) =  rot.conjugate() * (- wolf::gravity());
+
+        cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
+        cap_imu_ptr->setData(data);
+        sensor_ptr->process(cap_imu_ptr);
+    }
+
+    x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+}
+
+TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
+{
+    // time
+    wolf::Scalar dt(0.001);
+    t.set(0); // clock in 0,1 ms ticks
+
+    // bias
+    wolf::Scalar abx = 0.002;
+    Vector6s bias; bias << abx,0,0,  0,0,0;
+    Vector3s acc_bias = bias.head(3);
+    // state
+    x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    MatrixXs P0(9,9); P0.setIdentity();
+
+    // init things
+    problem->setPrior(x0, P0, t, 0.01);
+
+    problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
+    problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
+//    WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose());
+
+    // data
+    wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+    data << acc_bias - wolf::gravity(), rate_of_turn, 0, 0; // measure gravity
+
+    cap_imu_ptr->setData(data);
+    cap_imu_ptr->setTimeStamp(0.001);
+    sensor_ptr->process(cap_imu_ptr);
+
+    // Expected state after one integration
+    Quaternions quat_comp(Quaternions::Identity());
+    quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+    VectorXs x_true(10);
+    x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
+
+    VectorXs x_est(10);
+    x_est = problem->getCurrentState().head(10);
+
+    ASSERT_MATRIX_APPROX(x_est.transpose() , x_true.transpose(), wolf::Constants::EPS);
+
+    //do so for 5s
+    const unsigned int iter = 1000; //how many ms 
+    for(unsigned int i = 1; i < iter; i++) //already did one integration above
+    {
+        // quaternion composition
+        quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+        Quaternions rot(problem->getCurrentState().data()+3);
+        data.head(3) =  rot.conjugate() * (- wolf::gravity()) + acc_bias;
+
+        cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
+        cap_imu_ptr->setData(data);
+        sensor_ptr->process(cap_imu_ptr);
+    }
+
+    x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x_true, wolf::Constants::EPS);
+
+}
+
+TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
+{
+    wolf::Scalar dt(0.001);
+    t.set(0); // clock in 0,1 ms ticks
+    wolf::Scalar abx(0.002), aby(0.01);
+    x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    MatrixXs P0(9,9); P0.setIdentity();
+    problem->setPrior(x0, P0, t, 0.01);
+
+    Vector6s bias; bias << abx,aby,0,  0,0,0;
+    Vector3s acc_bias = bias.head(3);
+
+    problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
+    problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
+
+    wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+//    data << 0+abx, 0+aby, 9.806, rate_of_turn, rate_of_turn*1.5, 0; // measure gravity
+    data << acc_bias - wolf::gravity(), rate_of_turn*1.5, 0, 0; // measure gravity
+
+    cap_imu_ptr->setData(data);
+    cap_imu_ptr->setTimeStamp(0.001);
+    sensor_ptr->process(cap_imu_ptr);
+
+    // Expected state after one integration
+    Quaternions quat_comp(Quaternions::Identity());
+    quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+    VectorXs x(10);
+    x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;//, abx,aby,0, 0,0,0;
+
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);// << "expected state : " << problem->getCurrentState().transpose()
+//    << "\n estimated state : " << x.transpose();
+
+    //do so for 5s
+    const unsigned int iter = 1000; //how many ms 
+    for(unsigned int i = 1; i < iter; i++) //already did one integration above
+    {
+        // quaternion composition
+        quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+        Quaternions rot(problem->getCurrentState().data()+3);
+        data.head(3) =  rot.conjugate() * (- wolf::gravity()) + acc_bias;
+
+        cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
+        cap_imu_ptr->setData(data);
+        sensor_ptr->process(cap_imu_ptr);
+    }
+
+    x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);// << "estimated state is : \n" << problem->getCurrentState().transpose() <<
+//    "\n expected state is : \n" << x.transpose() << std::endl;
+}
+
+TEST_F(ProcessorIMUt, gyro_z)
+{
+    wolf::Scalar dt(0.001);
+    t.set(0); // clock in 0,1 ms ticks
+    x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    MatrixXs P0(9,9); P0.setIdentity();
+    problem->setPrior(x0, P0, t, 0.01);
+
+    wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+    data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
+
+    cap_imu_ptr->setData(data);
+    cap_imu_ptr->setTimeStamp(0.001);
+    sensor_ptr->process(cap_imu_ptr);
+
+    // Expected state after one integration
+    Quaternions quat_comp(Quaternions::Identity());
+    quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+    VectorXs x(10);
+    x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
+
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
+
+    //do so for 5s
+    const unsigned int iter = 1000; //how many ms 
+    for(unsigned int i = 1; i < iter; i++) //already did one integration above
+    {
+        // quaternion composition
+        quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+        cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
+        sensor_ptr->process(cap_imu_ptr);
+    }
+
+    x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+}
+
+TEST_F(ProcessorIMUt, gyro_xyz)
+{
+    t.set(0); // clock in 0,1 ms ticks
+    x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    MatrixXs P0(9,9); P0.setIdentity();
+    problem->setPrior(x0, P0, t, 0.01);
+
+    Vector3s tmp_vec; //will be used to store rate of turn data
+    Quaternions quat_comp(Quaternions::Identity());
+    Matrix3s R0(Matrix3s::Identity());
+    wolf::Scalar time = 0;
+    const unsigned int x_rot_vel = 5;
+    const unsigned int y_rot_vel = 6;
+    const unsigned int z_rot_vel = 13;
+
+    wolf::Scalar tmpx, tmpy, tmpz;
+    
+    /*
+        ox oy oz evolution in degrees (for understanding) --> converted in rad
+        with * pi/180
+        ox = pi*sin(alpha*t*pi/180); %express angle in rad before using sinus
+        oy = pi*sin(beta*t*pi/180);
+        oz = pi*sin(gamma*t*pi/180);
+
+        corresponding rate of turn
+        %rate of turn expressed in radians/s
+        wx = pi*alpha*cos(alpha*t*pi/180)*pi/180;
+        wy = pi*beta*cos(beta*t*pi/180)*pi/180;
+        wz = pi*gamma*cos(gamma*t*pi/180)*pi/180;
+     */
+
+     const wolf::Scalar dt = 0.001;
+
+    for(unsigned int data_iter = 0; data_iter <= 1000; data_iter ++)
+    {   
+        time += dt;
+
+        tmpx = M_PI*x_rot_vel*cos((M_PI/180) * x_rot_vel * time)*M_PI/180;
+        tmpy = M_PI*y_rot_vel*cos((M_PI/180) * y_rot_vel * time)*M_PI/180;
+        tmpz = M_PI*z_rot_vel*cos((M_PI/180) * z_rot_vel * time)*M_PI/180;
+        tmp_vec << tmpx, tmpy, tmpz;
+
+        // quaternion composition
+        quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
+        R0 = R0 * wolf::v2R(tmp_vec*dt);
+        // use processorIMU
+        Quaternions rot(problem->getCurrentState().data()+3);
+        data.head(3) =  rot.conjugate() * (- wolf::gravity()); //gravity measured
+        data.tail(3) = tmp_vec;
+
+        cap_imu_ptr->setData(data);
+        cap_imu_ptr->setTimeStamp(time);
+        sensor_ptr->process(cap_imu_ptr);
+    }
+
+    /* We focus on orientation here. position is supposed not to have moved
+     * we integrated using 2 ways : 
+        - a direct quaternion composition q = q (x) q(w*dt)
+        - using methods implemented in processorIMU
+
+        We check one against the other
+     */
+
+     // validating that the quaternion composition and rotation matrix composition actually describe the same rotation.
+    Quaternions R2quat(wolf::v2q(wolf::R2v(R0)));
+    Vector4s quat_comp_vec((Vector4s() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
+    Vector4s R2quat_vec((Vector4s() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() );
+
+    ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, wolf::Constants::EPS);// << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl;
+
+    VectorXs x(10);
+    x << 0,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0,0,0;
+
+    Quaternions result_quat(problem->getCurrentState().data() + 3);
+    //std::cout << "final orientation : " << wolf::q2v(result_quat).transpose() << std::endl;
+
+    //check position part
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(3) , x.head(3), wolf::Constants::EPS);
+
+    //check orientation part
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(3,4) , x.segment(3,4) , wolf::Constants::EPS);
+
+    //check velocity and bias parts
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(7,3) , x.segment(7,3), wolf::Constants::EPS);
+}
+
+TEST_F(ProcessorIMUt, gyro_z_ConstVelocity)
+{
+    wolf::Scalar dt(0.001);
+    t.set(0); // clock in 0,1 ms ticks
+    x0 << 0,0,0,  0,0,0,1,  2,0,0;
+    MatrixXs P0(9,9); P0.setIdentity();
+    problem->setPrior(x0, P0, t, 0.01);
+
+    wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+    data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
+
+    cap_imu_ptr->setData(data);
+    cap_imu_ptr->setTimeStamp(0.001);
+    sensor_ptr->process(cap_imu_ptr);
+
+    // Expected state after one integration
+    Quaternions quat_comp(Quaternions::Identity());
+    quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+    VectorXs x(10);
+    x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002m
+
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
+
+    //do so for 1s
+    const unsigned int iter = 1000; //how many ms 
+    for(unsigned int i = 1; i < iter; i++) //already did one integration above
+    {
+        // quaternion composition
+        quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+        cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 5.000
+        sensor_ptr->process(cap_imu_ptr);
+    }
+
+    x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+}
+
+TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
+{
+    wolf::Scalar dt(0.001);
+    t.set(0); // clock in 0,1 ms ticks
+    x0 << 0,0,0,  0,0,0,1,  2,0,0;
+    MatrixXs P0(9,9); P0.setIdentity();
+    problem->setPrior(x0, P0, t, 0.01);
+
+    wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+    data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
+
+    cap_imu_ptr->setData(data);
+    cap_imu_ptr->setTimeStamp(0.001);
+    sensor_ptr->process(cap_imu_ptr);
+
+    // Expected state after one integration
+    Quaternions quat_comp(Quaternions::Identity());
+    quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+    VectorXs x(10);
+    // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
+    x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
+
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
+
+    //do so for 1s
+    const unsigned int iter = 1000; //how many ms 
+    for(unsigned int i = 1; i < iter; i++) //already did one integration above
+    {
+        // quaternion composition
+        quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+        Quaternions rot(problem->getCurrentState().data()+3);
+        data.head(3) =  rot.conjugate() * (- wolf::gravity());
+
+        cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
+        cap_imu_ptr->setData(data);
+        sensor_ptr->process(cap_imu_ptr);
+    }
+
+    x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+}
+
+TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
+{
+    wolf::Scalar dt(0.001);
+    t.set(0); // clock in 0,1 ms ticks
+    x0 << 0,0,0,  0,0,0,1,  2,0,0;
+    MatrixXs P0(9,9); P0.setIdentity();
+    problem->setPrior(x0, P0, t, 0.01);
+
+    wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+    data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
+
+    cap_imu_ptr->setData(data);
+    cap_imu_ptr->setTimeStamp(0.001);
+    sensor_ptr->process(cap_imu_ptr);
+
+    // Expected state after one integration
+    Quaternions quat_comp(Quaternions::Identity());
+    quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+    VectorXs x(10);
+    // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
+    x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
+
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
+
+    //do so for 1s
+    const unsigned int iter = 1000; //how many ms 
+    for(unsigned int i = 1; i < iter; i++) //already did one integration above
+    {
+        // quaternion composition
+        quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+        Quaternions rot(problem->getCurrentState().data()+3);
+        data.head(3) =  rot.conjugate() * (- wolf::gravity());
+
+        cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
+        cap_imu_ptr->setData(data);
+        sensor_ptr->process(cap_imu_ptr);
+    }
+
+    x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+}
+
+TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
+{
+    wolf::Scalar dt(0.001);
+    t.set(0); // clock in 0,1 ms ticks
+    x0 << 0,0,0,  0,0,0,1,  2,0,0;
+    MatrixXs P0(9,9); P0.setIdentity();
+    problem->setPrior(x0, P0, t, 0.01);
+
+    wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+    data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
+
+    cap_imu_ptr->setData(data);
+    cap_imu_ptr->setTimeStamp(0.001);
+    sensor_ptr->process(cap_imu_ptr);
+
+    // Expected state after one integration
+    Quaternions quat_comp(Quaternions::Identity());
+    quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+    VectorXs x(10);
+    // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
+    x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
+
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
+
+    //do so for 1s
+    const unsigned int iter = 1000; //how many ms 
+    for(unsigned int i = 1; i < iter; i++) //already did one integration above
+    {
+        // quaternion composition
+        quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+        Quaternions rot(problem->getCurrentState().data()+3);
+        data.head(3) =  rot.conjugate() * (- wolf::gravity());
+
+        cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
+        cap_imu_ptr->setData(data);
+        sensor_ptr->process(cap_imu_ptr);
+    }
+
+    x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+}
+
+TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
+{
+    t.set(0); // clock in 0,1 ms ticks
+    x0 << 0,0,0,  0,0,0,1,  2,0,0;
+    MatrixXs P0(9,9); P0.setIdentity();
+    problem->setPrior(x0, P0, t, 0.01);
+
+    Vector3s tmp_vec; //will be used to store rate of turn data
+    Quaternions quat_comp(Quaternions::Identity());
+    Matrix3s R0(Matrix3s::Identity());
+    wolf::Scalar time = 0;
+    const unsigned int x_rot_vel = 5;
+    const unsigned int y_rot_vel = 6;
+    const unsigned int z_rot_vel = 13;
+
+    wolf::Scalar tmpx, tmpy, tmpz;
+    
+    /*
+        ox oy oz evolution in degrees (for understanding) --> converted in rad
+        with * pi/180
+        ox = pi*sin(alpha*t*pi/180); %express angle in rad before using sinus
+        oy = pi*sin(beta*t*pi/180);
+        oz = pi*sin(gamma*t*pi/180);
+
+        corresponding rate of turn
+        %rate of turn expressed in radians/s
+        wx = pi*alpha*cos(alpha*t*pi/180)*pi/180;
+        wy = pi*beta*cos(beta*t*pi/180)*pi/180;
+        wz = pi*gamma*cos(gamma*t*pi/180)*pi/180;
+     */
+
+     const wolf::Scalar dt = 0.001;
+
+    for(unsigned int data_iter = 0; data_iter < 1000; data_iter ++)
+    {   
+        time += dt;
+
+        tmpx = M_PI*x_rot_vel*cos((M_PI/180) * x_rot_vel * time)*M_PI/180;
+        tmpy = M_PI*y_rot_vel*cos((M_PI/180) * y_rot_vel * time)*M_PI/180;
+        tmpz = M_PI*z_rot_vel*cos((M_PI/180) * z_rot_vel * time)*M_PI/180;
+        tmp_vec << tmpx, tmpy, tmpz;
+
+        // quaternion composition
+        quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
+        R0 = R0 * wolf::v2R(tmp_vec*dt);
+        // use processorIMU
+        Quaternions rot(problem->getCurrentState().data()+3);
+        data.head(3) =  rot.conjugate() * (- wolf::gravity()); //gravity measured
+        data.tail(3) = tmp_vec;
+
+        cap_imu_ptr->setData(data);
+        cap_imu_ptr->setTimeStamp(time);
+        sensor_ptr->process(cap_imu_ptr);
+    }
+
+    /* We focus on orientation here. position is supposed not to have moved
+     * we integrated using 2 ways : 
+        - a direct quaternion composition q = q (x) q(w*dt)
+        - using methods implemented in processorIMU
+
+        We check one against the other
+     */
+
+     // validating that the quaternion composition and rotation matrix composition actually describe the same rotation.
+    Quaternions R2quat(wolf::v2q(wolf::R2v(R0)));
+    Vector4s quat_comp_vec((Vector4s() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
+    Vector4s R2quat_vec((Vector4s() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() );
+
+    ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, wolf::Constants::EPS); // << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl;
+
+    VectorXs x(10);
+    //rotation + translation due to initial velocity
+    x << 2,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 2,0,0;
+
+    Quaternions result_quat(problem->getCurrentState().data() + 3);
+    //std::cout << "final orientation : " << wolf::q2v(result_quat).transpose() << std::endl;
+
+    //check position part
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(3) , x.head(3), wolf::Constants::EPS);
+
+    //check orientation part
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(3,4) , x.segment(3,4) , wolf::Constants::EPS);
+
+    //check velocity
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(7,3) , x.segment(7,3), wolf::Constants::EPS);
+
+}
+
+TEST_F(ProcessorIMUt, gyro_x_acc_x)
+{
+    wolf::Scalar dt(0.001);
+    t.set(0); // clock in 0,1 ms ticks
+    x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    MatrixXs P0(9,9); P0.setIdentity();
+    problem->setPrior(x0, P0, t, 0.01);
+
+    wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+    data << 1, 0, -wolf::gravity()(2), rate_of_turn, 0, 0; // measure gravity
+
+    cap_imu_ptr->setData(data);
+    cap_imu_ptr->setTimeStamp(0.001);
+    sensor_ptr->process(cap_imu_ptr);
+
+    // Expected state after one integration
+    Quaternions quat_comp(Quaternions::Identity());
+    quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+    VectorXs x(10);
+    // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on x axis
+    // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on x axis
+    // v = a*dt = 0.001
+    x << 0.0000005,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0.001,0,0;
+
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); // << "1. current state is : \n" << problem->getCurrentState().transpose() <<
+//    "\n current x is : \n" << x.transpose() << std::endl;
+
+    //do so for 1s
+    const unsigned int iter = 1000; //how many ms 
+    for(unsigned int i = 2; i <= iter; i++) //already did one integration above
+    {
+        // quaternion composition
+        quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+        Quaternions rot(problem->getCurrentState().data()+3);
+        data.head(3) =  rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<1,0,0).finished();
+
+        cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
+        cap_imu_ptr->setData(data);
+        sensor_ptr->process(cap_imu_ptr);
+    }
+
+    // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on x axis
+    // v = a*dt = 1
+    x << 0.5,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 1,0,0;
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+}
+
+TEST_F(ProcessorIMUt, gyro_y_acc_y)
+{
+    wolf::Scalar dt(0.001);
+    t.set(0); // clock in 0,1 ms ticks
+    x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    MatrixXs P0(9,9); P0.setIdentity();
+    problem->setPrior(x0, P0, t, 0.01);
+
+    wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+    data << 0, 1, -wolf::gravity()(2), 0, rate_of_turn, 0; // measure gravity
+
+    cap_imu_ptr->setData(data);
+    cap_imu_ptr->setTimeStamp(0.001);
+    sensor_ptr->process(cap_imu_ptr);
+
+    // Expected state after one integration
+    Quaternions quat_comp(Quaternions::Identity());
+    quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+    VectorXs x(10);
+    // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on y axis
+    // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on y axis
+    // v = a*dt = 0.001
+    x << 0,0.0000005,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0.001,0;
+
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); // << "1. current state is : \n" << problem->getCurrentState().transpose() <<
+//    "\n current x is : \n" << x.transpose() << std::endl;
+
+    //do so for 1s
+    const unsigned int iter = 1000; //how many ms 
+    for(unsigned int i = 2; i <= iter; i++) //already did one integration above
+    {
+        // quaternion composition
+        quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+        Quaternions rot(problem->getCurrentState().data()+3);
+        data.head(3) =  rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<0,1,0).finished();
+
+        cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
+        cap_imu_ptr->setData(data);
+        sensor_ptr->process(cap_imu_ptr);
+    }
+
+    // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on y axis
+    // v = a*dt = 1
+    x << 0,0.5,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,1,0;
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+}
+
+TEST_F(ProcessorIMUt, gyro_z_acc_z)
+{
+    wolf::Scalar dt(0.001);
+    t.set(0); // clock in 0,1 ms ticks
+    x0 << 0,0,0,  0,0,0,1,  0,0,0;
+    MatrixXs P0(9,9); P0.setIdentity();
+    problem->setPrior(x0, P0, t, 0.01);
+
+    wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+    data << 0, 0, -wolf::gravity()(2) + 1, 0, 0, rate_of_turn; // measure gravity
+
+    cap_imu_ptr->setData(data);
+    cap_imu_ptr->setTimeStamp(0.001);
+    sensor_ptr->process(cap_imu_ptr);
+
+    // Expected state after one integration
+    Quaternions quat_comp(Quaternions::Identity());
+    quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+    VectorXs x(10);
+    // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on z axis
+    // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on z axis
+    // v = a*dt = 0.001
+    x << 0,0,0.0000005, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0.001;
+
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+
+    //do so for 1s
+    const unsigned int iter = 1000; //how many ms 
+    for(unsigned int i = 2; i <= iter; i++) //already did one integration above
+    {
+        // quaternion composition
+        quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
+
+        Quaternions rot(problem->getCurrentState().data()+3);
+        data.head(3) =  rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<0,0,1).finished();
+
+        cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
+        cap_imu_ptr->setData(data);
+        sensor_ptr->process(cap_imu_ptr);
+    }
+
+    // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on z axis
+    // v = a*dt = 1
+    x << 0,0,0.5, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,1;
+    ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  //::testing::GTEST_FLAG(filter) = "ProcessorIMUt.check_Covariance";
+  return RUN_ALL_TESTS();
+}
+
-- 
GitLab