diff --git a/CMakeLists.txt b/CMakeLists.txt
index 20472dda112ee4ec7d6883858ddb0d6b981f657d..a5b1e21c2f3510bd97ad176c181cfa3d6d171264 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -581,7 +581,6 @@ IF (laser_scan_utils_FOUND)
   SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
     include/base/processor/processor_tracker_feature_corner.h
     include/base/processor/processor_tracker_landmark_corner.h
-    include/base/processor/processor_tracker_landmark_polyline.h
     )
   SET(HDRS_SENSOR ${HDRS_SENSOR}
     include/base/sensor/sensor_laser_2D.h
@@ -590,7 +589,6 @@ IF (laser_scan_utils_FOUND)
     src/sensor/sensor_laser_2D.cpp
     src/processor/processor_tracker_feature_corner.cpp
     src/processor/processor_tracker_landmark_corner.cpp
-    src/processor/processor_tracker_landmark_polyline.cpp
     )
 ENDIF(laser_scan_utils_FOUND)
 
diff --git a/include/base/capture/capture_base.h b/include/base/capture/capture_base.h
index 7643f243ea4b8a05e0acdad82eb4454a8e87a6c4..0082d9d314fa28cf9f26a64b0cb455ab07102244 100644
--- a/include/base/capture/capture_base.h
+++ b/include/base/capture/capture_base.h
@@ -57,7 +57,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
         void setTimeStampToNow();
 
         FrameBasePtr getFrame() const;
-        void setFramePtr(const FrameBasePtr _frm_ptr);
+        void setFrame(const FrameBasePtr _frm_ptr);
         void unlinkFromFrame(){frame_ptr_.reset();}
 
         virtual void setProblem(ProblemPtr _problem) final;
@@ -69,7 +69,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
         void getFactorList(FactorBasePtrList& _fac_list);
 
         SensorBasePtr getSensor() const;
-        virtual void setSensorPtr(const SensorBasePtr sensor_ptr);
+        virtual void setSensor(const SensorBasePtr sensor_ptr);
 
         // constrained by
         virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
@@ -79,8 +79,8 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
         // State blocks
         const std::vector<StateBlockPtr>& getStateBlockVec() const;
         std::vector<StateBlockPtr>& getStateBlockVec();
-        StateBlockPtr getStateBlockPtr(unsigned int _i) const;
-        void setStateBlockPtr(unsigned int _i, const StateBlockPtr _sb_ptr);
+        StateBlockPtr getStateBlock(unsigned int _i) const;
+        void setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr);
 
         StateBlockPtr getSensorP() const;
         StateBlockPtr getSensorO() const;
@@ -143,24 +143,24 @@ inline std::vector<StateBlockPtr>& CaptureBase::getStateBlockVec()
     return state_block_vec_;
 }
 
-inline void CaptureBase::setStateBlockPtr(unsigned int _i, const StateBlockPtr _sb_ptr)
+inline void CaptureBase::setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr)
 {
     state_block_vec_[_i] = _sb_ptr;
 }
 
 inline StateBlockPtr CaptureBase::getSensorP() const
 {
-    return getStateBlockPtr(0);
+    return getStateBlock(0);
 }
 
 inline StateBlockPtr CaptureBase::getSensorO() const
 {
-    return getStateBlockPtr(1);
+    return getStateBlock(1);
 }
 
 inline StateBlockPtr CaptureBase::getSensorIntrinsic() const
 {
-    return getStateBlockPtr(2);
+    return getStateBlock(2);
 }
 
 inline unsigned int CaptureBase::id()
@@ -173,7 +173,7 @@ inline FrameBasePtr CaptureBase::getFrame() const
     return frame_ptr_.lock();
 }
 
-inline void CaptureBase::setFramePtr(const FrameBasePtr _frm_ptr)
+inline void CaptureBase::setFrame(const FrameBasePtr _frm_ptr)
 {
     frame_ptr_ = _frm_ptr;
 }
@@ -203,7 +203,7 @@ inline SensorBasePtr CaptureBase::getSensor() const
     return sensor_ptr_.lock();
 }
 
-inline void CaptureBase::setSensorPtr(const SensorBasePtr sensor_ptr)
+inline void CaptureBase::setSensor(const SensorBasePtr sensor_ptr)
 {
   sensor_ptr_ = sensor_ptr;
 }
diff --git a/include/base/capture/capture_laser_2D.h b/include/base/capture/capture_laser_2D.h
index 9abd6bf4b874a8970d71929355aed095e52c3838..96a434547b5e802376472f472be0aeb0e27af940 100644
--- a/include/base/capture/capture_laser_2D.h
+++ b/include/base/capture/capture_laser_2D.h
@@ -28,7 +28,7 @@ class CaptureLaser2D : public CaptureBase
 
         laserscanutils::LaserScan& getScan();
 
-        void setSensorPtr(const SensorBasePtr sensor_ptr);
+        void setSensor(const SensorBasePtr sensor_ptr);
 
     private:
         SensorLaser2DPtr laser_ptr_; //specific pointer to sensor laser 2D object
@@ -41,9 +41,9 @@ inline laserscanutils::LaserScan& CaptureLaser2D::getScan()
     return scan_;
 }
 
-inline void CaptureLaser2D::setSensorPtr(const SensorBasePtr sensor_ptr)
+inline void CaptureLaser2D::setSensor(const SensorBasePtr sensor_ptr)
 {
-  CaptureBase::setSensorPtr(sensor_ptr);
+  CaptureBase::setSensor(sensor_ptr);
   laser_ptr_ = std::static_pointer_cast<SensorLaser2D>(sensor_ptr);
 }
 
diff --git a/include/base/capture/capture_motion.h b/include/base/capture/capture_motion.h
index ab932da224bd610c130f6bc9f108e8d746c75f7e..ed71171e99ef6d221777f1ab89a7e0ba4a8dc0d7 100644
--- a/include/base/capture/capture_motion.h
+++ b/include/base/capture/capture_motion.h
@@ -97,7 +97,7 @@ class CaptureMotion : public CaptureBase
 
         // Origin frame
         FrameBasePtr getOriginFrame();
-        void setOriginFramePtr(FrameBasePtr _frame_ptr);
+        void setOriginFrame(FrameBasePtr _frame_ptr);
 
         // member data:
     private:
@@ -161,7 +161,7 @@ inline FrameBasePtr CaptureMotion::getOriginFrame()
     return origin_frame_ptr_.lock();
 }
 
-inline void CaptureMotion::setOriginFramePtr(FrameBasePtr _frame_ptr)
+inline void CaptureMotion::setOriginFrame(FrameBasePtr _frame_ptr)
 {
     origin_frame_ptr_ = _frame_ptr;
 }
diff --git a/include/base/capture/capture_wheel_joint_position.h b/include/base/capture/capture_wheel_joint_position.h
index 0bade4029f499ba78b6cc71a7f260f05df44a79e..a90cdb205178dd4c36cd302a7793e7e74c668df1 100644
--- a/include/base/capture/capture_wheel_joint_position.h
+++ b/include/base/capture/capture_wheel_joint_position.h
@@ -155,7 +155,7 @@ protected:
 
 //  ~CaptureWheelJointPosition() = default;
 
-////  void setSensorPtr(const SensorBasePtr sensor_ptr) override;
+////  void setSensor(const SensorBasePtr sensor_ptr) override;
 
 //  std::size_t getNumWheels() const noexcept
 //  {
diff --git a/include/base/factor/factor_autodiff_distance_3D.h b/include/base/factor/factor_autodiff_distance_3D.h
index 0eb1fc7570494b4731656034c9fc56025516c9af..77eb08e2ae049978c7f2978eee3106fcec66ba9c 100644
--- a/include/base/factor/factor_autodiff_distance_3D.h
+++ b/include/base/factor/factor_autodiff_distance_3D.h
@@ -34,7 +34,7 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D,
                             _feat->getFrame()->getP(),
                             _frm_other->getP())
         {
-            setFeaturePtr(_feat);
+            setFeature(_feat);
         }
 
         virtual ~FactorAutodiffDistance3D() { /* nothing */ }
diff --git a/include/base/factor/factor_autodiff_trifocal.h b/include/base/factor/factor_autodiff_trifocal.h
index b7e56e0ac17b15d857f05522f42f911de3668291..d7c2091fb2c547b1511d8f5985e66723a24fb247 100644
--- a/include/base/factor/factor_autodiff_trifocal.h
+++ b/include/base/factor/factor_autodiff_trifocal.h
@@ -152,7 +152,7 @@ FactorAutodiffTrifocal::FactorAutodiffTrifocal(const FeatureBasePtr& _feature_pr
         camera_ptr_(std::static_pointer_cast<SensorCamera>(_processor_ptr->getSensor())),
         sqrt_information_upper(Matrix3s::Zero())
 {
-    setFeaturePtr(_feature_last_ptr);
+    setFeature(_feature_last_ptr);
     Matrix3s K_inv           = camera_ptr_->getIntrinsicMatrix().inverse();
     pixel_canonical_prev_    = K_inv * Vector3s(_feature_prev_ptr  ->getMeasurement(0), _feature_prev_ptr  ->getMeasurement(1), 1.0);
     pixel_canonical_origin_  = K_inv * Vector3s(_feature_origin_ptr->getMeasurement(0), _feature_origin_ptr->getMeasurement(1), 1.0);
diff --git a/include/base/factor/factor_base.h b/include/base/factor/factor_base.h
index ab5e362e8efb4d50e198a324770aaf19d9518f7c..4562d5f75b39995fc6578b8498fc5ee95e223e61 100644
--- a/include/base/factor/factor_base.h
+++ b/include/base/factor/factor_base.h
@@ -113,7 +113,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
         /** \brief Returns a pointer to the feature constrained from
          **/
         FeatureBasePtr getFeature() const;
-        void setFeaturePtr(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
+        void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
 
         /** \brief Returns a pointer to its capture
          **/
@@ -142,22 +142,22 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
         /** \brief Returns a pointer to the frame constrained to
          **/
         FrameBasePtr getFrameOther() const       { return frame_other_ptr_.lock(); }
-        void setFrameOtherPtr(FrameBasePtr _frm_o)  { frame_other_ptr_ = _frm_o; }
+        void setFrameOther(FrameBasePtr _frm_o)  { frame_other_ptr_ = _frm_o; }
 
         /** \brief Returns a pointer to the frame constrained to
          **/
         CaptureBasePtr getCaptureOther() const       { return capture_other_ptr_.lock(); }
-        void setCaptureOtherPtr(CaptureBasePtr _cap_o)  { capture_other_ptr_ = _cap_o; }
+        void setCaptureOther(CaptureBasePtr _cap_o)  { capture_other_ptr_ = _cap_o; }
 
         /** \brief Returns a pointer to the feature constrained to
          **/
         FeatureBasePtr getFeatureOther() const       { return feature_other_ptr_.lock(); }
-        void setFeatureOtherPtr(FeatureBasePtr _ftr_o)  { feature_other_ptr_ = _ftr_o; }
+        void setFeatureOther(FeatureBasePtr _ftr_o)  { feature_other_ptr_ = _ftr_o; }
 
         /** \brief Returns a pointer to the landmark constrained to
          **/
         LandmarkBasePtr getLandmarkOther() const     { return landmark_other_ptr_.lock(); }
-        void setLandmarkOtherPtr(LandmarkBasePtr _lmk_o){ landmark_other_ptr_ = _lmk_o; }
+        void setLandmarkOther(LandmarkBasePtr _lmk_o){ landmark_other_ptr_ = _lmk_o; }
 
         /**
          * @brief getProcessor
diff --git a/include/base/factor/factor_point_2D.h b/include/base/factor/factor_point_2D.h
index 2034260de9483f592e54de2a1e3f125a12c19879..4d9686e3805c9fb5d337ed033e90a904cbb4ae53 100644
--- a/include/base/factor/factor_point_2D.h
+++ b/include/base/factor/factor_point_2D.h
@@ -30,11 +30,11 @@ class FactorPoint2D: public FactorAutodiff<FactorPoint2D, 2,2,1,2,1,2>
                       const ProcessorBasePtr& _processor_ptr,
                       unsigned int _ftr_point_id, int _lmk_point_id, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
         FactorAutodiff<FactorPoint2D,2,2,1,2,1,2>("POINT 2D",
-                nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id)),
-        feature_point_id_(_ftr_point_id), landmark_point_id_(_lmk_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
+                nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlock(_lmk_point_id)),
+        feature_point_id_(_ftr_point_id), landmark_point_id_(_lmk_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
     {
         //std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl;
-        //std::cout << "landmark state block " << _lmk_ptr->getPointStateBlockPtr(_lmk_point_id)->getVector().transpose() << std::endl;
+        //std::cout << "landmark state block " << _lmk_ptr->getPointStateBlock(_lmk_point_id)->getVector().transpose() << std::endl;
         Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
         Eigen::MatrixXs measurement_sqrt_covariance = lltOfA.matrixU();
         measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U  in the decomposition
diff --git a/include/base/factor/factor_point_to_line_2D.h b/include/base/factor/factor_point_to_line_2D.h
index e60c8d3c7ef2677637cb06d86a832064fdaa44ce..d4b891f3ee5d055d3e7500464e9ce59b1780f9f9 100644
--- a/include/base/factor/factor_point_to_line_2D.h
+++ b/include/base/factor/factor_point_to_line_2D.h
@@ -31,8 +31,8 @@ class FactorPointToLine2D: public FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,
                             unsigned int _ftr_point_id, int _lmk_point_id,  int _lmk_point_aux_id,
                             bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
         FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,2,2>("POINT TO LINE 2D",
-                nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id), _lmk_ptr->getPointStateBlockPtr(_lmk_point_aux_id)),
-        landmark_point_id_(_lmk_point_id), landmark_point_aux_id_(_lmk_point_aux_id), feature_point_id_(_ftr_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_id)), point_aux_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_aux_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
+                nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlock(_lmk_point_id), _lmk_ptr->getPointStateBlock(_lmk_point_aux_id)),
+        landmark_point_id_(_lmk_point_id), landmark_point_aux_id_(_lmk_point_aux_id), feature_point_id_(_ftr_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)), point_aux_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_aux_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
     {
         //std::cout << "FactorPointToLine2D" << std::endl;
         Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
diff --git a/include/base/feature/feature_base.h b/include/base/feature/feature_base.h
index df014203d7f5bb530f192347b4f19136c822de0b..3732bc55247d9d39b90992a07afe7bb04d7f7681 100644
--- a/include/base/feature/feature_base.h
+++ b/include/base/feature/feature_base.h
@@ -76,7 +76,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
         FrameBasePtr getFrame() const;
 
         CaptureBasePtr getCapture() const;
-        void setCapturePtr(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
+        void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
 
         FactorBasePtr addFactor(FactorBasePtr _co_ptr);
         FactorBasePtrList& getFactorList();
diff --git a/include/base/frame_base.h b/include/base/frame_base.h
index e6e4c14db28b5e5c897d851773b1f2ff4534ec84..be97eb80a333fac1391f7b19bf12d1895e9dbb6d 100644
--- a/include/base/frame_base.h
+++ b/include/base/frame_base.h
@@ -86,17 +86,17 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
         const std::vector<StateBlockPtr>& getStateBlockVec() const;
         std::vector<StateBlockPtr>& getStateBlockVec();
     protected:
-        StateBlockPtr getStateBlockPtr(unsigned int _i) const;
-        void setStateBlockPtr(unsigned int _i, const StateBlockPtr _sb_ptr);
+        StateBlockPtr getStateBlock(unsigned int _i) const;
+        void setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr);
         void resizeStateBlockVec(unsigned int _size);
 
     public:
         StateBlockPtr getP() const;
         StateBlockPtr getO() const;
         StateBlockPtr getV() const;
-        void setPPtr(const StateBlockPtr _p_ptr);
-        void setOPtr(const StateBlockPtr _o_ptr);
-        void setVPtr(const StateBlockPtr _v_ptr);
+        void setP(const StateBlockPtr _p_ptr);
+        void setO(const StateBlockPtr _o_ptr);
+        void setV(const StateBlockPtr _v_ptr);
         void registerNewStateBlocks();
         void removeStateBlocks();
 
@@ -120,7 +120,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
         virtual void setProblem(ProblemPtr _problem) final;
 
         TrajectoryBasePtr getTrajectory() const;
-        void setTrajectoryPtr(TrajectoryBasePtr _trj_ptr);
+        void setTrajectory(TrajectoryBasePtr _trj_ptr);
 
         FrameBasePtr getPreviousFrame() const;
         FrameBasePtr getNextFrame() const;
@@ -197,7 +197,7 @@ inline StateBlockPtr FrameBase::getP() const
 {
     return state_block_vec_[0];
 }
-inline void FrameBase::setPPtr(const StateBlockPtr _p_ptr)
+inline void FrameBase::setP(const StateBlockPtr _p_ptr)
 {
     state_block_vec_[0] = _p_ptr;
 }
@@ -206,7 +206,7 @@ inline StateBlockPtr FrameBase::getO() const
 {
     return state_block_vec_[1];
 }
-inline void FrameBase::setOPtr(const StateBlockPtr _o_ptr)
+inline void FrameBase::setO(const StateBlockPtr _o_ptr)
 {
     state_block_vec_[1] = _o_ptr;
 }
@@ -216,18 +216,18 @@ inline StateBlockPtr FrameBase::getV() const
     return state_block_vec_[2];
 }
 
-inline void FrameBase::setVPtr(const StateBlockPtr _v_ptr)
+inline void FrameBase::setV(const StateBlockPtr _v_ptr)
 {
     state_block_vec_[2] = _v_ptr;
 }
 
-inline StateBlockPtr FrameBase::getStateBlockPtr(unsigned int _i) const
+inline StateBlockPtr FrameBase::getStateBlock(unsigned int _i) const
 {
     assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
     return state_block_vec_[_i];
 }
 
-inline void FrameBase::setStateBlockPtr(unsigned int _i, const StateBlockPtr _sb_ptr)
+inline void FrameBase::setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr)
 {
     assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
     state_block_vec_[_i] = _sb_ptr;
@@ -266,7 +266,7 @@ inline FactorBasePtrList& FrameBase::getConstrainedByList()
     return constrained_by_list_;
 }
 
-inline void FrameBase::setTrajectoryPtr(TrajectoryBasePtr _trj_ptr)
+inline void FrameBase::setTrajectory(TrajectoryBasePtr _trj_ptr)
 {
     trajectory_ptr_ = _trj_ptr;
 }
diff --git a/include/base/landmark/landmark_base.h b/include/base/landmark/landmark_base.h
index f12da14aabfd3dd33c03be8f2bc22fd5de6c2ced..9019ee85efd04f54ddfa920e98413c8aabf3846e 100644
--- a/include/base/landmark/landmark_base.h
+++ b/include/base/landmark/landmark_base.h
@@ -62,12 +62,12 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
         const std::vector<StateBlockPtr>& getStateBlockVec() const;
         std::vector<StateBlockPtr>& getStateBlockVec();
         std::vector<StateBlockPtr> getUsedStateBlockVec() const;
-        StateBlockPtr getStateBlockPtr(unsigned int _i) const;
-        void setStateBlockPtr(unsigned int _i, StateBlockPtr _sb_ptr);
+        StateBlockPtr getStateBlock(unsigned int _i) const;
+        void setStateBlock(unsigned int _i, StateBlockPtr _sb_ptr);
         StateBlockPtr getP() const;
         StateBlockPtr getO() const;
-        void setPPtr(const StateBlockPtr _p_ptr);
-        void setOPtr(const StateBlockPtr _o_ptr);
+        void setP(const StateBlockPtr _p_ptr);
+        void setO(const StateBlockPtr _o_ptr);
         virtual void registerNewStateBlocks();
         Eigen::VectorXs getState() const;
         void getState(Eigen::VectorXs& _state) const;
@@ -90,7 +90,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
         unsigned int getHits() const;
         FactorBasePtrList& getConstrainedByList();
 
-        void setMapPtr(const MapBasePtr _map_ptr);
+        void setMap(const MapBasePtr _map_ptr);
         MapBasePtr getMap();
 
 };
@@ -113,7 +113,7 @@ inline MapBasePtr LandmarkBase::getMap()
     return map_ptr_.lock();
 }
 
-inline void LandmarkBase::setMapPtr(const MapBasePtr _map_ptr)
+inline void LandmarkBase::setMap(const MapBasePtr _map_ptr)
 {
     map_ptr_ = _map_ptr;
 }
@@ -150,35 +150,35 @@ inline std::vector<StateBlockPtr>& LandmarkBase::getStateBlockVec()
     return state_block_vec_;
 }
 
-inline StateBlockPtr LandmarkBase::getStateBlockPtr(unsigned int _i) const
+inline StateBlockPtr LandmarkBase::getStateBlock(unsigned int _i) const
 {
     assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
     return state_block_vec_[_i];
 }
 
-inline void LandmarkBase::setStateBlockPtr(unsigned int _i, StateBlockPtr _sb_ptr)
+inline void LandmarkBase::setStateBlock(unsigned int _i, StateBlockPtr _sb_ptr)
 {
     state_block_vec_[_i] = _sb_ptr;
 }
 
 inline StateBlockPtr LandmarkBase::getP() const
 {
-    return getStateBlockPtr(0);
+    return getStateBlock(0);
 }
 
 inline StateBlockPtr LandmarkBase::getO() const
 {
-    return getStateBlockPtr(1);
+    return getStateBlock(1);
 }
 
-inline void LandmarkBase::setPPtr(const StateBlockPtr _st_ptr)
+inline void LandmarkBase::setP(const StateBlockPtr _st_ptr)
 {
-    setStateBlockPtr(0, _st_ptr);
+    setStateBlock(0, _st_ptr);
 }
 
-inline void LandmarkBase::setOPtr(const StateBlockPtr _st_ptr)
+inline void LandmarkBase::setO(const StateBlockPtr _st_ptr)
 {
-    setStateBlockPtr(1, _st_ptr);
+    setStateBlock(1, _st_ptr);
 }
 
 inline void LandmarkBase::setDescriptor(const Eigen::VectorXs& _descriptor)
diff --git a/include/base/landmark/landmark_polyline_2D.h b/include/base/landmark/landmark_polyline_2D.h
index 4414a69f6175f36280656c2df33e4e94697d0d7d..d6b5d51f9edee7b70a7376a2e0771b34c1de612e 100644
--- a/include/base/landmark/landmark_polyline_2D.h
+++ b/include/base/landmark/landmark_polyline_2D.h
@@ -74,7 +74,7 @@ class LandmarkPolyline2D : public LandmarkBase
 
         const Eigen::VectorXs getPointVector(int _i) const;
 
-        StateBlockPtr getPointStateBlockPtr(int _i);
+        StateBlockPtr getPointStateBlock(int _i);
 
         /** \brief Gets a vector of all state blocks pointers
          **/
diff --git a/include/base/problem.h b/include/base/problem.h
index 85e5dbe7f300c705d0b8d7573f7ee36684b6aa15..06a574a67a8ed5dc3712b2c676082058adf9926f 100644
--- a/include/base/problem.h
+++ b/include/base/problem.h
@@ -95,7 +95,7 @@ class Problem : public std::enable_shared_from_this<Problem>
         /** \brief get a sensor pointer by its name
          * \param _sensor_name The sensor name, as it was installed with installSensor()
          */
-        SensorBasePtr getSensorPtr(const std::string& _sensor_name);
+        SensorBasePtr getSensor(const std::string& _sensor_name);
 
         /** \brief Factory method to install (create, and add to sensor) processors only from its properties
          *
diff --git a/include/base/processor/processor_base.h b/include/base/processor/processor_base.h
index 994e3dc89b79fd7ac2d602a92ff25713059dc891..e996a382605975ccda456320961ab775e3593583 100644
--- a/include/base/processor/processor_base.h
+++ b/include/base/processor/processor_base.h
@@ -190,7 +190,7 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
 
         SensorBasePtr getSensor();
         const SensorBasePtr getSensor() const;
-        void setSensorPtr(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
+        void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
 
         virtual bool isMotion();
 
diff --git a/include/base/processor/processor_tracker_landmark_polyline.h b/include/base/processor/processor_tracker_landmark_polyline.h
deleted file mode 100644
index a6f902c930d696dc595ae19f133a0df4fd0e00c2..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_tracker_landmark_polyline.h
+++ /dev/null
@@ -1,248 +0,0 @@
-/*
- * processor_tracker_landmark_polyline.h
- *
- *  Created on: May 26, 2016
- *      Author: jvallve
- */
-
-#ifndef SRC_PROCESSOR_TRACKER_LANDMARK_POLYLINE_H_
-#define SRC_PROCESSOR_TRACKER_LANDMARK_POLYLINE_H_
-
-// Wolf includes
-#include "base/sensor/sensor_laser_2D.h"
-#include "base/capture/capture_laser_2D.h"
-#include "base/feature/feature_polyline_2D.h"
-#include "base/landmark/landmark_polyline_2D.h"
-#include "base/factor/factor_point_2D.h"
-#include "base/factor/factor_point_to_line_2D.h"
-#include "base/state_block.h"
-#include "base/association/association_tree.h"
-#include "base/processor/processor_tracker_landmark.h"
-
-//laser_scan_utils
-#include "laser_scan_utils/laser_scan.h"
-#include "laser_scan_utils/line_finder_iterative.h"
-#include "laser_scan_utils/polyline.h"
-
-namespace wolf
-{
-
-//some consts.. TODO: this tuning params should be grouped in a struct and passed to the class from ros node, at constructor level
-const Scalar position_error_th_ = 1;
-const Scalar min_features_ratio_th_ = 0.5;
-
-//forward declaration to typedef class pointers
-struct LandmarkPolylineMatch;
-typedef std::shared_ptr<LandmarkPolylineMatch> LandmarkPolylineMatchPtr;
-
-//forward declaration to typedef class pointers
-struct ProcessorParamsPolyline;
-typedef std::shared_ptr<ProcessorParamsPolyline> ProcessorParamsPolylinePtr;
-
-WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkPolyline);
-
-// Match Feature - Landmark
-struct LandmarkPolylineMatch : public LandmarkMatch
-{
-        int landmark_match_from_id_;
-        int feature_match_from_id_;
-        int landmark_match_to_id_;
-        int feature_match_to_id_;
-
-//    std::vector<unsigned int> landmark_points_match_;
-//    std::vector<unsigned int> feature_points_match_;
-//    std::vector<unsigned int> feature_points_add_front_;
-//    std::vector<unsigned int> feature_points_add_back_;
-
-        LandmarkPolylineMatch() :
-            landmark_match_from_id_(0),
-            feature_match_from_id_(0),
-            landmark_match_to_id_(0),
-            feature_match_to_id_(0)
-        {
-            //
-        }
-        LandmarkPolylineMatch(int _landmark_match_from_id,
-                              int _feature_match_from_id,
-                              int _landmark_match_to_id,
-                              int _feature_match_to_id) :
-                                  landmark_match_from_id_(_landmark_match_from_id),
-                                  feature_match_from_id_(_feature_match_from_id),
-                                  landmark_match_to_id_(_landmark_match_to_id),
-                                  feature_match_to_id_(_landmark_match_to_id)
-        {
-            //
-        }
-};
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsPolyline);
-struct ProcessorParamsPolyline : public ProcessorParamsTrackerLandmark
-{
-        laserscanutils::LineFinderIterativeParams line_finder_params;
-        Scalar position_error_th;
-        unsigned int new_features_th;
-        unsigned int loop_frames_th;
-
-        // These values below are constant and defined within the class -- provide a setter or accept them at construction time if you need to configure them
-        //        Scalar aperture_error_th_ = 20.0 * M_PI / 180.; //20 degrees
-        //        Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees;
-        //        Scalar position_error_th_ = 1;
-        //        Scalar min_features_ratio_th_ = 0.5;
-};
-
-class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
-{
-    private:
-        laserscanutils::LineFinderIterative line_finder_;
-        ProcessorParamsPolylinePtr params_;
-
-        FeatureBasePtrList polylines_incoming_;
-        FeatureBasePtrList polylines_last_;
-
-        Eigen::Matrix2s R_sensor_world_, R_world_sensor_;
-        Eigen::Matrix2s R_robot_sensor_;
-        Eigen::Matrix2s R_current_prev_;
-        Eigen::Vector2s t_sensor_world_, t_world_sensor_, t_world_sensor_prev_, t_sensor_world_prev_;
-        Eigen::Vector2s t_robot_sensor_;
-        Eigen::Vector2s t_current_prev_;
-        Eigen::Vector2s t_world_robot_;
-        bool extrinsics_transformation_computed_;
-
-    public:
-        EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
-        ProcessorTrackerLandmarkPolyline(ProcessorParamsPolylinePtr _params);
-
-        virtual ~ProcessorTrackerLandmarkPolyline();
-        virtual void configure(SensorBasePtr _sensor) { };
-
-        const FeatureBasePtrList& getLastPolylines() const;
-
-    protected:
-
-        virtual void preProcess();
-        void computeTransformations(const TimeStamp& _ts);
-        virtual void postProcess();
-
-        void advanceDerived();
-
-        void resetDerived();
-
-        /** \brief Find provided landmarks in the incoming capture
-         * \param _landmarks_in input list of landmarks to be found in incoming
-         * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
-         * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
-         */
-        virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in, FeatureBasePtrList& _features_incoming_out,
-                                           LandmarkMatchMap& _feature_landmark_correspondences);
-
-        /** \brief Vote for KeyFrame generation
-         *
-         * If a KeyFrame criterion is validated, this function returns true,
-         * meaning that it wants to create a KeyFrame at the \b last Capture.
-         *
-         * WARNING! This function only votes! It does not create KeyFrames!
-         */
-        virtual bool voteForKeyFrame();
-
-        /** \brief Detect new Features
-         * \param _capture_ptr Capture for feature detection. Defaults to incoming_ptr_.
-         * \param _new_features_list The list of detected Features. Defaults to member new_features_list_.
-         * \return The number of detected Features.
-         *
-         * This function detects Features that do not correspond to known Features/Landmarks in the system.
-         *
-         * The function sets the member new_features_list_, the list of newly detected features,
-         * to be used for landmark initialization.
-         */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
-
-        /** \brief Creates a landmark for each of new_features_last_
-         **/
-        virtual void createNewLandmarks();
-
-        /** \brief Create one landmark
-         *
-         * Implement in derived classes to build the type of landmark you need for this tracker.
-         */
-        virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
-
-        /** \brief Establish factors between features in Captures \b last and \b origin
-         */
-        virtual void establishFactors();
-
-        /** \brief look for known objects in the list of unclassified polylines
-        */
-        void classifyPolilines(LandmarkBasePtrList& _lmk_list);
-
-        /** \brief Create a new factor
-         * \param _feature_ptr pointer to the Feature to constrain
-         * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
-         *
-         * Implement this method in derived classes.
-         */
-        virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
-
-    private:
-
-        void extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _polyline_list);
-
-        void expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_,
-                             Eigen::MatrixXs& expected_feature_cov_);
-
-        Eigen::VectorXs computeSquaredMahalanobisDistances(const Eigen::Vector2s& _feature,
-                                                           const Eigen::Matrix2s& _feature_cov,
-                                                           const Eigen::Vector2s& _expected_feature,
-                                                           const Eigen::Matrix2s& _expected_feature_cov,
-                                                           const Eigen::MatrixXs& _mu);
-        Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux, const Eigen::Vector3s& _B,
-                               bool _A_extreme, bool _B_extreme);
-    // Factory method
-    public:
-        static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
-};
-
-inline ProcessorTrackerLandmarkPolyline::ProcessorTrackerLandmarkPolyline(ProcessorParamsPolylinePtr _params) :
-        ProcessorTrackerLandmark("TRACKER LANDMARK POLYLINE", _params),
-        line_finder_(_params->line_finder_params),
-        params_(_params),
-        extrinsics_transformation_computed_(false)
-{
-}
-
-inline unsigned int ProcessorTrackerLandmarkPolyline::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
-{
-    // already computed since each scan is computed in preprocess()
-    _features_incoming_out = std::move(polylines_last_);
-    return _features_incoming_out.size();
-}
-
-inline void ProcessorTrackerLandmarkPolyline::advanceDerived()
-{
-    //std::cout << "\tProcessorTrackerLandmarkPolyline::advance:" << std::endl;
-    //std::cout << "\t\tcorners_last: " << polylines_last_.size() << std::endl;
-    //std::cout << "\t\tcorners_incoming_: " << polylines_incoming_.size() << std::endl;
-    ProcessorTrackerLandmark::advanceDerived();
-    for (auto polyline : polylines_last_)
-        polyline->remove();
-    polylines_last_ = std::move(polylines_incoming_);
-    //std::cout << "advanced" << std::endl;
-}
-
-inline void ProcessorTrackerLandmarkPolyline::resetDerived()
-{
-    //std::cout << "\tProcessorTrackerLandmarkPolyline::reset:" << std::endl;
-    //std::cout << "\t\tcorners_last: " << corners_last_.size() << std::endl;
-    //std::cout << "\t\tcorners_incoming_: " << polylines_incoming_.size() << std::endl;
-    ProcessorTrackerLandmark::resetDerived();
-    polylines_last_ = std::move(polylines_incoming_);
-}
-
-inline const FeatureBasePtrList& ProcessorTrackerLandmarkPolyline::getLastPolylines() const
-{
-    return polylines_last_;
-}
-
-} // namespace wolf
-
-#endif /* SRC_PROCESSOR_TRACKER_LASER_H_ */
diff --git a/include/base/sensor/sensor_base.h b/include/base/sensor/sensor_base.h
index d9000252b6f6a8d5ad736ee68205e307bef18a2b..b4f5e233ba5db866a885b84ac35f9ae3002581cd 100644
--- a/include/base/sensor/sensor_base.h
+++ b/include/base/sensor/sensor_base.h
@@ -95,7 +95,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
         virtual void setProblem(ProblemPtr _problem) final;
 
         HardwareBasePtr getHardware();
-        void setHardwarePtr(const HardwareBasePtr _hw_ptr);
+        void setHardware(const HardwareBasePtr _hw_ptr);
 
         ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr);
         ProcessorBasePtrList& getProcessorList();
@@ -109,8 +109,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
         const std::vector<StateBlockPtr>& getStateBlockVec() const;
         std::vector<StateBlockPtr>& getStateBlockVec();
         StateBlockPtr getStateBlockPtrStatic(unsigned int _i) const;
-        StateBlockPtr getStateBlockPtr(unsigned int _i);
-        StateBlockPtr getStateBlockPtr(unsigned int _i, const TimeStamp& _ts);
+        StateBlockPtr getStateBlock(unsigned int _i);
+        StateBlockPtr getStateBlock(unsigned int _i, const TimeStamp& _ts);
         void setStateBlockPtrStatic(unsigned int _i, const StateBlockPtr _sb_ptr);
         void resizeStateBlockVec(unsigned int _size);
 
@@ -119,15 +119,15 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
         bool isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts);
         bool isStateBlockDynamic(unsigned int _i);
 
-        StateBlockPtr getPPtr(const TimeStamp _ts);
-        StateBlockPtr getOPtr(const TimeStamp _ts);
-        StateBlockPtr getIntrinsicPtr(const TimeStamp _ts);
+        StateBlockPtr getP(const TimeStamp _ts);
+        StateBlockPtr getO(const TimeStamp _ts);
+        StateBlockPtr getIntrinsic(const TimeStamp _ts);
         StateBlockPtr getP() ;
         StateBlockPtr getO();
         StateBlockPtr getIntrinsic();
-        void setPPtr(const StateBlockPtr _p_ptr);
-        void setOPtr(const StateBlockPtr _o_ptr);
-        void setIntrinsicPtr(const StateBlockPtr _intr_ptr);
+        void setP(const StateBlockPtr _p_ptr);
+        void setO(const StateBlockPtr _o_ptr);
+        void setIntrinsic(const StateBlockPtr _intr_ptr);
         void removeStateBlocks();
 
         void fix();
@@ -264,22 +264,22 @@ inline HardwareBasePtr SensorBase::getHardware()
     return hardware_ptr_.lock();
 }
 
-inline void SensorBase::setPPtr(const StateBlockPtr _p_ptr)
+inline void SensorBase::setP(const StateBlockPtr _p_ptr)
 {
     setStateBlockPtrStatic(0, _p_ptr);
 }
 
-inline void SensorBase::setOPtr(const StateBlockPtr _o_ptr)
+inline void SensorBase::setO(const StateBlockPtr _o_ptr)
 {
     setStateBlockPtrStatic(1, _o_ptr);
 }
 
-inline void SensorBase::setIntrinsicPtr(const StateBlockPtr _intr_ptr)
+inline void SensorBase::setIntrinsic(const StateBlockPtr _intr_ptr)
 {
     setStateBlockPtrStatic(2, _intr_ptr);
 }
 
-inline void SensorBase::setHardwarePtr(const HardwareBasePtr _hw_ptr)
+inline void SensorBase::setHardware(const HardwareBasePtr _hw_ptr)
 {
     hardware_ptr_ = _hw_ptr;
 }
diff --git a/include/base/state_block.h b/include/base/state_block.h
index 09811c41e583e1155c7aa59a5ce9e93098b188ee..447e8a0e9b3534f8048f3aa55131bdaed617e0d7 100644
--- a/include/base/state_block.h
+++ b/include/base/state_block.h
@@ -112,7 +112,7 @@ public:
 
         /** \brief Sets a local parametrization
          **/
-        void setLocalParametrizationPtr(LocalParametrizationBasePtr _local_param);
+        void setLocalParametrization(LocalParametrizationBasePtr _local_param);
 
         /** \brief Removes the state_block local parametrization
          **/
@@ -242,7 +242,7 @@ inline void StateBlock::removeLocalParametrization()
     local_param_updated_.store(true);
 }
 
-inline void StateBlock::setLocalParametrizationPtr(LocalParametrizationBasePtr _local_param)
+inline void StateBlock::setLocalParametrization(LocalParametrizationBasePtr _local_param)
 {
 	assert(_local_param != nullptr && "setting a null local parametrization");
     local_param_ptr_ = _local_param;
diff --git a/include/base/trajectory_base.h b/include/base/trajectory_base.h
index 9150f8fa0daeb9f7999f38f35a5db02712a63c10..6ceddf9e6a8742133b01ee9277369a0a3bcaa5be 100644
--- a/include/base/trajectory_base.h
+++ b/include/base/trajectory_base.h
@@ -41,7 +41,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
         FrameBasePtr getLastKeyFrame();
         FrameBasePtr findLastKeyFrame();
         FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts);
-        void setLastKeyFramePtr(FrameBasePtr _key_frame_ptr);
+        void setLastKeyFrame(FrameBasePtr _key_frame_ptr);
         void sortFrame(FrameBasePtr _frm_ptr);
         void moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place);
         FrameBaseIter computeFrameOrder(FrameBasePtr _frame_ptr);
@@ -66,7 +66,7 @@ inline FrameBasePtr TrajectoryBase::getLastKeyFrame()
     return last_key_frame_ptr_;
 }
 
-inline void TrajectoryBase::setLastKeyFramePtr(FrameBasePtr _key_frame_ptr)
+inline void TrajectoryBase::setLastKeyFrame(FrameBasePtr _key_frame_ptr)
 {
     last_key_frame_ptr_ = _key_frame_ptr;
 }
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 86165e4503302e5898a1ae0c6f2129297d2b2b73..1d2aa58dbd1b0a5b1767def1eb55d8978f729ac4 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -99,7 +99,7 @@ FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr)
 {
     //std::cout << "Adding feature" << std::endl;
     feature_list_.push_back(_ft_ptr);
-    _ft_ptr->setCapturePtr(shared_from_this());
+    _ft_ptr->setCapture(shared_from_this());
     _ft_ptr->setProblem(getProblem());
     return _ft_ptr;
 }
@@ -108,7 +108,7 @@ void CaptureBase::addFeatureList(FeatureBasePtrList& _new_ft_list)
 {
     for (FeatureBasePtr feature_ptr : _new_ft_list)
     {
-        feature_ptr->setCapturePtr(shared_from_this());
+        feature_ptr->setCapture(shared_from_this());
         if (getProblem() != nullptr)
             feature_ptr->setProblem(getProblem());
     }
@@ -124,11 +124,11 @@ void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
 FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
 {
     constrained_by_list_.push_back(_fac_ptr);
-    _fac_ptr->setCaptureOtherPtr(shared_from_this());
+    _fac_ptr->setCaptureOther(shared_from_this());
     return _fac_ptr;
 }
 
-StateBlockPtr CaptureBase::getStateBlockPtr(unsigned int _i) const
+StateBlockPtr CaptureBase::getStateBlock(unsigned int _i) const
 {
     if (getSensor())
     {
@@ -168,7 +168,7 @@ void CaptureBase::removeStateBlocks()
             {
                 getProblem()->removeStateBlock(sbp);
             }
-            setStateBlockPtr(i, nullptr);
+            setStateBlock(i, nullptr);
         }
     }
 }
@@ -177,7 +177,7 @@ void CaptureBase::fix()
 {
     for (unsigned int i = 0; i<getStateBlockVec().size(); i++)
     {
-        auto sbp = getStateBlockPtr(i);
+        auto sbp = getStateBlock(i);
         if (sbp != nullptr)
             sbp->fix();
     }
@@ -188,7 +188,7 @@ void CaptureBase::unfix()
 {
     for (unsigned int i = 0; i<getStateBlockVec().size(); i++)
     {
-        auto sbp = getStateBlockPtr(i);
+        auto sbp = getStateBlock(i);
         if (sbp != nullptr)
             sbp->unfix();
     }
@@ -199,7 +199,7 @@ void CaptureBase::fixExtrinsics()
 {
     for (unsigned int i = 0; i < 2; i++)
     {
-        auto sbp = getStateBlockPtr(i);
+        auto sbp = getStateBlock(i);
         if (sbp != nullptr)
             sbp->fix();
     }
@@ -210,7 +210,7 @@ void CaptureBase::unfixExtrinsics()
 {
     for (unsigned int i = 0; i < 2; i++)
     {
-        auto sbp = getStateBlockPtr(i);
+        auto sbp = getStateBlock(i);
         if (sbp != nullptr)
             sbp->unfix();
     }
@@ -221,7 +221,7 @@ void CaptureBase::fixIntrinsics()
 {
     for (unsigned int i = 2; i < getStateBlockVec().size(); i++)
     {
-        auto sbp = getStateBlockPtr(i);
+        auto sbp = getStateBlock(i);
         if (sbp != nullptr)
             sbp->fix();
     }
@@ -232,7 +232,7 @@ void CaptureBase::unfixIntrinsics()
 {
     for (unsigned int i = 2; i < getStateBlockVec().size(); i++)
     {
-        auto sbp = getStateBlockPtr(i);
+        auto sbp = getStateBlock(i);
         if (sbp != nullptr)
             sbp->unfix();
     }
@@ -254,7 +254,7 @@ SizeEigen CaptureBase::computeCalibSize() const
     SizeEigen sz = 0;
     for (unsigned int i = 0; i < state_block_vec_.size(); i++)
     {
-        auto sb = getStateBlockPtr(i);
+        auto sb = getStateBlock(i);
         if (sb && !sb->isFixed())
             sz += sb->getSize();
     }
@@ -267,7 +267,7 @@ Eigen::VectorXs CaptureBase::getCalibration() const
     SizeEigen index = 0;
     for (unsigned int i = 0; i < getStateBlockVec().size(); i++)
     {
-        auto sb = getStateBlockPtr(i);
+        auto sb = getStateBlock(i);
         if (sb && !sb->isFixed())
         {
             calib.segment(index, sb->getSize()) = sb->getState();
@@ -284,7 +284,7 @@ void CaptureBase::setCalibration(const VectorXs& _calib)
     SizeEigen index = 0;
     for (unsigned int i = 0; i < getStateBlockVec().size(); i++)
     {
-        auto sb = getStateBlockPtr(i);
+        auto sb = getStateBlock(i);
         if (sb && !sb->isFixed())
         {
             sb->setState(_calib.segment(index, sb->getSize()));
diff --git a/src/examples/test_factor_imu.cpp b/src/examples/test_factor_imu.cpp
index 59d98814f8d1995ad6abf6055a3158caa009a33f..5dde0e16a573a9b8e4e5fb90d6b65e905dcb18e6 100644
--- a/src/examples/test_factor_imu.cpp
+++ b/src/examples/test_factor_imu.cpp
@@ -54,7 +54,7 @@ int main(int argc, char** argv)
     
     // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
     CaptureIMUPtr imu_ptr( std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6s::Identity()) );
-    imu_ptr->setFramePtr(wolf_problem_ptr_->getTrajectory()->getFrameList().back());
+    imu_ptr->setFrame(wolf_problem_ptr_->getTrajectory()->getFrameList().back());
 
     // set variables
     using namespace std;
@@ -87,7 +87,7 @@ int main(int argc, char** argv)
     delta_preint_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
     delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
     std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
-    feat_imu->setCapturePtr(imu_ptr);
+    feat_imu->setCapture(imu_ptr);
 
         //create a factorIMU
     FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, last_frame);
@@ -115,7 +115,7 @@ int main(int argc, char** argv)
 
     //reset origin of motion to new frame
     wolf_problem_ptr_->getProcessorMotion()->setOrigin(last_frame);
-    imu_ptr->setFramePtr(last_frame);
+    imu_ptr->setFrame(last_frame);
     }
     /// ******************************************************************************************** ///
 
@@ -148,7 +148,7 @@ int main(int argc, char** argv)
     delta_preint_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
     delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
     std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
-    feat_imu->setCapturePtr(imu_ptr);
+    feat_imu->setCapture(imu_ptr);
 
         //create a factorIMU
     FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, last_frame);
@@ -176,7 +176,7 @@ int main(int argc, char** argv)
 
     //reset origin of motion to new frame
     wolf_problem_ptr_->getProcessorMotion()->setOrigin(last_frame);
-    imu_ptr->setFramePtr(last_frame);
+    imu_ptr->setFrame(last_frame);
     }
 
     mpu_clock = 0.004046;
@@ -206,7 +206,7 @@ int main(int argc, char** argv)
     delta_preint_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
     delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
     std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
-    feat_imu->setCapturePtr(imu_ptr);
+    feat_imu->setCapture(imu_ptr);
 
         //create a factorIMU
     FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, last_frame);
diff --git a/src/examples/test_map_yaml.cpp b/src/examples/test_map_yaml.cpp
index e9a2af536ec532ed94aa8340d0d4d11810e4089a..7dedb1200e1fcfa14483a848bf5b3032aac3ccd5 100644
--- a/src/examples/test_map_yaml.cpp
+++ b/src/examples/test_map_yaml.cpp
@@ -32,7 +32,7 @@ void print(MapBase& _map)
             std::cout << "\nFirst def: " << poly_ptr->isFirstDefined();
             std::cout << "\nLast  def: " << poly_ptr->isLastDefined();
             for (int idx = poly_ptr->getFirstId(); idx <= poly_ptr->getLastId(); idx++)
-                std::cout << "\n  point: " << idx << ": " << poly_ptr->getPointStateBlockPtr(idx)->getState().transpose();
+                std::cout << "\n  point: " << idx << ": " << poly_ptr->getPointStateBlock(idx)->getState().transpose();
             break;
         }
         else if (lmk_ptr->getType() == "AHP")
diff --git a/src/feature/feature_IMU.cpp b/src/feature/feature_IMU.cpp
index 0f811cff599f6bbc62e28dd16b0cc6a82648b02b..b35baf2d0a7dc9191e0a17886aea0a6134b060df 100644
--- a/src/feature/feature_IMU.cpp
+++ b/src/feature/feature_IMU.cpp
@@ -13,7 +13,7 @@ FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated,
     jacobian_bias_(_dD_db_jacobians)
 {
     if (_cap_imu_ptr)
-        this->setCapturePtr(_cap_imu_ptr);
+        this->setCapture(_cap_imu_ptr);
 }
 
 FeatureIMU::FeatureIMU(CaptureMotionPtr _cap_imu_ptr):
@@ -22,7 +22,7 @@ FeatureIMU::FeatureIMU(CaptureMotionPtr _cap_imu_ptr):
         gyro_bias_preint_(_cap_imu_ptr->getCalibrationPreint().tail<3>()),
         jacobian_bias_(_cap_imu_ptr->getJacobianCalib())
 {
-    this->setCapturePtr(_cap_imu_ptr);
+    this->setCapture(_cap_imu_ptr);
 }
 
 FeatureIMU::~FeatureIMU()
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 39061dea53cbe3ee5e63f4ca1b542bd12afed644..b47c956b9f39e574f8a8c019788669bc024efbe7 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -54,7 +54,7 @@ void FeatureBase::remove()
 FactorBasePtr FeatureBase::addFactor(FactorBasePtr _co_ptr)
 {
     factor_list_.push_back(_co_ptr);
-    _co_ptr->setFeaturePtr(shared_from_this());
+    _co_ptr->setFeature(shared_from_this());
     _co_ptr->setProblem(getProblem());
     // add factor to be added in solver
     if (getProblem() != nullptr)
@@ -75,7 +75,7 @@ FrameBasePtr FeatureBase::getFrame() const
 FactorBasePtr FeatureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
 {
     constrained_by_list_.push_back(_fac_ptr);
-    _fac_ptr->setFeatureOtherPtr(shared_from_this());
+    _fac_ptr->setFeatureOther(shared_from_this());
     return _fac_ptr;
 }
 
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index fcf542af51341dbe6e747ae99f383e76a6304c38..ccd51dc7f71cf6e696ed68c5e77a50f314b5bf45 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -72,7 +72,7 @@ void FrameBase::remove()
             removeStateBlocks();
 
         if (getTrajectory()->getLastKeyFrame()->id() == this_F->id())
-            getTrajectory()->setLastKeyFramePtr(getTrajectory()->findLastKeyFrame());
+            getTrajectory()->setLastKeyFrame(getTrajectory()->findLastKeyFrame());
 
 //        std::cout << "Removed       F" << id() << std::endl;
     }
@@ -99,14 +99,14 @@ void FrameBase::removeStateBlocks()
 {
     for (unsigned int i = 0; i < state_block_vec_.size(); i++)
     {
-        StateBlockPtr sbp = getStateBlockPtr(i);
+        StateBlockPtr sbp = getStateBlock(i);
         if (sbp != nullptr)
         {
             if (getProblem() != nullptr)
             {
                 getProblem()->removeStateBlock(sbp);
             }
-            setStateBlockPtr(i, nullptr);
+            setStateBlock(i, nullptr);
         }
     }
 }
@@ -119,7 +119,7 @@ void FrameBase::setKey()
         registerNewStateBlocks();
 
         if (getTrajectory()->getLastKeyFrame() == nullptr || getTrajectory()->getLastKeyFrame()->getTimeStamp() < time_stamp_)
-            getTrajectory()->setLastKeyFramePtr(shared_from_this());
+            getTrajectory()->setLastKeyFrame(shared_from_this());
 
         getTrajectory()->sortFrame(shared_from_this());
 
@@ -279,7 +279,7 @@ FrameBasePtr FrameBase::getNextFrame() const
 CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
 {
     capture_list_.push_back(_capt_ptr);
-    _capt_ptr->setFramePtr(shared_from_this());
+    _capt_ptr->setFrame(shared_from_this());
     _capt_ptr->setProblem(getProblem());
     _capt_ptr->registerNewStateBlocks();
     return _capt_ptr;
@@ -341,7 +341,7 @@ void FrameBase::getFactorList(FactorBasePtrList& _fac_list)
 FactorBasePtr FrameBase::addConstrainedBy(FactorBasePtr _fac_ptr)
 {
     constrained_by_list_.push_back(_fac_ptr);
-    _fac_ptr->setFrameOtherPtr(shared_from_this());
+    _fac_ptr->setFrameOther(shared_from_this());
     return _fac_ptr;
 }
 
diff --git a/src/hardware_base.cpp b/src/hardware_base.cpp
index 3349005129e275a5dd930cd0927d408149994680..945b412c87cbab6be87a8d7d5536d2850996a14f 100644
--- a/src/hardware_base.cpp
+++ b/src/hardware_base.cpp
@@ -18,7 +18,7 @@ SensorBasePtr HardwareBase::addSensor(SensorBasePtr _sensor_ptr)
 {
     sensor_list_.push_back(_sensor_ptr);
     _sensor_ptr->setProblem(getProblem());
-    _sensor_ptr->setHardwarePtr(shared_from_this());
+    _sensor_ptr->setHardware(shared_from_this());
 
     _sensor_ptr->registerNewStateBlocks();
 
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 48f66967cbfc59d2fbe237ca5ae8320070c1789e..d1fb63043ac989955ab8954c9cc9a19754184bec 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -108,14 +108,14 @@ void LandmarkBase::removeStateBlocks()
 {
     for (unsigned int i = 0; i < state_block_vec_.size(); i++)
     {
-        auto sbp = getStateBlockPtr(i);
+        auto sbp = getStateBlock(i);
         if (sbp != nullptr)
         {
             if (getProblem() != nullptr)
             {
                 getProblem()->removeStateBlock(sbp);
             }
-            setStateBlockPtr(i, nullptr);
+            setStateBlock(i, nullptr);
         }
     }
 }
@@ -173,7 +173,7 @@ YAML::Node LandmarkBase::saveToYaml() const
 FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr)
 {
     constrained_by_list_.push_back(_fac_ptr);
-    _fac_ptr->setLandmarkOtherPtr(shared_from_this());
+    _fac_ptr->setLandmarkOther(shared_from_this());
     return _fac_ptr;
 }
 
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp
index b31117566ac69542436e471dd1b1639492680a86..d7c5c7d76a3e3b3237805b0b55218f9c258cbeaf 100644
--- a/src/landmark/landmark_polyline_2D.cpp
+++ b/src/landmark/landmark_polyline_2D.cpp
@@ -26,9 +26,9 @@ LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_pt
         point_state_ptr_vector_.push_back(std::make_shared<StateBlock>(_points.col(i).head<2>()));
 
     if (!first_defined_)
-        point_state_ptr_vector_.front()->setLocalParametrizationPtr(std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_[1]));
+        point_state_ptr_vector_.front()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_[1]));
     if (!last_defined_)
-        point_state_ptr_vector_.back()->setLocalParametrizationPtr(std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_[point_state_ptr_vector_.size() - 2]));
+        point_state_ptr_vector_.back()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_[point_state_ptr_vector_.size() - 2]));
 
     assert(point_state_ptr_vector_.front()->hasLocalParametrization() ? !first_defined_ : first_defined_);
     assert(point_state_ptr_vector_.back()->hasLocalParametrization() ? !last_defined_ : last_defined_);
@@ -67,7 +67,7 @@ const Eigen::VectorXs LandmarkPolyline2D::getPointVector(int _i) const
     return point_state_ptr_vector_[_i-first_id_]->getState();
 }
 
-StateBlockPtr LandmarkPolyline2D::getPointStateBlockPtr(int _i)
+StateBlockPtr LandmarkPolyline2D::getPointStateBlock(int _i)
 {
 	assert(_i-first_id_ >= 0 && _i-first_id_ <= (int)(point_state_ptr_vector_.size()) && "out of range!");
 	return point_state_ptr_vector_[_i-first_id_];
@@ -230,7 +230,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
     assert(_remain_id < getLastId() || last_defined_);
 
     // take a defined extreme as remaining
-    StateBlockPtr remove_state = getPointStateBlockPtr(_remove_id);
+    StateBlockPtr remove_state = getPointStateBlock(_remove_id);
     std::cout << "state block to remove " << remove_state->getState().transpose() << std::endl;
 
     // Change factors from remove_state to remain_state
diff --git a/src/map_base.cpp b/src/map_base.cpp
index fdfbf11fbb0965d1ebab126539cb4e5c27bb3a2c..d1ad03121691b64052bfab01372010f68de863f5 100644
--- a/src/map_base.cpp
+++ b/src/map_base.cpp
@@ -30,7 +30,7 @@ MapBase::~MapBase()
 LandmarkBasePtr MapBase::addLandmark(LandmarkBasePtr _landmark_ptr)
 {
     landmark_list_.push_back(_landmark_ptr);
-    _landmark_ptr->setMapPtr(shared_from_this());
+    _landmark_ptr->setMap(shared_from_this());
     _landmark_ptr->setProblem(getProblem());
     _landmark_ptr->registerNewStateBlocks();
     return _landmark_ptr;
@@ -41,7 +41,7 @@ void MapBase::addLandmarkList(LandmarkBasePtrList& _landmark_list)
 	LandmarkBasePtrList lmk_list_copy = _landmark_list; //since _landmark_list will be empty after addDownNodeList()
     for (LandmarkBasePtr landmark_ptr : lmk_list_copy)
     {
-        landmark_ptr->setMapPtr(shared_from_this());
+        landmark_ptr->setMap(shared_from_this());
         landmark_ptr->setProblem(getProblem());
         landmark_ptr->registerNewStateBlocks();
     }
diff --git a/src/problem.cpp b/src/problem.cpp
index 8bb5bdac7e2164a9ee2e6338fb203383daba3e57..8e682083d4ab8204c38c5d0f874ba5f60c39d78c 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -148,7 +148,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
                                            const std::string& _corresponding_sensor_name, //
                                            const std::string& _params_filename)
 {
-    SensorBasePtr sen_ptr = getSensorPtr(_corresponding_sensor_name);
+    SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name);
     if (sen_ptr == nullptr)
         throw std::runtime_error("Sensor not found. Cannot bind Processor.");
     if (_params_filename == "")
@@ -166,7 +166,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
                                            const std::string& _corresponding_sensor_name, //
                                            const paramsServer& _server)
 {
-    SensorBasePtr sen_ptr = getSensorPtr(_corresponding_sensor_name);
+    SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name);
     if (sen_ptr == nullptr)
         throw std::runtime_error("Sensor not found. Cannot bind Processor.");
     ProcessorBasePtr prc_ptr = NewProcessorFactory::get().create(uppercase(_prc_type), _unique_processor_name, _server, sen_ptr);
@@ -184,7 +184,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
     return prc_ptr;
 }
 
-SensorBasePtr Problem::getSensorPtr(const std::string& _sensor_name)
+SensorBasePtr Problem::getSensor(const std::string& _sensor_name)
 {
     auto sen_it = std::find_if(getHardware()->getSensorList().begin(),
                                getHardware()->getSensorList().end(),
@@ -776,7 +776,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
                 {
                     if (i==0) cout << "    Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
                     if (i==2) cout << "    Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
-                    auto sb = S->getStateBlockPtr(i);
+                    auto sb = S->getStateBlock(i);
                     if (sb)
                     {
                         cout << (sb->isFixed() ? " Fix( " : " Est( ") << sb->getState().transpose() << " )";
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 4798ead84ec1c9ebdf1bea82bd2155bf9d17f454..ef5cbb1cd61cfe586cfcda1b573ddfc7056142ae 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -63,7 +63,7 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
     }
 
     // Update the existing capture
-    _capture_source->setOriginFramePtr(_keyframe_target);
+    _capture_source->setOriginFrame(_keyframe_target);
 
     // re-integrate existing buffer -- note: the result of re-integration is stored in the same buffer!
     reintegrateBuffer(_capture_source);
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index e502ba296a1a4fee6a355839c9d7a6d52871a54a..afdc9446a7f3dc0cc54c8453bc07c190a821f90e 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -53,7 +53,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
         case FIRST_TIME_WITH_PACK :
         {
             PackKeyFramePtr pack = kf_pack_buffer_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
-            kf_pack_buffer_.removeUpTo( incoming_ptr_->getTimeStamp() );
+            kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() );
 
             WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
 
@@ -124,7 +124,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
         case RUNNING_WITH_PACK :
         {
             PackKeyFramePtr pack = kf_pack_buffer_.selectPack( last_ptr_ , params_tracker_->time_tolerance);
-            kf_pack_buffer_.removeUpTo( last_ptr_->getTimeStamp() );
+            kf_pack_buffer_.removeUpTo( pack->key_frame->getTimeStamp() );
 
             WOLF_DEBUG( "PT ", getName(), ": KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp().get() );
 
@@ -170,6 +170,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
                 // We create a KF
 
                 // set KF on last
+                last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
                 last_ptr_->getFrame()->setKey();
 
                 // make F; append incoming to new F
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index 670f3aaf776d371bc1db000d84184f5643cc8e2e..0ee10e6f75b5fa6e843421eaba7728edc7b2f4ea 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -152,17 +152,6 @@ void ProcessorTrackerFeature::establishFactors()
         feature_in_last  ->addFactor(fac_ptr);
         feature_in_origin->addConstrainedBy(fac_ptr);
 
-        if (fac_ptr != nullptr) // factor links
-        {
-            FrameBasePtr frm = fac_ptr->getFrameOther();
-            if (frm)
-                frm->addConstrainedBy(fac_ptr);
-            CaptureBasePtr cap = fac_ptr->getCaptureOther();
-            if (cap)
-                cap->addConstrainedBy(fac_ptr);
-        }
-
-
         WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
                     " origin: "           , feature_in_origin->id() ,
                     " from last: "        , feature_in_last->id() );
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index 3d5b75c8fcc6e9ef70ef396c31a6960a5aa3a016..d19610391edb98ee2565c2f477970a11c0088075 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -137,12 +137,6 @@ void ProcessorTrackerLandmark::establishFactors()
         {
             last_feature->addFactor(fac_ptr);
             lmk->addConstrainedBy(fac_ptr);
-            FrameBasePtr frm = fac_ptr->getFrameOther();
-            if (frm)
-                frm->addConstrainedBy(fac_ptr);
-            CaptureBasePtr cap = fac_ptr->getCaptureOther();
-            if (cap)
-                cap->addConstrainedBy(fac_ptr);
         }
     }
 }
diff --git a/src/processor/processor_tracker_landmark_polyline.cpp b/src/processor/processor_tracker_landmark_polyline.cpp
deleted file mode 100644
index a8037d037e0cad2f3739a4c1766ae86dcdeccc89..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker_landmark_polyline.cpp
+++ /dev/null
@@ -1,1020 +0,0 @@
-#include "base/processor/processor_tracker_landmark_polyline.h"
-
-namespace wolf
-{
-
-void ProcessorTrackerLandmarkPolyline::preProcess()
-{
-    //std::cout << "PreProcess: " << std::endl;
-
-    // extract corners of incoming
-    extractPolylines(std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_), polylines_incoming_);
-
-    // compute transformations
-    computeTransformations(incoming_ptr_->getTimeStamp());
-
-    //std::cout << "PreProcess: incoming new features: " << polylines_incoming_.size() << std::endl;
-}
-
-void ProcessorTrackerLandmarkPolyline::computeTransformations(const TimeStamp& _ts)
-{
-    Eigen::Vector3s vehicle_pose = getProblem()->getState(_ts);
-    t_world_robot_ = vehicle_pose.head<2>();
-
-    // world_robot
-    Eigen::Matrix2s R_world_robot = Eigen::Rotation2Ds(vehicle_pose(2)).matrix();
-
-    // robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
-    if (getSensor()->extrinsicsInCaptures() || !getSensor()->getP()->isFixed()
-            || !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_)
-    {
-        t_robot_sensor_ = getSensor()->getP()->getState();
-        R_robot_sensor_ = Eigen::Rotation2Ds(getSensor()->getO()->getState()(0)).matrix();
-        extrinsics_transformation_computed_ = true;
-    }
-
-    // global_sensor
-    R_world_sensor_ = R_world_robot * R_robot_sensor_;
-    t_world_sensor_ = t_world_robot_ + R_world_robot * t_robot_sensor_;
-
-    // sensor_global
-    R_sensor_world_ = R_robot_sensor_.transpose() * R_world_robot.transpose();
-    t_sensor_world_ = -R_robot_sensor_.transpose() * t_robot_sensor_ -R_sensor_world_ * t_world_robot_ ;
-
-    //std::cout << "t_world_robot_ " << t_world_robot_.transpose() << std::endl;
-    //std::cout << "t_robot_sensor_ " << t_robot_sensor_.transpose() << std::endl;
-    //std::cout << "R_robot_sensor_ " << std::endl << R_robot_sensor_ << std::endl;
-    //std::cout << "t_world_sensor_ " << t_world_sensor_.transpose() << std::endl;
-    //std::cout << "R_world_sensor_ " << std::endl << R_world_sensor_ << std::endl;
-    //std::cout << "t_sensor_world_ " << t_sensor_world_.transpose() << std::endl;
-    //std::cout << "R_sensor_world_ " << std::endl << R_sensor_world_ << std::endl;
-
-}
-
-unsigned int ProcessorTrackerLandmarkPolyline::findLandmarks(const LandmarkBasePtrList& _landmarks_searched,
-                                                           FeatureBasePtrList& _features_found,
-                                                           LandmarkMatchMap& _feature_landmark_correspondences)
-{
-    //std::cout << "ProcessorTrackerLandmarkPolyline::findLandmarks: " << _landmarks_searched.size() << " features: " << polylines_incoming_.size()  << std::endl;
-
-    // COMPUTING ALL EXPECTED FEATURES
-    std::map<LandmarkBasePtr, Eigen::MatrixXs> expected_features;
-    std::map<LandmarkBasePtr, Eigen::MatrixXs> expected_features_covs;
-    for (auto landmark : _landmarks_searched)
-        if (landmark->getType() == "POLYLINE 2D")
-        {
-            expected_features[landmark] = Eigen::MatrixXs(3, (std::static_pointer_cast<LandmarkPolyline2D>(landmark))->getNPoints());
-            expected_features_covs[landmark] = Eigen::MatrixXs(2, 2*(std::static_pointer_cast<LandmarkPolyline2D>(landmark))->getNPoints());
-            expectedFeature(landmark, expected_features[landmark], expected_features_covs[landmark]);
-        }
-
-    // NAIVE NEAREST NEIGHBOR MATCHING
-    LandmarkPolylineMatchPtr best_match = nullptr;
-    FeaturePolyline2DPtr polyline_feature;
-    LandmarkPolyline2DPtr polyline_landmark;
-
-    auto next_feature_it = polylines_incoming_.begin();
-    auto feature_it = next_feature_it++;
-    int max_ftr, max_lmk, max_offset, min_offset, offset, from_ftr, from_lmk, to_ftr, to_lmk, N_overlapped;
-
-    // iterate over all polylines features
-    while (feature_it != polylines_incoming_.end())
-    {
-        polyline_feature = std::static_pointer_cast<FeaturePolyline2D>(*feature_it);
-        max_ftr = polyline_feature->getNPoints() - 1;
-
-        // Check with all landmarks
-        for (auto landmark_it = _landmarks_searched.begin(); landmark_it != _landmarks_searched.end(); landmark_it++)
-        {
-            polyline_landmark = std::static_pointer_cast<LandmarkPolyline2D>(*landmark_it);
-
-            // Open landmark polyline
-            if (!polyline_landmark->isClosed())
-            {
-                //std::cout << "MATCHING WITH OPEN LANDMARK" << std::endl;
-                //std::cout << "\tfeature  " << polyline_feature->id() << ": 0-" << max_ftr << std::endl;
-                //std::cout << "\tlandmark " << polyline_landmark->id() << ": 0-" << polyline_landmark->getNPoints() - 1 << std::endl;
-                max_lmk = polyline_landmark->getNPoints() - 1;
-                max_offset = max_ftr;
-                min_offset = -max_lmk;
-
-                // Check all overlapping positions between each feature-landmark pair
-                for (offset = min_offset; offset <= max_offset; offset++)
-                {
-                    if (offset == min_offset && !polyline_landmark->isLastDefined() && !polyline_feature->isFirstDefined())
-                        continue;
-
-                    if (offset == max_offset && !polyline_landmark->isFirstDefined() && !polyline_feature->isLastDefined())
-                        continue;
-
-                    from_lmk = std::max(0, -offset);
-                    from_ftr = std::max(0, offset);
-                    N_overlapped = std::min(max_ftr - from_ftr, max_lmk - from_lmk)+1;
-                    to_lmk = from_lmk+N_overlapped-1;
-                    to_ftr = from_ftr+N_overlapped-1;
-
-                    //std::cout << "\t\toffset " << offset << std::endl;
-                    //std::cout << "\t\t\tfrom_lmk " << from_lmk << std::endl;
-                    //std::cout << "\t\t\tfrom_ftr " << from_ftr << std::endl;
-                    //std::cout << "\t\t\tN_overlapped " << N_overlapped << std::endl;
-
-                    // Compute the squared distance for all overlapped points
-                    Eigen::ArrayXXd d = (polyline_feature->getPoints().block(0,from_ftr, 2,N_overlapped) -
-                                         expected_features[*landmark_it].block(0,from_lmk, 2, N_overlapped)).array();
-
-                    Eigen::ArrayXd dist2 = d.row(0).pow(2) + d.row(1).pow(2);
-                    //std::cout << "\t\t\tsquared distances = " << dist2.transpose() << std::endl;
-
-                    if (offset != min_offset && offset != max_offset)
-                    {
-                        // Point-to-line first distance
-                        bool from_ftr_not_defined = (from_ftr == 0 && !polyline_feature->isFirstDefined());
-                        bool from_lmk_not_defined = (from_lmk == 0 && !polyline_landmark->isFirstDefined());
-                        //std::cout << "\t\tfrom_ftr_not_defined " << from_ftr_not_defined << (from_ftr == 0) << !polyline_feature->isFirstDefined() << std::endl;
-                        //std::cout << "\t\tfrom_lmk_not_defined " << from_lmk_not_defined << (from_lmk == 0) << !polyline_landmark->isFirstDefined() << std::endl;
-                        if (from_ftr_not_defined || from_lmk_not_defined)
-                        {
-                            //std::cout << "\t\t\tFirst feature not defined distance to line" << std::endl;
-                            //std::cout << "\t\t\tA" << expected_features[*landmark_it].col(from_lmk).transpose() << std::endl;
-                            //std::cout << "\t\t\tAaux" << expected_features[*landmark_it].col(from_lmk+1).transpose() << std::endl;
-                            //std::cout << "\t\t\tB" << polyline_feature->getPoints().col(from_ftr).transpose() << std::endl;
-                            dist2(0) = sqDistPointToLine(expected_features[*landmark_it].col(from_lmk),
-                                                         expected_features[*landmark_it].col(from_lmk+1),
-                                                         polyline_feature->getPoints().col(from_ftr),
-                                                         !from_lmk_not_defined,
-                                                         !from_ftr_not_defined);
-                        }
-
-                        // Point-to-line last distance
-                        bool last_ftr_not_defined = !polyline_feature->isLastDefined() && to_ftr == max_ftr;
-                        bool last_lmk_not_defined = !polyline_landmark->isLastDefined() && to_lmk == max_lmk;
-                        //std::cout << "\t\tlast_ftr_not_defined " << last_ftr_not_defined << (to_ftr == max_ftr) << !polyline_feature->isLastDefined() << std::endl;
-                        //std::cout << "\t\tlast_lmk_not_defined " << last_lmk_not_defined << (to_lmk == max_lmk) << !polyline_landmark->isLastDefined() << std::endl;
-                        if (last_ftr_not_defined || last_lmk_not_defined)
-                        {
-                            //std::cout << "\t\t\tLast feature not defined distance to line" << std::endl;
-                            //std::cout << "\t\t\tA" << expected_features[*landmark_it].col(to_lmk).transpose() << std::endl;
-                            //std::cout << "\t\t\tAaux" << expected_features[*landmark_it].col(to_lmk-1).transpose() << std::endl;
-                            //std::cout << "\t\t\tB" << polyline_feature->getPoints().col(to_ftr).transpose() << std::endl;
-                            dist2(N_overlapped-1) = sqDistPointToLine(expected_features[*landmark_it].col(to_lmk),
-                                                                      expected_features[*landmark_it].col(to_lmk-1),
-                                                                      polyline_feature->getPoints().col(to_ftr),
-                                                                      !last_lmk_not_defined,
-                                                                      !last_ftr_not_defined);
-                        }
-                    }
-                    //std::cout << "\t\t\tsquared distances = " << dist2.transpose() << std::endl;
-
-                    // All squared distances should be witin a threshold
-                    // Choose the most overlapped one
-                    if ((dist2 < params_->position_error_th*params_->position_error_th).all() && (best_match == nullptr ||
-                                                                                  (N_overlapped >= best_match->feature_match_to_id_-best_match->feature_match_from_id_+1 &&
-                                                                                   dist2.mean() < best_match->normalized_score_ )))
-                    {
-                        //std::cout << "BEST MATCH" << std::endl;
-                        best_match = std::make_shared<LandmarkPolylineMatch>();
-                        best_match->feature_match_from_id_= from_ftr;
-                        best_match->landmark_match_from_id_= from_lmk+polyline_landmark->getFirstId();
-                        best_match->feature_match_to_id_= from_ftr+N_overlapped-1;
-                        best_match->landmark_match_to_id_= from_lmk+N_overlapped-1+polyline_landmark->getFirstId();
-                        best_match->landmark_ptr_=polyline_landmark;
-                        best_match->normalized_score_ = dist2.mean();
-                    }
-                }
-            }
-            // Closed landmark polyline
-            else
-            {
-                if (polyline_feature->getNPoints() > polyline_landmark->getNPoints())
-                    continue;
-
-                //std::cout << "MATCHING WITH CLOSED LANDMARK" << std::endl;
-                //std::cout << "\tfeature  " << polyline_feature->id() << ": 0-" << max_ftr << std::endl;
-                //std::cout << "\tlandmark " << polyline_landmark->id() << ": 0-" << polyline_landmark->getNPoints() - 1 << std::endl;
-
-                max_offset = 0;
-                min_offset = -polyline_landmark->getNPoints() + 1;
-
-                // Check all overlapping positions between each feature-landmark pair
-                for (offset = min_offset; offset <= max_offset; offset++)
-                {
-                    from_lmk = -offset;
-                    to_lmk = from_lmk+polyline_feature->getNPoints()-1;
-                    if (to_lmk >= polyline_landmark->getNPoints())
-                        to_lmk -= polyline_landmark->getNPoints();
-
-                    //std::cout << "\t\toffset " << offset << std::endl;
-                    //std::cout << "\t\t\tfrom_lmk " << from_lmk << std::endl;
-                    //std::cout << "\t\t\tto_lmk " << to_lmk << std::endl;
-
-                    // Compute the squared distance for all overlapped points
-                    Eigen::ArrayXXd d = polyline_feature->getPoints().topRows(2).array();
-                    if (to_lmk > from_lmk)
-                        d -= expected_features[*landmark_it].block(0,from_lmk, 2, polyline_feature->getNPoints()).array();
-                    else
-                    {
-                        d.leftCols(polyline_landmark->getNPoints()-from_lmk) -= expected_features[*landmark_it].block(0,from_lmk, 2, polyline_landmark->getNPoints()-from_lmk).array();
-                        d.rightCols(to_lmk+1) -= expected_features[*landmark_it].block(0, 0, 2, to_lmk+1).array();
-                    }
-                    Eigen::ArrayXd dist2 = d.row(0).pow(2) + d.row(1).pow(2);
-                    //std::cout << "\t\t\tsquared distances = " << dist2.transpose() << std::endl;
-
-                    // Point-to-line first distance
-                    if (!polyline_feature->isFirstDefined())
-                    {
-                        int next_from_lmk = (from_lmk+1 == polyline_landmark->getNPoints() ? 0 : from_lmk+1);
-                        dist2(0) = sqDistPointToLine(expected_features[*landmark_it].col(from_lmk),
-                                                     expected_features[*landmark_it].col(next_from_lmk),
-                                                     polyline_feature->getPoints().col(0),
-                                                     true,
-                                                     false);
-                    }
-
-                    // Point-to-line last distance
-                    if (!polyline_feature->isLastDefined())
-                    {
-                        int prev_to_lmk = (to_lmk == 0 ? polyline_landmark->getNPoints()-1 : to_lmk-1);
-                        dist2(polyline_feature->getNPoints()-1) = sqDistPointToLine(expected_features[*landmark_it].col(to_lmk),
-                                                                                    expected_features[*landmark_it].col(prev_to_lmk),
-                                                                                    polyline_feature->getPoints().col(polyline_feature->getNPoints()-1),
-                                                                                    true,
-                                                                                    false);
-                    }
-                    //std::cout << "\t\t\tsquared distances = " << dist2.transpose() << std::endl;
-
-                    // All squared distances should be witin a threshold
-                    // Choose the most overlapped one
-                    if ((dist2 < params_->position_error_th*params_->position_error_th).all() && (best_match == nullptr || dist2.mean() < best_match->normalized_score_ ))
-                    {
-                        //std::cout << "BEST MATCH" << std::endl;
-                        best_match = std::make_shared<LandmarkPolylineMatch>();
-                        best_match->feature_match_from_id_= 0;
-                        best_match->landmark_match_from_id_= from_lmk+polyline_landmark->getFirstId();
-                        best_match->feature_match_to_id_= polyline_feature->getNPoints()-1;
-                        best_match->landmark_match_to_id_= to_lmk+polyline_landmark->getFirstId();
-                        best_match->landmark_ptr_=polyline_landmark;
-                        best_match->normalized_score_ = dist2.mean();
-                    }
-                }
-            }
-
-            //std::cout << "landmark " << (*landmark_it)->id() << ": 0-" << max_lmk << " - defined " << polyline_landmark->isFirstDefined() << polyline_landmark->isLastDefined() << std::endl;
-            //std::cout << "feature " << (*feature_it)->id() << ": 0-" << max_ftr << " - defined " << polyline_feature->isFirstDefined() << polyline_feature->isLastDefined() << std::endl;
-            //std::cout << expected_features[*landmark_it] << std::endl;
-            //std::cout << "\tmax_offset " << max_offset << std::endl;
-            //std::cout << "\tmin_offset " << min_offset << std::endl;
-            //if (!polyline_landmark->isFirstDefined() && !polyline_feature->isLastDefined())
-            //    std::cout << "\tLIMITED max offset " << max_offset << std::endl;
-            //if (!polyline_feature->isFirstDefined() && !polyline_landmark->isLastDefined())
-            //    std::cout << "\tLIMITED min offset " << min_offset << std::endl;
-
-        }
-        // Match found for this feature
-        if (best_match != nullptr)
-        {
-            //std::cout << "\tclosest landmark: " << best_match->landmark_ptr_->id() << std::endl;
-            // match
-            matches_landmark_from_incoming_[*feature_it] = best_match;
-            // move corner feature to output list
-            _features_found.splice(_features_found.end(), polylines_incoming_, feature_it);
-            // reset match
-            best_match = nullptr;
-        }
-        //else
-        //{
-        //    std::cout << "\t-------------------------->NO LANDMARK CLOSE ENOUGH!!!!" << std::endl;
-        //    std::getchar();
-        //}
-        feature_it = next_feature_it++;
-    }
-    return matches_landmark_from_incoming_.size();
-}
-
-bool ProcessorTrackerLandmarkPolyline::voteForKeyFrame()
-{
-    //std::cout << "------------- ProcessorTrackerLandmarkPolyline::voteForKeyFrame:" << std::endl;
-    //std::cout << "polylines_last_.size():" << polylines_last_.size()<< std::endl;
-    // option 1: more than TH new features in last
-    if (polylines_last_.size() >= params_->new_features_th)
-    {
-        std::cout << "------------- NEW KEY FRAME: Option 1 - Enough new features" << std::endl;
-        //std::cout << "\tnew features in last = " << corners_last_.size() << std::endl;
-        return true;
-    }
-    // option 2: loop closure (if the newest frame from which a matched landmark was observed is old enough)
-    for (auto new_ftr : new_features_last_)
-    {
-        if (last_ptr_->getFrame()->id() - matches_landmark_from_last_[new_ftr]->landmark_ptr_->getConstrainedByList().back()->getCapture()->getFrame()->id() > params_->loop_frames_th)
-        {
-            std::cout << "------------- NEW KEY FRAME: Option 2 - Loop closure" << std::endl;
-            return true;
-        }
-    }
-    return false;
-}
-
-void ProcessorTrackerLandmarkPolyline::extractPolylines(CaptureLaser2DPtr _capture_laser_ptr,
-                                                        FeatureBasePtrList& _polyline_list)
-{
-    //std::cout << "ProcessorTrackerLandmarkPolyline::extractPolylines: " << std::endl;
-    // TODO: sort corners by bearing
-    std::list<laserscanutils::Polyline> polylines;
-
-    line_finder_.findPolylines(_capture_laser_ptr->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(), polylines);
-
-    for (auto&& pl : polylines)
-    {
-        //std::cout << "new polyline detected: Defined" << pl.first_defined_ << pl.last_defined_ << std::endl;
-        //std::cout << "covs: " << std::endl << pl.covs_ << std::endl;
-        _polyline_list.push_back(std::make_shared<FeaturePolyline2D>(pl.points_, pl.covs_, pl.first_defined_, pl.last_defined_));
-        //std::cout << "new polyline detected: " << std::endl;
-    }
-    //std::cout << _polyline_list.size() << " polylines extracted" << std::endl;
-}
-
-void ProcessorTrackerLandmarkPolyline::expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_,
-                                                   Eigen::MatrixXs& expected_feature_cov_)
-{
-    assert(_landmark_ptr->getType() == "POLYLINE 2D" && "ProcessorTrackerLandmarkPolyline::expectedFeature: Bad landmark type");
-    LandmarkPolyline2DPtr polyline_landmark = std::static_pointer_cast<LandmarkPolyline2D>(_landmark_ptr);
-    assert(expected_feature_.cols() == polyline_landmark->getNPoints() && expected_feature_.rows() == 3 && "ProcessorTrackerLandmarkPolyline::expectedFeature: bad expected_feature_ sizes");
-
-    //std::cout << "ProcessorTrackerLandmarkPolyline::expectedFeature" << std::endl;
-    //std::cout << "t_world_sensor_: " << t_world_sensor_.transpose() << std::endl;
-    //std::cout << "R_sensor_world_: "  << std::endl << R_sensor_world_ << std::endl;
-
-    expected_feature_ = Eigen::MatrixXs::Zero(3,polyline_landmark->getNPoints());
-    expected_feature_cov_ = Eigen::MatrixXs::Zero(2,2*polyline_landmark->getNPoints());
-    Eigen::Vector3s col = Eigen::Vector3s::Ones();
-
-    ////////// global coordinates points
-    if (polyline_landmark->getClassification() == UNCLASSIFIED)
-        for (auto i = 0; i < polyline_landmark->getNPoints(); i++)
-        {
-            //std::cout << "Point " << i+polyline_landmark->getFirstId() << std::endl;
-            //std::cout << "First Point " << polyline_landmark->getFirstId() << std::endl;
-            //std::cout << "Landmark global position: " << polyline_landmark->getPointVector(i+polyline_landmark->getFirstId()).transpose() << std::endl;
-
-            // ------------ expected feature point
-            col.head<2>() = R_sensor_world_ * (polyline_landmark->getPointVector(i+polyline_landmark->getFirstId()) - t_world_sensor_);
-            expected_feature_.col(i) = col;
-
-            //std::cout << "Expected point " << i << ": " << expected_feature_.col(i).transpose() << std::endl;
-            // ------------ expected feature point covariance
-            // TODO
-            expected_feature_cov_.middleCols(i*2, 2) = Eigen::MatrixXs::Identity(2,2);
-        }
-
-    ////////// landmark with origin
-    else
-    {
-        Eigen::Matrix2s R_world_points = Eigen::Rotation2Ds(polyline_landmark->getO()->getState()(0)).matrix();
-        const Eigen::VectorXs& t_world_points = polyline_landmark->getP()->getState();
-
-        for (auto i = 0; i < polyline_landmark->getNPoints(); i++)
-        {
-            //std::cout << "Point " << i+polyline_landmark->getFirstId() << std::endl;
-            //std::cout << "First Point " << polyline_landmark->getFirstId() << std::endl;
-            //std::cout << "Landmark global position: " << polyline_landmark->getPointVector(i+polyline_landmark->getFirstId()).transpose() << std::endl;
-
-            // ------------ expected feature point
-            col.head<2>() = R_sensor_world_ * (R_world_points * polyline_landmark->getPointVector(i+polyline_landmark->getFirstId()) + t_world_points - t_world_sensor_);
-            expected_feature_.col(i) = col;
-
-            //std::cout << "Expected point " << i << ": " << expected_feature_.col(i).transpose() << std::endl;
-            // ------------ expected feature point covariance
-            // TODO
-            expected_feature_cov_.middleCols(i*2, 2) = Eigen::MatrixXs::Identity(2,2);
-        }
-    }
-}
-
-Eigen::VectorXs ProcessorTrackerLandmarkPolyline::computeSquaredMahalanobisDistances(const Eigen::Vector2s& _feature,
-                                                                                     const Eigen::Matrix2s& _feature_cov,
-                                                                                     const Eigen::Vector2s& _expected_feature,
-                                                                                     const Eigen::Matrix2s& _expected_feature_cov,
-                                                                                     const Eigen::MatrixXs& _mu)
-{
-    // ------------------------ d
-    Eigen::Vector2s d = _feature - _expected_feature;
-
-    // ------------------------ Sigma_d
-    Eigen::Matrix2s iSigma_d = (_feature_cov + _expected_feature_cov).inverse();
-    Eigen::VectorXs squared_mahalanobis_distances(_mu.cols());
-    for (unsigned int i = 0; i < _mu.cols(); i++)
-    {
-        squared_mahalanobis_distances(i) = (d - _mu.col(i)).transpose() * iSigma_d * (d - _mu.col(i));
-        //if ((d - _mu.col(i)).norm() < 1)
-        //{
-        //    std::cout << "distance:    " << (d - _mu.col(i)).norm() << std::endl;
-        //    std::cout << "iSigma_d:    " << std::endl << iSigma_d << std::endl;
-        //    std::cout << "mahalanobis: " << squared_mahalanobis_distances(i) << std::endl;
-        //}
-    }
-
-    return squared_mahalanobis_distances;
-}
-
-Scalar ProcessorTrackerLandmarkPolyline::sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux,
-                                                         const Eigen::Vector3s& _B, bool _A_defined, bool _B_defined)
-{
-    /* Squared distance from B to the line A_aux-A (match evaluated is A-B)
-     *
-     * No matter if A_aux is defined or not.
-     *
-     * Case 1: B not defined
-     *
-     *      The projection of B over the line AAaux must be in [A_aux, inf(in A direction)). Otherwise, return squared distance Aaux-B.
-     *      Check: the angle BAauxA is <= 90º:
-     *          (BA)² <= (BAaux)² + (AAaux)²
-     *
-     *      Case 1.1: A not defined
-     *
-     *          No more restrictions: return distance line to point.
-     *
-     *      Case 1.2: A defined
-     *
-     *          The projection of B over the line AAaux must be in (inf(in Aaux direction), A]. Otherwise, return squared distance A-B.
-     *          Additional check: the angle BAAaux is <= 90º:
-     *              (BAaux)² <= (BA)² + (AAaux)²
-     *
-     * Case 2: B is defined and A is not
-     *
-     *      Returns the distance B-Line if the projection of B to the line is in [A, inf). Otherwise, return squared distance A-B.
-     *      Checks if the angle BAAaux is >=  90º: (BAaux)² >= (BA)² + (AAaux)²
-     *
-     * ( Case B and A are defined is not point-to-line, is point to point -> assertion )
-     *
-     */
-
-    assert((!_A_defined || !_B_defined) && "ProcessorTrackerLandmarkPolyline::sqDistPointToLine: at least one point must not be defined.");
-
-    Scalar AB_sq = (_B-_A).head<2>().squaredNorm();
-    Scalar AauxB_sq = (_B-_A_aux).head<2>().squaredNorm();
-    Scalar AAaux_sq = (_A_aux-_A).head<2>().squaredNorm();
-
-    // Case 1
-    if (!_B_defined)
-    {
-        if (AB_sq <= AauxB_sq + AAaux_sq)
-        {
-            // Case 1.1
-            if (!_A_defined)
-                return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq; //squared distance to line
-            // Case 1.2
-            else if (AauxB_sq <= AB_sq + AAaux_sq)
-                return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq; //squared distance to line
-        }
-    }
-    // Case 2
-    else if (!_A_defined && _B_defined)
-        if (AauxB_sq >= AB_sq + AAaux_sq)
-            return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq; //squared distance to line
-
-    // Default return A-B squared distance
-    return (_A.head<2>() - _B.head<2>()).squaredNorm();
-}
-
-void ProcessorTrackerLandmarkPolyline::createNewLandmarks()
-{
-    //std::cout << "ProcessorTrackerLandmarkPolyline::createNewLandmarks" << std::endl;
-    FeaturePolyline2DPtr polyline_ft_ptr;
-    LandmarkBasePtr new_lmk_ptr;
-    for (auto new_feature_ptr : new_features_last_)
-    {
-        // create new landmark
-        new_lmk_ptr = createLandmark(new_feature_ptr);
-        //std::cout << "\tfeature: " << new_feature_ptr->id() << std::endl;
-        //std::cout << "\tnew_landmark: " << new_lmk_ptr->id() << ": "<< ((LandmarkPolyline2D*)new_lmk_ptr)->getNPoints() << " points" << std::endl;
-        new_landmarks_.push_back(new_lmk_ptr);
-        // cast
-        polyline_ft_ptr = std::static_pointer_cast<FeaturePolyline2D>(new_feature_ptr);
-        // create new correspondence
-        LandmarkPolylineMatchPtr match = std::make_shared<LandmarkPolylineMatch>();
-        match->feature_match_from_id_= 0; // all points match
-        match->landmark_match_from_id_ = 0;
-        match->feature_match_to_id_= polyline_ft_ptr->getNPoints()-1; // all points match
-        match->landmark_match_to_id_ = polyline_ft_ptr->getNPoints()-1;
-        match->normalized_score_ = 1.0; // max score
-        match->landmark_ptr_ = new_lmk_ptr;
-        matches_landmark_from_last_[new_feature_ptr] = match;
-    }
-}
-
-LandmarkBasePtr ProcessorTrackerLandmarkPolyline::createLandmark(FeatureBasePtr _feature_ptr)
-{
-    assert(_feature_ptr->getType() == "POLYLINE 2D");
-    //std::cout << "ProcessorTrackerLandmarkPolyline::createLandmark" << std::endl;
-    //std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl;
-    //std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl;
-
-    FeaturePolyline2DPtr polyline_ptr = std::static_pointer_cast<FeaturePolyline2D>(_feature_ptr);
-    // compute feature global pose
-    Eigen::MatrixXs points_global = R_world_sensor_ * polyline_ptr->getPoints().topRows<2>() +
-                                    t_world_sensor_ * Eigen::VectorXs::Ones(polyline_ptr->getNPoints()).transpose();
-
-    //std::cout << "Feature local points: " << std::endl << polyline_ptr->getPoints().topRows<2>() << std::endl;
-    //std::cout << "Landmark global points: " << std::endl << points_global << std::endl;
-    //std::cout << "New landmark: extremes defined " << polyline_ptr->isFirstDefined() << polyline_ptr->isLastDefined() << std::endl;
-
-    // Create new landmark
-    return std::make_shared<LandmarkPolyline2D>(std::make_shared<StateBlock>(Eigen::Vector2s::Zero(), true), std::make_shared<StateBlock>(Eigen::Vector1s::Zero(), true), points_global, polyline_ptr->isFirstDefined(), polyline_ptr->isLastDefined());
-}
-
-ProcessorTrackerLandmarkPolyline::~ProcessorTrackerLandmarkPolyline()
-{
-    while (!polylines_last_.empty())
-    {
-        polylines_last_.front()->remove();
-        polylines_last_.pop_front(); // TODO see if this is needed
-    }
-    while (!polylines_incoming_.empty())
-    {
-        polylines_incoming_.front()->remove();
-        polylines_incoming_.pop_front(); // TODO see if this is needed
-    }
-}
-
-void ProcessorTrackerLandmarkPolyline::establishFactors()
-{
-	//TODO: update with new index in landmarks
-
-    //std::cout << "ProcessorTrackerLandmarkPolyline::establishFactors" << std::endl;
-    LandmarkPolylineMatchPtr polyline_match;
-    FeaturePolyline2DPtr polyline_feature;
-    LandmarkPolyline2DPtr polyline_landmark;
-
-    for (auto last_feature : last_ptr_->getFeatureList())
-    {
-        polyline_feature = std::static_pointer_cast<FeaturePolyline2D>(last_feature);
-        polyline_match = std::static_pointer_cast<LandmarkPolylineMatch>(matches_landmark_from_last_[last_feature]);
-        polyline_landmark = std::static_pointer_cast<LandmarkPolyline2D>(polyline_match->landmark_ptr_);
-
-        assert(polyline_landmark != nullptr && polyline_match != nullptr);
-
-        // Modify landmark (only for not closed)
-        if (!polyline_landmark->isClosed())
-        {
-            //std::cout << std::endl << "MODIFY LANDMARK" << std::endl;
-            //std::cout << "feature " << polyline_feature->id() << ": " << std::endl;
-            //std::cout << "\tpoints " << polyline_feature->getNPoints() << std::endl;
-            //std::cout << "\tfirst defined " << polyline_feature->isFirstDefined() << std::endl;
-            //std::cout << "\tlast defined " << polyline_feature->isLastDefined() << std::endl;
-            //std::cout << "landmark " << polyline_landmark->id() << ": " << std::endl;
-            //std::cout << "\tpoints " << polyline_landmark->getNPoints() << std::endl;
-            //std::cout << "\tfirst defined " << polyline_landmark->isFirstDefined() << std::endl;
-            //std::cout << "\tlast defined " << polyline_landmark->isLastDefined() << std::endl << std::endl;
-            //std::cout << "\tmatch from feature point " << polyline_match->feature_match_from_id_ << std::endl;
-            //std::cout << "\tmatch to feature point " << polyline_match->feature_match_to_id_ << std::endl;
-            //std::cout << "\tmatch from landmark point " << polyline_match->landmark_match_from_id_ << std::endl;
-            //std::cout << "\tmatch to landmark point " << polyline_match->landmark_match_to_id_ << std::endl;
-
-            Eigen::MatrixXs points_global = R_world_sensor_ * polyline_feature->getPoints().topRows<2>() +
-                                            t_world_sensor_ * Eigen::VectorXs::Ones(polyline_feature->getNPoints()).transpose();
-            // GROW/CLOSE LANDMARK
-            // -----------------Front-----------------
-            bool check_front_closing = // Sufficient conditions
-                                       // condition 1: feature first defined point not matched
-                                       (polyline_feature->isFirstDefined() && polyline_match->feature_match_from_id_ > 0) ||
-                                       // condition 2: feature second point not matched
-                                       (polyline_match->feature_match_from_id_ > 1) ||
-                                       // condition 3: matched front points but feature front point defined and landmark front point not defined
-                                       (polyline_match->landmark_match_from_id_ == polyline_landmark->getFirstId() && polyline_feature->isFirstDefined() && !polyline_landmark->isFirstDefined());
-
-            // Check closing with landmark's last points
-            if (check_front_closing)
-            {
-                //std::cout << "---------------- Trying to close polyline..." << std::endl;
-                //std::cout << "feature " << polyline_feature->id() << ": " << std::endl;
-                //std::cout << "\tpoints " << polyline_feature->getNPoints() << std::endl;
-                //std::cout << "\tfirst defined " << polyline_feature->isFirstDefined() << std::endl;
-                //std::cout << "\tlast defined " << polyline_feature->isLastDefined() << std::endl;
-                //std::cout << "\tmatch from feature point " << polyline_match->feature_match_from_id_ << std::endl;
-                //std::cout << "\tmatch to feature point " << polyline_match->feature_match_to_id_ << std::endl;
-                //std::cout << "landmark " << polyline_landmark->id() << ": " << std::endl;
-                //std::cout << "\tpoints " << polyline_landmark->getNPoints() << std::endl;
-                //std::cout << "\tfirst defined " << polyline_landmark->isFirstDefined() << std::endl;
-                //std::cout << "\tlast defined " << polyline_landmark->isLastDefined() << std::endl << std::endl;
-                //std::cout << "\tmatch from landmark point " << polyline_match->landmark_match_from_id_ << std::endl;
-                //std::cout << "\tmatch to landmark point " << polyline_match->landmark_match_to_id_ << std::endl;
-
-                int feat_point_id_matching = polyline_feature->isFirstDefined() ? 0 : 1;
-                int lmk_last_defined_point = polyline_landmark->getLastId() - (polyline_landmark->isLastDefined() ? 0 : 1);
-                //std::cout << std::endl << "\tfeat point matching " << feat_point_id_matching << std::endl;
-                //std::cout << std::endl << "\tlmk last defined point " << lmk_last_defined_point << std::endl;
-
-                for (int id_lmk = lmk_last_defined_point; id_lmk > polyline_match->landmark_match_to_id_; id_lmk--)
-                {
-                    //std::cout << "\t\tid_lmk " << id_lmk << std::endl;
-                    //std::cout << "\t\td2 = " << (points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() << " (th = " << params_->position_error_th*params_->position_error_th << std::endl;
-
-                    if ((points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() < params_->position_error_th*params_->position_error_th)
-                    {
-                        std::cout << "CLOSING POLYLINE" << std::endl;
-
-                        unsigned int N_back_overlapped = polyline_landmark->getLastId() - id_lmk + 1;
-                        int N_feature_new_points = polyline_match->feature_match_from_id_ - feat_point_id_matching - N_back_overlapped;
-
-                        // define first point (if not defined and no points have to be merged)
-                        if (!polyline_landmark->isFirstDefined() && N_feature_new_points >= 0)
-                            polyline_landmark->setFirst(points_global.col(polyline_match->feature_match_from_id_), true);
-
-                        // add other points (if there are)
-                        if (N_feature_new_points > 0)
-                            polyline_landmark->addPoints(points_global.middleCols(feat_point_id_matching + N_back_overlapped, N_feature_new_points), // points matrix in global coordinates
-                                                         N_feature_new_points-1, // last index to be added
-                                                         true, // defined
-                                                         false); // front (!back)
-
-                        // define last point (if not defined and no points have to be merged)
-                        if (!polyline_landmark->isLastDefined() && N_feature_new_points >= 0)
-                            polyline_landmark->setLast(points_global.col(feat_point_id_matching + N_back_overlapped - 1), true);
-
-                        // close landmark
-                        polyline_landmark->setClosed();
-
-                        polyline_match->landmark_match_from_id_ = id_lmk - (polyline_feature->isFirstDefined() ? 0 : 1);
-                        polyline_match->feature_match_from_id_ = 0;
-                        break;
-                    }
-                }
-            }
-            // Add new front points (if not matched feature points)
-            if (polyline_match->feature_match_from_id_ > 0) // && !polyline_landmark->isClosed()
-            {
-                assert(!polyline_landmark->isClosed() && "feature not matched points in a closed landmark");
-                //std::cout << "Add new front points. Defined: " << polyline_feature->isFirstDefined() << std::endl;
-                //std::cout << "\tfeat from " << polyline_match->feature_match_from_id_ << std::endl;
-                //std::cout << "\tfeat to " << polyline_match->feature_match_to_id_ << std::endl;
-                //std::cout << "\tland from " << polyline_match->landmark_match_from_id_ << std::endl;
-                //std::cout << "\tland to " << polyline_match->landmark_match_to_id_ << std::endl;
-
-                polyline_landmark->addPoints(points_global, // points matrix in global coordinates
-                                             polyline_match->feature_match_from_id_-1, // last feature point index to be added
-                                             polyline_feature->isFirstDefined(), // is defined
-                                             false); // front
-
-                polyline_match->landmark_match_from_id_ = polyline_landmark->getFirstId();
-                polyline_match->feature_match_from_id_ = 0;
-                //std::cout << "\tfeat from " << polyline_match->feature_match_from_id_ << std::endl;
-                //std::cout << "\tfeat to " << polyline_match->feature_match_to_id_ << std::endl;
-                //std::cout << "\tland from " << polyline_match->landmark_match_from_id_ << std::endl;
-                //std::cout << "\tland to " << polyline_match->landmark_match_to_id_ << std::endl;
-            }
-            // Change first point
-            else if (polyline_match->landmark_match_from_id_ == polyline_landmark->getFirstId() && !polyline_landmark->isFirstDefined())
-            {
-                //std::cout << "Change first point. Defined: " << polyline_feature->isFirstDefined() << std::endl;
-                //std::cout << "\tpoint " << polyline_landmark->getFirstId() << ": " << polyline_landmark->getPointVector(polyline_landmark->getFirstId()).transpose() << std::endl;
-                //std::cout << "\tpoint " << polyline_landmark->getFirstId()+1 << ": " << polyline_landmark->getPointVector(polyline_landmark->getFirstId()+1).transpose() << std::endl;
-                //std::cout << "\tfeature point: " << points_global.topLeftCorner(2,1).transpose() << std::endl;
-
-                if (// new defined first
-                    polyline_feature->isFirstDefined() ||
-                    // new not defined first
-                    (points_global.topLeftCorner(2,1)-polyline_landmark->getPointVector(polyline_landmark->getFirstId()+1)).squaredNorm() >
-                    (points_global.topLeftCorner(2,1)-polyline_landmark->getPointVector(polyline_landmark->getFirstId())).squaredNorm() +
-                    (polyline_landmark->getPointVector(polyline_landmark->getFirstId()+1)-polyline_landmark->getPointVector(polyline_landmark->getFirstId())).squaredNorm())
-                    polyline_landmark->setFirst(points_global.leftCols(1), polyline_feature->isFirstDefined());
-
-            }
-            // -----------------Back-----------------
-            bool check_back_closing = // Sufficient conditions
-                                      // condition 1: feature last defined point not matched
-                                      (polyline_feature->isLastDefined() && polyline_match->feature_match_to_id_ < polyline_feature->getNPoints()-1) ||
-                                      // condition 2: feature second last point not matched
-                                      (polyline_match->feature_match_to_id_ < polyline_feature->getNPoints() - 2) ||
-                                      // condition 3: matched back points but feature last point defined and landmark last point not defined
-                                      (polyline_match->landmark_match_to_id_ == polyline_landmark->getLastId() && polyline_feature->isLastDefined() && !polyline_landmark->isLastDefined());
-
-            // Necessary condition: still open landmark
-            check_back_closing = check_back_closing && !polyline_landmark->isClosed();
-
-            // Check closing with landmark's first points
-            if (check_back_closing)
-            {
-                int feat_point_id_matching = polyline_feature->getNPoints() - (polyline_feature->isLastDefined() ? 1 : 2);
-                int lmk_first_defined_point = polyline_landmark->getFirstId() + (polyline_landmark->isFirstDefined() ? 0 : 1);
-                //std::cout << std::endl << "\tfeat point matching " << feat_point_id_matching << std::endl;
-                //std::cout << std::endl << "\tlmk first defined point " << lmk_first_defined_point << std::endl;
-
-                for (int id_lmk = lmk_first_defined_point; id_lmk < polyline_match->landmark_match_from_id_; id_lmk++)
-                {
-                    //std::cout << "\t\tid_lmk " << id_lmk << std::endl;
-                    //std::cout << "\t\td2 = " << (points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() << " (th = " << params_->position_error_th*params_->position_error_th << std::endl;
-
-                    if ((points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() < params_->position_error_th*params_->position_error_th)
-                    {
-                        std::cout << "CLOSING POLYLINE" << std::endl;
-
-                        unsigned int N_front_overlapped = id_lmk - polyline_landmark->getFirstId() + 1;
-                        int N_feature_new_points = feat_point_id_matching - polyline_match->feature_match_to_id_ - N_front_overlapped;
-
-                        // define first point (if not defined and no points have to be merged)
-                        if (!polyline_landmark->isFirstDefined() && N_feature_new_points >= 0)
-                            polyline_landmark->setFirst(points_global.col(feat_point_id_matching - N_front_overlapped + 1), true);
-
-                        // add other points (if there are)
-                        if (N_feature_new_points > 0)
-                            polyline_landmark->addPoints(points_global.middleCols(polyline_match->feature_match_to_id_ + 1, N_feature_new_points), // points matrix in global coordinates
-                                                         N_feature_new_points-1, // last index to be added
-                                                         true, // defined
-                                                         false); // front (!back)
-
-                        // define last point (if not defined and no points have to be merged)
-                        if (!polyline_landmark->isLastDefined() && N_feature_new_points >= 0)
-                            polyline_landmark->setLast(points_global.col(polyline_match->feature_match_to_id_), true);
-
-                        // close landmark
-                        polyline_landmark->setClosed();
-
-                        polyline_match->landmark_match_to_id_ = id_lmk + (polyline_feature->isLastDefined() ? 0 : 1);
-                        polyline_match->feature_match_to_id_ = polyline_feature->getNPoints() - 1;
-                        break;
-                    }
-                }
-            }
-            // Add new back points (if it wasn't closed)
-            if (polyline_match->feature_match_to_id_ < polyline_feature->getNPoints()-1)
-            {
-                assert(!polyline_landmark->isClosed() && "feature not matched points in a closed landmark");
-                //std::cout << "Add back points. Defined: " << polyline_feature->isLastDefined() << std::endl;
-                //std::cout << "\tfeat from " << polyline_match->feature_match_from_id_ << std::endl;
-                //std::cout << "\tfeat to " << polyline_match->feature_match_to_id_ << std::endl;
-                //std::cout << "\tland from " << polyline_match->landmark_match_from_id_ << std::endl;
-                //std::cout << "\tland to " << polyline_match->landmark_match_to_id_ << std::endl;
-
-                polyline_landmark->addPoints(points_global, // points matrix in global coordinates
-                                             polyline_match->feature_match_to_id_+1, // last feature point index to be added
-                                             polyline_feature->isLastDefined(), // is defined
-                                             true); // back
-
-                polyline_match->landmark_match_to_id_ = polyline_landmark->getLastId();
-                polyline_match->feature_match_to_id_ = polyline_feature->getNPoints()-1;
-                //std::cout << "\tfeat from " << polyline_match->feature_match_from_id_ << std::endl;
-                //std::cout << "\tfeat to " << polyline_match->feature_match_to_id_ << std::endl;
-                //std::cout << "\tland from " << polyline_match->landmark_match_from_id_ << std::endl;
-                //std::cout << "\tland to " << polyline_match->landmark_match_to_id_ << std::endl;
-            }
-            // Change last point
-            else if (polyline_match->landmark_match_to_id_ == polyline_landmark->getLastId() && !polyline_landmark->isLastDefined()) //&& polyline_match->feature_match_to_id_ == polyline_feature->getNPoints()-1
-            {
-                //std::cout << "Change last point. Defined: " << (polyline_feature->isLastDefined() ? 1 : 0) << std::endl;
-                //std::cout << "\tpoint " << polyline_landmark->getLastId() << ": " << polyline_landmark->getPointVector(polyline_landmark->getLastId()).transpose() << std::endl;
-                //std::cout << "\tpoint " << polyline_landmark->getLastId()-1 << ": " << polyline_landmark->getPointVector(polyline_landmark->getLastId()-1).transpose() << std::endl;
-                //std::cout << "\tfeature point: " << points_global.topRightCorner(2,1).transpose() << std::endl;
-                if (// new defined last
-                    polyline_feature->isLastDefined() ||
-                    // new not defined last
-                    (points_global.topRightCorner(2,1)-polyline_landmark->getPointVector(polyline_landmark->getLastId()-1)).squaredNorm() >
-                    (points_global.topRightCorner(2,1)-polyline_landmark->getPointVector(polyline_landmark->getLastId())).squaredNorm() +
-                    (polyline_landmark->getPointVector(polyline_landmark->getLastId()-1)-polyline_landmark->getPointVector(polyline_landmark->getLastId())).squaredNorm())
-                    polyline_landmark->setLast(points_global.rightCols(1), polyline_feature->isLastDefined());
-            }
-
-            //std::cout << "MODIFIED LANDMARK" << std::endl;
-            //std::cout << "feature " << polyline_feature->id() << ": " << std::endl;
-            //std::cout << "\tpoints " << polyline_feature->getNPoints() << std::endl;
-            //std::cout << "\tfirst defined " << polyline_feature->isFirstDefined() << std::endl;
-            //std::cout << "\tlast defined " << polyline_feature->isLastDefined() << std::endl;
-            //std::cout << "landmark " << polyline_landmark->id() << ": " << std::endl;
-            //std::cout << "\tpoints " << polyline_landmark->getNPoints() << std::endl;
-            //std::cout << "\tfirst defined " << polyline_landmark->isFirstDefined() << std::endl;
-            //std::cout << "\tlast defined " << polyline_landmark->isLastDefined() << std::endl << std::endl;
-        }
-
-        // ESTABLISH CONSTRAINTS
-        //std::cout << "ESTABLISH CONSTRAINTS" << std::endl;
-        //std::cout << "\tfeature " << polyline_feature->id() << std::endl;
-        //std::cout << "\tlandmark " << polyline_landmark->id() << std::endl;
-        //std::cout << "\tmatch from feature point " << polyline_match->feature_match_from_id_ << std::endl;
-        //std::cout << "\tmatch to feature point " << polyline_match->feature_match_to_id_ << std::endl;
-        //std::cout << "\tmatch from landmark point " << polyline_match->landmark_match_from_id_ << std::endl;
-        //std::cout << "\tmatch to landmark point " << polyline_match->landmark_match_to_id_ << std::endl;
-
-        // Factors for all inner and defined feature points
-        int lmk_point_id = polyline_match->landmark_match_from_id_;
-
-        for (int ftr_point_id = 0; ftr_point_id < polyline_feature->getNPoints(); ftr_point_id++, lmk_point_id++)
-        {
-            if (lmk_point_id > polyline_landmark->getLastId())
-                lmk_point_id -= polyline_landmark->getNPoints();
-            if (lmk_point_id < polyline_landmark->getFirstId())
-                lmk_point_id += polyline_landmark->getNPoints();
-
-            //std::cout << "feature point " << ftr_point_id << std::endl;
-            //std::cout << "landmark point " << lmk_point_id << std::endl;
-
-            // First not defined point
-            if (ftr_point_id == 0 && !polyline_feature->isFirstDefined())
-                // first point to line factor
-            {
-                int lmk_next_point_id = lmk_point_id+1;
-                if (lmk_next_point_id > polyline_landmark->getLastId())
-                    lmk_next_point_id -= polyline_landmark->getNPoints();
-                //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_next_point_id << std::endl;
-                last_feature->addFactor(std::make_shared<FactorPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(),
-                                                                                      ftr_point_id, lmk_point_id, lmk_next_point_id));
-                //std::cout << "factor added" << std::endl;
-            }
-
-            // Last not defined point
-            else if (ftr_point_id == polyline_feature->getNPoints()-1 && !polyline_feature->isLastDefined())
-                // last point to line factor
-            {
-                int lmk_prev_point_id = lmk_point_id-1;
-                if (lmk_prev_point_id < polyline_landmark->getFirstId())
-                    lmk_prev_point_id += polyline_landmark->getNPoints();
-                //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_prev_point_id << std::endl;
-                last_feature->addFactor(std::make_shared<FactorPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(),
-                                                                                      ftr_point_id, lmk_point_id, lmk_prev_point_id));
-                //std::cout << "factor added" << std::endl;
-            }
-
-            // Defined point
-            else
-                // point to point factor
-            {
-                //std::cout << "point-point: landmark point " << lmk_point_id << std::endl;
-				//std::cout << "landmark first id:" << polyline_landmark->getFirstId() << std::endl;
-				//std::cout << "landmark last id:" << polyline_landmark->getLastId() << std::endl;
-				//std::cout << "landmark n points:" << polyline_landmark->getNPoints() << std::endl;
-                last_feature->addFactor(std::make_shared<FactorPoint2D>(polyline_feature, polyline_landmark, shared_from_this(),
-                                                                                ftr_point_id, lmk_point_id));
-                //std::cout << "factor added" << std::endl;
-            }
-        }
-        //std::cout << "Factors established" << std::endl;
-    }
-}
-
-void ProcessorTrackerLandmarkPolyline::classifyPolilines(LandmarkBasePtrList& _lmk_list)
-{
-    //std::cout << "ProcessorTrackerLandmarkPolyline::classifyPolilines: " << _lmk_list->size() << std::endl;
-    std::vector<Scalar> object_L({12, 5.9, 1.2});
-    std::vector<Scalar> object_W({2.345, 2.345, 0.9});
-    std::vector<Scalar> object_D({sqrt(12*12+2.345*2.345), sqrt(5.9*5.9+2.345*2.345), sqrt(0.9*0.9+1.2*1.2)});
-    std::vector<LandmarkClassification> object_class({CONTAINER, SMALL_CONTAINER, PALLET});
-
-    for (auto lmk_ptr : _lmk_list)
-        if (lmk_ptr->getType() == "POLYLINE 2D")
-        {
-            LandmarkPolyline2DPtr polyline_ptr = std::static_pointer_cast<LandmarkPolyline2D>(lmk_ptr);
-            auto n_defined_points = polyline_ptr->getNPoints() - (polyline_ptr->isFirstDefined() ? 0 : 1) - (polyline_ptr->isLastDefined() ? 0 : 1);
-            auto A_id = polyline_ptr->getFirstId() + (polyline_ptr->isFirstDefined() ? 0 : 1);
-            auto B_id = A_id + 1;
-            auto C_id = B_id + 1;
-            auto D_id = C_id + 1;
-
-            // necessary conditions
-            if (polyline_ptr->getClassification() != UNCLASSIFIED ||
-                n_defined_points < 3 ||
-                n_defined_points > 4 )
-                continue;
-
-            //std::cout << "landmark " << lmk_ptr->id() << std::endl;
-
-            // consider 3 first defined points
-            Scalar dAB = (polyline_ptr->getPointVector(A_id) - polyline_ptr->getPointVector(B_id)).norm();
-            Scalar dBC = (polyline_ptr->getPointVector(B_id) - polyline_ptr->getPointVector(C_id)).norm();
-            Scalar dAC = (polyline_ptr->getPointVector(A_id) - polyline_ptr->getPointVector(C_id)).norm();
-
-            //std::cout << "dAB = " << dAB << " error 1: " << fabs(dAB-CONT_L) << " error 2: " << fabs(dAB-CONT_W) << std::endl;
-            //std::cout << "dBC = " << dBC << " error 1: " << fabs(dBC-CONT_W) << " error 2: " << fabs(dBC-CONT_L)   << std::endl;
-            //std::cout << "dAC = " << dAC << " error 1&2: " << fabs(dAC-CONT_D) << std::endl;
-
-            auto classification = -1;
-            bool configuration;
-
-            for (unsigned int i = 0; i < object_L.size(); i++)
-            {
-                // check configuration 1
-                if(fabs(dAB-object_L[i]) < params_->position_error_th &&
-                   fabs(dBC-object_W[i]) < params_->position_error_th &&
-                   fabs(dAC-object_D[i]) < params_->position_error_th)
-                {
-                    configuration = true;
-                    classification = i;
-                    break;
-                }
-
-                // check configuration 2
-                if(fabs(dAB-object_W[i]) < params_->position_error_th &&
-                   fabs(dBC-object_L[i]) < params_->position_error_th &&
-                   fabs(dAC-object_D[i]) < params_->position_error_th)
-                {
-                    configuration = false;
-                    classification = i;
-                    break;
-                }
-            }
-
-            // any container position fitted
-            if (classification != -1)
-            {
-                // if 4 defined checking
-                if (n_defined_points == 4)
-                {
-                    //std::cout << "checking with 4th point... " << std::endl;
-
-                    Scalar dAD = (polyline_ptr->getPointVector(A_id) - polyline_ptr->getPointVector(D_id)).norm();
-                    Scalar dBD = (polyline_ptr->getPointVector(B_id) - polyline_ptr->getPointVector(D_id)).norm();
-                    Scalar dCD = (polyline_ptr->getPointVector(C_id) - polyline_ptr->getPointVector(D_id)).norm();
-
-                    // necessary conditions
-                    if (fabs(dAD-dBC) > params_->position_error_th ||
-                        fabs(dBD-dAC) > params_->position_error_th ||
-                        fabs(dCD-dAB) > params_->position_error_th)
-                        continue;
-                }
-
-                // if not 4 defined add/define points
-                else
-                {
-                    //std::cout << "adding/defining points... " << std::endl;
-                    if (!polyline_ptr->isFirstDefined())
-                    {
-                        polyline_ptr->defineExtreme(false);
-                        D_id = polyline_ptr->getFirstId();
-                    }
-                    else if (!polyline_ptr->isLastDefined())
-                        polyline_ptr->defineExtreme(true);
-                    else
-                        polyline_ptr->addPoint(Eigen::Vector2s::Zero(), true, true);
-                }
-                //std::cout << "landmark " << lmk_ptr->id() << " classified as " << object_class[classification] << " in configuration " << configuration << std::endl;
-
-                // Close
-                polyline_ptr->setClosed();
-
-                // Classify
-                polyline_ptr->classify(object_class[classification]);
-
-                // Unfix origin
-                polyline_ptr->getP()->unfix();
-                polyline_ptr->getO()->unfix();
-
-                // Move origin to B
-                polyline_ptr->getP()->setState(polyline_ptr->getPointVector((configuration ? B_id : A_id)));
-                Eigen::Vector2s frame_x = (configuration ? polyline_ptr->getPointVector(A_id)-polyline_ptr->getPointVector(B_id) : polyline_ptr->getPointVector(C_id)-polyline_ptr->getPointVector(B_id));
-                polyline_ptr->getO()->setState(Eigen::Vector1s::Constant(atan2(frame_x(1),frame_x(0))));
-
-                //std::cout << "A: " << polyline_ptr->getPointVector(A_id).transpose() << std::endl;
-                //std::cout << "B: " << polyline_ptr->getPointVector(B_id).transpose() << std::endl;
-                //std::cout << "C: " << polyline_ptr->getPointVector(C_id).transpose() << std::endl;
-                //std::cout << "frame_x:           " << frame_x.transpose() << std::endl;
-                //std::cout << "frame position:    " << polyline_ptr->getP()->getVector().transpose() << std::endl;
-                //std::cout << "frame orientation: " << polyline_ptr->getO()->getVector() << std::endl;
-
-                // Fix polyline points to its respective relative positions
-                if (configuration)
-                {
-                    polyline_ptr->getPointStateBlockPtr(A_id)->setState(Eigen::Vector2s(object_L[classification], 0));
-                    polyline_ptr->getPointStateBlockPtr(B_id)->setState(Eigen::Vector2s(0, 0));
-                    polyline_ptr->getPointStateBlockPtr(C_id)->setState(Eigen::Vector2s(0, object_W[classification]));
-                    polyline_ptr->getPointStateBlockPtr(D_id)->setState(Eigen::Vector2s(object_L[classification], object_W[classification]));
-                }
-                else
-                {
-                    polyline_ptr->getPointStateBlockPtr(A_id)->setState(Eigen::Vector2s(0, 0));
-                    polyline_ptr->getPointStateBlockPtr(B_id)->setState(Eigen::Vector2s(0, object_W[classification]));
-                    polyline_ptr->getPointStateBlockPtr(C_id)->setState(Eigen::Vector2s(object_L[classification], object_W[classification]));
-                    polyline_ptr->getPointStateBlockPtr(D_id)->setState(Eigen::Vector2s(object_L[classification], 0));
-                }
-                for (auto id = polyline_ptr->getFirstId(); id <= polyline_ptr->getLastId(); id++)
-                {
-                    polyline_ptr->getPointStateBlockPtr(id)->fix();
-                }
-            }
-        }
-}
-
-void ProcessorTrackerLandmarkPolyline::postProcess()
-{
-    //std::cout << "postProcess: " << std::endl;
-    //std::cout << "New Last features: " << getNewFeaturesListLast().size() << std::endl;
-    //std::cout << "Last features: " << last_ptr_->getFeatureList().size() << std::endl;
-    classifyPolilines(getProblem()->getMap()->getLandmarkList());
-}
-
-FactorBasePtr ProcessorTrackerLandmarkPolyline::createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
-{
-    // not used
-    return nullptr;
-}
-
-ProcessorBasePtr ProcessorTrackerLandmarkPolyline::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr)
-{
-    ProcessorParamsPolylinePtr params = std::static_pointer_cast<ProcessorParamsPolyline>(_params);
-    ProcessorTrackerLandmarkPolylinePtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkPolyline>(params);
-    prc_ptr->setName(_unique_name);
-    return prc_ptr;
-}
-
-}        //namespace wolf
-
-// Register in the SensorFactory
-#include "base/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("POLYLINE", ProcessorTrackerLandmarkPolyline)
-} // namespace wolf
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index dd821901cc05ae6cf67b6b35a228d54501717ae3..a30f777fc109da531c0d56237a650ccad0090bf5 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -260,34 +260,34 @@ CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts)
     return capture;
 }
 
-StateBlockPtr SensorBase::getPPtr(const TimeStamp _ts)
+StateBlockPtr SensorBase::getP(const TimeStamp _ts)
 {
-    return getStateBlockPtr(0, _ts);
+    return getStateBlock(0, _ts);
 }
 
-StateBlockPtr SensorBase::getOPtr(const TimeStamp _ts)
+StateBlockPtr SensorBase::getO(const TimeStamp _ts)
 {
-    return getStateBlockPtr(1, _ts);
+    return getStateBlock(1, _ts);
 }
 
-StateBlockPtr SensorBase::getIntrinsicPtr(const TimeStamp _ts)
+StateBlockPtr SensorBase::getIntrinsic(const TimeStamp _ts)
 {
-    return getStateBlockPtr(2, _ts);
+    return getStateBlock(2, _ts);
 }
 
 StateBlockPtr SensorBase::getP()
 {
-    return getStateBlockPtr(0);
+    return getStateBlock(0);
 }
 
 StateBlockPtr SensorBase::getO()
 {
-    return getStateBlockPtr(1);
+    return getStateBlock(1);
 }
 
 StateBlockPtr SensorBase::getIntrinsic()
 {
-    return getStateBlockPtr(2);
+    return getStateBlock(2);
 }
 
 SizeEigen SensorBase::computeCalibSize() const
@@ -321,7 +321,7 @@ Eigen::VectorXs SensorBase::getCalibration() const
 
 bool SensorBase::process(const CaptureBasePtr capture_ptr)
 {
-    capture_ptr->setSensorPtr(shared_from_this());
+    capture_ptr->setSensor(shared_from_this());
 
     for (const auto processor : processor_list_)
     {
@@ -334,27 +334,27 @@ bool SensorBase::process(const CaptureBasePtr capture_ptr)
 ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr)
 {
     processor_list_.push_back(_proc_ptr);
-    _proc_ptr->setSensorPtr(shared_from_this());
+    _proc_ptr->setSensor(shared_from_this());
     _proc_ptr->setProblem(getProblem());
     return _proc_ptr;
 }
 
-StateBlockPtr SensorBase::getStateBlockPtr(unsigned int _i)
+StateBlockPtr SensorBase::getStateBlock(unsigned int _i)
 {
     CaptureBasePtr cap;
 
     if (isStateBlockDynamic(_i, cap))
-        return cap->getStateBlockPtr(_i);
+        return cap->getStateBlock(_i);
 
     return getStateBlockPtrStatic(_i);
 }
 
-StateBlockPtr SensorBase::getStateBlockPtr(unsigned int _i, const TimeStamp& _ts)
+StateBlockPtr SensorBase::getStateBlock(unsigned int _i, const TimeStamp& _ts)
 {
     CaptureBasePtr cap;
 
     if (isStateBlockDynamic(_i, _ts, cap))
-        return cap->getStateBlockPtr(_i);
+        return cap->getStateBlock(_i);
 
     return getStateBlockPtrStatic(_i);
 }
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 17f6e562bb242ba315369effd722391f52f7cf4c..493bc9dcd60683a2836811175a6c57c13c63ff98 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -79,6 +79,9 @@ void SolverManager::update()
     // UPDATE STATE BLOCKS (state, fix or local parameterization)
     for (auto state_ptr : wolf_problem_->getStateBlockPtrList())
     {
+        if (state_blocks_.find(state_ptr)==state_blocks_.end())
+            continue;
+
         assert(state_blocks_.find(state_ptr)!=state_blocks_.end() && "Updating the state of an unregistered StateBlock !");
 
         // state update
@@ -106,8 +109,8 @@ void SolverManager::update()
         }
     }
 
-    assert(wolf_problem_->getFactorNotificationMap().empty() && "wolf problem's factors notification map not empty after update");
-    assert(wolf_problem_->getStateBlockNotificationMap().empty() && "wolf problem's state_blocks notification map not empty after update");
+    //assert(wolf_problem_->getFactorNotificationMap().empty() && "wolf problem's factors notification map not empty after update");
+    //assert(wolf_problem_->getStateBlockNotificationMap().empty() && "wolf problem's state_blocks notification map not empty after update");
 }
 
 wolf::ProblemPtr SolverManager::getProblem()
diff --git a/src/state_block.cpp b/src/state_block.cpp
index 46411e7c30aefb15f921bb9f9fa9c7a114a6154b..b4427c2aae025a794ae433bcb723011f1110f499 100644
--- a/src/state_block.cpp
+++ b/src/state_block.cpp
@@ -33,7 +33,7 @@ void StateBlock::setFixed(bool _fixed)
 //
 //void StateBlock::removeFromProblem(ProblemPtr _problem_ptr)
 //{
-//    _problem_ptr->removeStateBlockPtr(shared_from_this());
+//    _problem_ptr->removeStateBlock(shared_from_this());
 //}
 
 }
diff --git a/src/track_matrix.cpp b/src/track_matrix.cpp
index 54c2bd50b3525d9a68ed9db58bcf18a1b7310d02..50ab808912128f28279b9d0ad39b8580f251faca 100644
--- a/src/track_matrix.cpp
+++ b/src/track_matrix.cpp
@@ -50,7 +50,7 @@ void TrackMatrix::add(size_t _track_id, CaptureBasePtr _cap, FeatureBasePtr _ftr
 
     _ftr->setTrackId(_track_id);
     if (_cap != _ftr->getCapture())
-        _ftr->setCapturePtr(_cap);
+        _ftr->setCapture(_cap);
     tracks_[_track_id].emplace(_cap->getTimeStamp(), _ftr);
     snapshots_[_cap->id()].emplace(_track_id, _ftr);        // will create new snapshot if _cap_id   is not present
 }
diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp
index cf01f2d788ed6a28390a460ee526e566412c9bb6..5820b99ba5df29f281ee2e4e61c82368a93d3561 100644
--- a/src/trajectory_base.cpp
+++ b/src/trajectory_base.cpp
@@ -19,7 +19,7 @@ TrajectoryBase::~TrajectoryBase()
 FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
 {
     // link up
-    _frame_ptr->setTrajectoryPtr(shared_from_this());
+    _frame_ptr->setTrajectory(shared_from_this());
     _frame_ptr->setProblem(getProblem());
 
     if (_frame_ptr->isKey())
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index b6f183675277a9638a85189dbafc5940578a1c84..71217c8e6df3dc2cd0a9ce5bc2e73c97ec19c72a 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -107,7 +107,7 @@ TEST(CaptureBase, process)
     SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2));
     CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, nullptr));
     ASSERT_DEATH({C->process();},""); // No sensor in the capture should fail
-    C->setSensorPtr(S);
+    C->setSensor(S);
     ASSERT_TRUE(C->process()); // This should not fail (although it does nothing)
 }
 
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 3f92fc0872e1f1869a998c0acda2fffaad25f3e3..cbe91a6fdf1c276f55bf50e9c32451f0618feb5b 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -462,7 +462,7 @@ TEST(CeresManager, AddStateBlockLocalParam)
 
     // Local param
     LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
-    sb_ptr->setLocalParametrizationPtr(local_param_ptr);
+    sb_ptr->setLocalParametrization(local_param_ptr);
 
     // add stateblock
     P->addStateBlock(sb_ptr);
@@ -489,7 +489,7 @@ TEST(CeresManager, RemoveLocalParam)
 
     // Local param
     LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
-    sb_ptr->setLocalParametrizationPtr(local_param_ptr);
+    sb_ptr->setLocalParametrization(local_param_ptr);
 
     // add stateblock
     P->addStateBlock(sb_ptr);
@@ -530,7 +530,7 @@ TEST(CeresManager, AddLocalParam)
 
     // Local param
     LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
-    sb_ptr->setLocalParametrizationPtr(local_param_ptr);
+    sb_ptr->setLocalParametrization(local_param_ptr);
 
     // update solver
     ceres_manager_ptr->update();
@@ -611,7 +611,7 @@ TEST(CeresManager, FactorsUpdateLocalParam)
 
     // remove local param
     LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationQuaternionGlobal>();
-    F->getO()->setLocalParametrizationPtr(local_param_ptr);
+    F->getO()->setLocalParametrization(local_param_ptr);
 
     // update solver
     ceres_manager_ptr->update();
diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp
index 2e8245e1544c1a51e280a0874a24d400527a1466..82828c6c274a3ed2e72d53f15e8f625583644697 100644
--- a/test/gtest_feature_IMU.cpp
+++ b/test/gtest_feature_IMU.cpp
@@ -61,7 +61,7 @@ class FeatureIMU_test : public testing::Test
     // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
     // give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
         imu_ptr = std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6s::Identity(), Eigen::Vector6s::Zero());
-        imu_ptr->setFramePtr(origin_frame); //to get ptr to Frm in processorIMU and then get biases
+        imu_ptr->setFrame(origin_frame); //to get ptr to Frm in processorIMU and then get biases
 
     //process data
         data_ << 2, 0, 9.8, 0, 0, 0;
@@ -90,7 +90,7 @@ class FeatureIMU_test : public testing::Test
                                                 calib_preint,
                                                 dD_db_jacobians,
                                                 imu_ptr);
-        feat_imu->setCapturePtr(imu_ptr); //associate the feature to a capture
+        feat_imu->setCapture(imu_ptr); //associate the feature to a capture
 
     }
 
diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
index a3e18e59c74962936a906681143f9003fa9d99f0..425f6a96b9a9af4dcdcf5a491a8fb2ac75f98cf3 100644
--- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
+++ b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
@@ -148,7 +148,7 @@ TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated)
                                                         1.2,
                                                         sensor_ptr);
   // Make 3rd frame last Keyframe
-  problem->getTrajectory()->setLastKeyFramePtr(F3);
+  problem->getTrajectory()->setLastKeyFrame(F3);
 
   // trigger search for loopclosure
   processor_ptr_point2d->process(incomming_dummy);
@@ -181,7 +181,7 @@ TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance)
                                position_covariance_matrix);
 
   // Make 3rd frame last Keyframe
-  problem->getTrajectory()->setLastKeyFramePtr(F3);
+  problem->getTrajectory()->setLastKeyFrame(F3);
 
   // trigger search for loopclosure
   processor_ptr_ellipse2d->process(incomming_dummy);
@@ -193,7 +193,7 @@ TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance)
   ASSERT_EQ(testloops[1]->id(), (unsigned int) 2);
 
   // Make 4th frame last Keyframe
-  problem->getTrajectory()->setLastKeyFramePtr(F4);
+  problem->getTrajectory()->setLastKeyFrame(F4);
 
   // trigger search for loopclosure again
   processor_ptr_ellipse2d->process(incomming_dummy);
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index b6b5f2b85717df13c864551845bdf6c1725c0b39..04c7e6d7a06b2d76224c9b4c1df6e5da3a6f7f03 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -263,7 +263,7 @@ TEST(SolverManager, AddUpdateLocalParamStateBlock)
 
     // Local param
     LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
-    sb_ptr->setLocalParametrizationPtr(local_ptr);
+    sb_ptr->setLocalParametrization(local_ptr);
 
     // Fix state block
     sb_ptr->fix();
@@ -298,7 +298,7 @@ TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock)
 
     // Local param
     LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
-    sb_ptr->setLocalParametrizationPtr(local_ptr);
+    sb_ptr->setLocalParametrization(local_ptr);
 
     // check flags
     ASSERT_FALSE(sb_ptr->stateUpdated());
diff --git a/wolf_scripts/substitution.sh b/wolf_scripts/substitution.sh
index 3085c1f82dbeb0a8e69be38df3e3115938fa7bd2..04ebdd6be9a348b77d7d94fc75af1725eb336cc1 100755
--- a/wolf_scripts/substitution.sh
+++ b/wolf_scripts/substitution.sh
@@ -10,7 +10,8 @@ for file in $(ag 'Ptr\(' . -o); do
     # subs_line=${line}s/${subs}/${subs%List}PtrList/gp
     # echo $subs_line
     # sed -n -e $line's/Ptr\( \)*(\( \)*)/\1(\2)/gp' $target
-    sed -i -e $line's/Ptr\( \)*(\( \)*)/\1(\2)/g' $target
+    # sed -n -e $line's/Ptr\( \)*(\( \)*)/\1(\2)/gp' $target
+    sed -i -e $line's/(/(/g' $target
 done
 
 # for file in $(ag -l -g constraint $folder); do