diff --git a/include/base/capture/capture_gnss_single_diff.h b/include/base/capture/capture_gnss_single_diff.h
index 0c8801a4328a94aabd65771618231df76b842436..0eba8c0c69c697df9356698ebe90c0d08d8fcc83 100644
--- a/include/base/capture/capture_gnss_single_diff.h
+++ b/include/base/capture/capture_gnss_single_diff.h
@@ -35,7 +35,7 @@ class CaptureGnssSingleDiff : public CaptureBase
         const Eigen::Vector3s& getData() const;
         const Eigen::Matrix3s& getDataCovariance() const;
         void getDataAndCovariance(Eigen::Vector3s& data, Eigen::Matrix3s& data_cov) const;
-        FrameBasePtr getOriginFramePtr() const;
+        FrameBasePtr getOriginFrame() const;
 };
 
 inline const Eigen::Vector3s& CaptureGnssSingleDiff::getData() const
@@ -54,7 +54,7 @@ inline void CaptureGnssSingleDiff::getDataAndCovariance(Eigen::Vector3s& data, E
     data_cov = data_covariance_;
 }
 
-inline FrameBasePtr CaptureGnssSingleDiff::getOriginFramePtr() const
+inline FrameBasePtr CaptureGnssSingleDiff::getOriginFrame() const
 {
     return origin_frame_ptr_;
 }
diff --git a/include/base/factor/factor_autodiff_distance_3D.h b/include/base/factor/factor_autodiff_distance_3D.h
index 77eb08e2ae049978c7f2978eee3106fcec66ba9c..a2d74f77361d5de2a5e6fc495ab128981b5824e8 100644
--- a/include/base/factor/factor_autodiff_distance_3D.h
+++ b/include/base/factor/factor_autodiff_distance_3D.h
@@ -21,8 +21,8 @@ class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D,
         FactorAutodiffDistance3D(const FeatureBasePtr&   _feat,
                                  const FrameBasePtr&     _frm_other,
                                  const ProcessorBasePtr& _processor_ptr,
-                                 bool                    _apply_loss_function,
-                                 FactorStatus            _status) :
+                                 bool                    _apply_loss_function=false,
+                                 FactorStatus            _status=FAC_ACTIVE) :
             FactorAutodiff("DISTANCE 3D",
                             _frm_other,
                             nullptr,
diff --git a/include/base/factor/factor_diff_drive.h b/include/base/factor/factor_diff_drive.h
index 09af9aaeb1672d6508aca9d0ab1e2471a0c1c016..89078b139473251b40ec0587d7e3256cbdc85bfd 100644
--- a/include/base/factor/factor_diff_drive.h
+++ b/include/base/factor/factor_diff_drive.h
@@ -38,7 +38,9 @@ class FactorDiffDrive :
                               INTRINSICS_STATE_BLOCK_SIZE >
 {
     using Base = FactorAutodiff<FactorDiffDrive,
-                                RESIDUAL_SIZE, POSITION_STATE_BLOCK_SIZE, ORIENTATION_STATE_BLOCK_SIZE,
+                                RESIDUAL_SIZE,
+                                POSITION_STATE_BLOCK_SIZE, ORIENTATION_STATE_BLOCK_SIZE,
+                                POSITION_STATE_BLOCK_SIZE, ORIENTATION_STATE_BLOCK_SIZE,
                                 INTRINSICS_STATE_BLOCK_SIZE>;
 
 public:
@@ -56,12 +58,12 @@ public:
            _processor_ptr,
            _apply_loss_function,
            _status,
-           _frame_other_ptr->getPPtr(),
-           _frame_other_ptr->getOPtr(),
-           _ftr_ptr->getFramePtr()->getPPtr(),
-           _ftr_ptr->getFramePtr()->getOPtr(),
-           _ftr_ptr->getCapturePtr()->getSensorIntrinsicPtr()),
-           capture_wheel_joint_ptr_(std::static_pointer_cast<CaptureWheelJointPosition>(_ftr_ptr->getCapturePtr()))
+           _frame_other_ptr->getP(),
+           _frame_other_ptr->getO(),
+           _ftr_ptr->getFrame()->getP(),
+           _ftr_ptr->getFrame()->getO(),
+           _ftr_ptr->getCapture()->getSensorIntrinsic()),
+           capture_wheel_joint_ptr_(std::static_pointer_cast<CaptureWheelJointPosition>(_ftr_ptr->getCapture()))
   {
     //
   }
diff --git a/include/base/factor/factor_gnss_fix_2D.h b/include/base/factor/factor_gnss_fix_2D.h
index ebfe02028a09a2b7150cb297f991fed7385ab50c..70434ec608e060c8ac45c5010ea4a31c6a78bfd0 100644
--- a/include/base/factor/factor_gnss_fix_2D.h
+++ b/include/base/factor/factor_gnss_fix_2D.h
@@ -19,7 +19,7 @@ class FactorGnssFix2D : public FactorAutodiff<FactorGnssFix2D, 3, 2, 1, 3, 3, 1,
 
     public:
 
-        FactorGnssFix2D(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = CTR_ACTIVE) :
+        FactorGnssFix2D(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
             FactorAutodiff<FactorGnssFix2D, 3, 2, 1, 3, 3, 1, 1, 1>("GNSS FIX 2D",
                                                                      nullptr,
                                                                      nullptr,
@@ -28,13 +28,13 @@ class FactorGnssFix2D : public FactorAutodiff<FactorGnssFix2D, 3, 2, 1, 3, 3, 1,
                                                                      _processor_ptr,
                                                                      _apply_loss_function,
                                                                      _status,
-                                                                     _ftr_ptr->getFramePtr()->getPPtr(),
-                                                                     _ftr_ptr->getFramePtr()->getOPtr(),
-                                                                     _sensor_gnss_ptr->getStateBlockPtr(0),
-                                                                     _sensor_gnss_ptr->getEnuMapTranslationStatePtr(),
-                                                                     _sensor_gnss_ptr->getEnuMapRollStatePtr(),
-                                                                     _sensor_gnss_ptr->getEnuMapPitchStatePtr(),
-                                                                     _sensor_gnss_ptr->getEnuMapYawStatePtr()),
+                                                                     _ftr_ptr->getFrame()->getP(),
+                                                                     _ftr_ptr->getFrame()->getO(),
+                                                                     _sensor_gnss_ptr->getStateBlock(0),
+                                                                     _sensor_gnss_ptr->getEnuMapTranslationState(),
+                                                                     _sensor_gnss_ptr->getEnuMapRollState(),
+                                                                     _sensor_gnss_ptr->getEnuMapPitchState(),
+                                                                     _sensor_gnss_ptr->getEnuMapYawState()),
             sensor_gnss_ptr_(_sensor_gnss_ptr)
         {
             WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an active GNSS Fix 2D factor without initializing ENU");
diff --git a/include/base/factor/factor_gnss_fix_3D.h b/include/base/factor/factor_gnss_fix_3D.h
index e00a69108dd76f917a3162db285441df12a3eb9c..343958d355d92d2f822708d6c36a13583ed4a90e 100644
--- a/include/base/factor/factor_gnss_fix_3D.h
+++ b/include/base/factor/factor_gnss_fix_3D.h
@@ -27,13 +27,13 @@ class FactorGnssFix3D : public FactorAutodiff<FactorGnssFix3D, 3, 3, 4, 3, 3, 1,
                                                                      _processor_ptr,
                                                                      _apply_loss_function,
                                                                      _status,
-                                                                     _ftr_ptr->getFramePtr()->getPPtr(),
-                                                                     _ftr_ptr->getFramePtr()->getOPtr(),
-                                                                     _sensor_gnss_ptr->getStateBlockPtr(0),
-                                                                     _sensor_gnss_ptr->getEnuMapTranslationStatePtr(),
-                                                                     _sensor_gnss_ptr->getEnuMapRollStatePtr(),
-                                                                     _sensor_gnss_ptr->getEnuMapPitchStatePtr(),
-                                                                     _sensor_gnss_ptr->getEnuMapYawStatePtr()),
+                                                                     _ftr_ptr->getFrame()->getP(),
+                                                                     _ftr_ptr->getFrame()->getO(),
+                                                                     _sensor_gnss_ptr->getStateBlock(0),
+                                                                     _sensor_gnss_ptr->getEnuMapTranslationState(),
+                                                                     _sensor_gnss_ptr->getEnuMapRollState(),
+                                                                     _sensor_gnss_ptr->getEnuMapPitchState(),
+                                                                     _sensor_gnss_ptr->getEnuMapYawState()),
             sensor_gnss_ptr_(_sensor_gnss_ptr)
         {
             WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an ACTIVE GNSS Fix 3D factor without initializing ENU");
diff --git a/include/base/factor/factor_gnss_single_diff_2D.h b/include/base/factor/factor_gnss_single_diff_2D.h
index c5f9b6031f0310520aed9660c4bcc144b5eb1306..9766685930d7a9e758a6823e02e8a021b660f60d 100644
--- a/include/base/factor/factor_gnss_single_diff_2D.h
+++ b/include/base/factor/factor_gnss_single_diff_2D.h
@@ -28,14 +28,14 @@ class FactorGnssSingleDiff2D : public FactorAutodiff<FactorGnssSingleDiff2D, 3,
                                                                               _processor_ptr,
                                                                               _apply_loss_function,
                                                                               _status,
-                                                                              _frame_other_ptr->getPPtr(),
-                                                                              _frame_other_ptr->getOPtr(),
-                                                                              _ftr_ptr->getFramePtr()->getPPtr(),
-                                                                              _ftr_ptr->getFramePtr()->getOPtr(),
-                                                                              _sensor_gnss_ptr->getStateBlockPtr(0),
-                                                                              _sensor_gnss_ptr->getEnuMapRollStatePtr(),
-                                                                              _sensor_gnss_ptr->getEnuMapPitchStatePtr(),
-                                                                              _sensor_gnss_ptr->getEnuMapYawStatePtr()),
+                                                                              _frame_other_ptr->getP(),
+                                                                              _frame_other_ptr->getO(),
+                                                                              _ftr_ptr->getFrame()->getP(),
+                                                                              _ftr_ptr->getFrame()->getO(),
+                                                                              _sensor_gnss_ptr->getStateBlock(0),
+                                                                              _sensor_gnss_ptr->getEnuMapRollState(),
+                                                                              _sensor_gnss_ptr->getEnuMapPitchState(),
+                                                                              _sensor_gnss_ptr->getEnuMapYawState()),
             sensor_gnss_ptr_(_sensor_gnss_ptr)
         {
             WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 2D factor without initializing ENU");
diff --git a/include/base/factor/factor_odom_2D.h b/include/base/factor/factor_odom_2D.h
index e45570d79a8debe284310998cc87f48032521a10..95cf7559282c9bd356c53c92cab6f1234172e993 100644
--- a/include/base/factor/factor_odom_2D.h
+++ b/include/base/factor/factor_odom_2D.h
@@ -16,17 +16,17 @@ class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>
 {
     public:
         FactorOdom2D(const FeatureBasePtr& _ftr_ptr,
-                         const FrameBasePtr& _frame_other_ptr,
-                         const ProcessorBasePtr& _processor_ptr = nullptr,
-                         bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+                     const FrameBasePtr& _frame_other_ptr,
+                     const ProcessorBasePtr& _processor_ptr = nullptr,
+                     bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
              FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>("ODOM 2D",
-                                                                 _frame_other_ptr, nullptr, nullptr, nullptr,
-                                                                 _processor_ptr,
-                                                                 _apply_loss_function, _status,
-                                                                 _frame_other_ptr->getP(),
-                                                                 _frame_other_ptr->getO(),
-                                                                 _ftr_ptr->getFrame()->getP(),
-                                                                 _ftr_ptr->getFrame()->getO())
+                                                         _frame_other_ptr, nullptr, nullptr, nullptr,
+                                                         _processor_ptr,
+                                                         _apply_loss_function, _status,
+                                                         _frame_other_ptr->getP(),
+                                                         _frame_other_ptr->getO(),
+                                                         _ftr_ptr->getFrame()->getP(),
+                                                         _ftr_ptr->getFrame()->getO())
         {
             //
         }
diff --git a/include/base/landmark/landmark_polyline_2D.h b/include/base/landmark/landmark_polyline_2D.h
index df83c52cfbb49df5374df9eaad57264cebdb15c8..0e7b541a40e0504de4b47c58a9cb0f9e7d4cb5a4 100644
--- a/include/base/landmark/landmark_polyline_2D.h
+++ b/include/base/landmark/landmark_polyline_2D.h
@@ -212,7 +212,7 @@ class LandmarkPolyline2D : public LandmarkBase
 
         YAML::Node saveToYaml() const;
 
-        static void tryMergeLandmarks(LandmarkPolyline2DList& _lmk_list, const Scalar& _sq_dist_tol);
+        static void tryMergeLandmarks(LandmarkPolyline2DPtrList& _lmk_list, const Scalar& _sq_dist_tol);
 };
 
 inline StateBlockPtr& LandmarkPolyline2D::firstStateBlock()
diff --git a/include/base/processor/processor_gnss_fix.h b/include/base/processor/processor_gnss_fix.h
index 8fd8d13469a7ac27b21f711557d9c4e8a0b612ae..396b07b1b211076dadc78c83ba306e51a91c2adb 100644
--- a/include/base/processor/processor_gnss_fix.h
+++ b/include/base/processor/processor_gnss_fix.h
@@ -36,8 +36,8 @@ class ProcessorGnssFix : public ProcessorBase
         virtual bool voteForKeyFrame();
 
     private:
-        void emplaceConstraint(FeatureBasePtr& ftr_ptr);
-        bool rejectOutlier(ConstraintBasePtr ctr_ptr);
+        void emplaceFactor(FeatureBasePtr& ftr_ptr);
+        bool rejectOutlier(FactorBasePtr ctr_ptr);
 
     public:
         static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr);
diff --git a/include/base/processor/processor_polyline.h b/include/base/processor/processor_polyline.h
index a9f05d94c223e84c8e5784c8a08b9b93fc9e5e08..959c9d59dbc6fa70653b49b70b4d84509cd05c41 100644
--- a/include/base/processor/processor_polyline.h
+++ b/include/base/processor/processor_polyline.h
@@ -14,8 +14,8 @@
 #include "base/feature/feature_polyline_2D.h"
 #include "base/landmark/landmark_match.h"
 #include "base/landmark/landmark_polyline_2D.h"
-#include "base/constraint/constraint_point_2D.h"
-#include "base/constraint/constraint_point_to_line_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/processor/processor_base.h"
 
@@ -51,12 +51,12 @@ class ProcessorPolyline : public ProcessorBase
 
         ProcessorParamsPolylinePtr params_;
 
-        FeatureBaseList new_features_incoming_, new_features_last_, known_features_incoming_, known_features_last_;
-        LandmarkBaseList new_landmarks_;        ///< List of new detected landmarks
+        FeatureBasePtrList new_features_incoming_, new_features_last_, known_features_incoming_, known_features_last_;
+        LandmarkBasePtrList new_landmarks_;        ///< List of new detected landmarks
         LandmarkMatchMap matches_landmark_from_incoming_;
         LandmarkMatchMap matches_landmark_from_last_;
 
-        std::list<LandmarkPolyline2DList> merge_candidates_list_;
+        std::list<LandmarkPolyline2DPtrList> merge_candidates_list_;
 
         Eigen::Matrix2s R_sensor_world_, R_world_sensor_;
         Eigen::Matrix2s R_robot_sensor_;
@@ -85,32 +85,32 @@ class ProcessorPolyline : public ProcessorBase
         virtual bool voteForKeyFrame();
 
         // Implementation
-        void extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, FeatureBaseList& _polyline_list);
-        unsigned int findLandmarks(const LandmarkBaseList& _landmark_list_in, FeatureBaseList& _feature_list_out, LandmarkMatchMap& _feature_landmark_correspondences);
+        void extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _polyline_list);
+        unsigned int findLandmarks(const LandmarkBasePtrList& _landmark_list_in, FeatureBasePtrList& _feature_list_out, LandmarkMatchMap& _feature_landmark_correspondences);
 
         virtual void createNewLandmarks();
         virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
 
-        virtual void establishConstraints();
-        void emplaceConstraintPointToLine(FeaturePolyline2DPtr _polyline_feature,
+        virtual void establishFactors();
+        void emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature,
                                           LandmarkPolyline2DPtr _polyline_landmark,
                                           const int& _ftr_point_id,
                                           const int& _lmk_point_id,
                                           const int& _lmk_prev_point_id);
-        void emplaceConstraintPoint(FeaturePolyline2DPtr _polyline_feature,
+        void emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature,
                                     LandmarkPolyline2DPtr _polyline_landmark,
                                     const int& _ftr_point_id,
                                     const int& _lmk_point_id);
-        bool rejectOutlier(ConstraintBasePtr& ctr_ptr);
-        void classifyPolylineLandmarks(LandmarkBaseList& _lmk_list);
+        bool rejectOutlier(FactorBasePtr& fac_ptr);
+        void classifyPolylineLandmarks(LandmarkBasePtrList& _lmk_list);
         LandmarkMatchPolyline2DPtr tryMatch(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr);
         LandmarkMatchPolyline2DPtr tryMatch(const Eigen::MatrixXs& _lmk_expected_feature_points, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const;
 
         // Gets/Sets
-        const FeatureBaseList& getLastNewPolylines() const;
-        const FeatureBaseList& getLastKnownPolylines() const;
+        const FeatureBasePtrList& getLastNewPolylines() const;
+        const FeatureBasePtrList& getLastKnownPolylines() const;
         virtual void setKeyFrame(CaptureBasePtr _capture_ptr);
-        virtual CaptureBasePtr getLastPtr();
+        virtual CaptureBasePtr getLast();
 
         // Maths
         void computeTransformations(const TimeStamp& _ts);
@@ -122,49 +122,49 @@ class ProcessorPolyline : public ProcessorBase
         static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
 };
 
-inline void ProcessorPolyline::emplaceConstraintPointToLine(FeaturePolyline2DPtr _polyline_feature,
-                                                 LandmarkPolyline2DPtr _polyline_landmark,
-                                                 const int& _ftr_point_id,
-                                                 const int& _lmk_point_id,
-                                                 const int& _lmk_prev_point_id)
+inline void ProcessorPolyline::emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature,
+                                                        LandmarkPolyline2DPtr _polyline_landmark,
+                                                        const int& _ftr_point_id,
+                                                        const int& _lmk_point_id,
+                                                        const int& _lmk_prev_point_id)
 {
     assert(_polyline_landmark->isValidId(_lmk_point_id) && _polyline_landmark->isValidId(_lmk_prev_point_id));
 
     // CREATE CONSTRAINT --------------------
-    ConstraintBasePtr new_ctr = std::make_shared<ConstraintPointToLine2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id, _lmk_prev_point_id);
+    FactorBasePtr new_fac = std::make_shared<FactorPointToLine2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id, _lmk_prev_point_id);
 
     // ADD CONSTRAINT --------------------
-    _polyline_feature->addConstraint(new_ctr);
-    _polyline_landmark->addConstrainedBy(new_ctr);
+    _polyline_feature->addFactor(new_fac);
+    _polyline_landmark->addConstrainedBy(new_fac);
 }
 
-inline void ProcessorPolyline::emplaceConstraintPoint(FeaturePolyline2DPtr _polyline_feature,
+inline void ProcessorPolyline::emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature,
                                                  LandmarkPolyline2DPtr _polyline_landmark,
                                                  const int& _ftr_point_id,
                                                  const int& _lmk_point_id)
 {
     // CREATE CONSTRAINT --------------------
-    ConstraintBasePtr new_ctr = std::make_shared<ConstraintPoint2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id);
+    FactorBasePtr new_fac = std::make_shared<FactorPoint2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id);
 
     // ADD CONSTRAINT --------------------
-    _polyline_feature->addConstraint(new_ctr);
-    _polyline_landmark->addConstrainedBy(new_ctr);
+    _polyline_feature->addFactor(new_fac);
+    _polyline_landmark->addConstrainedBy(new_fac);
 }
 
-inline const FeatureBaseList& ProcessorPolyline::getLastNewPolylines() const
+inline const FeatureBasePtrList& ProcessorPolyline::getLastNewPolylines() const
 {
     return new_features_last_;
 }
 
-inline const FeatureBaseList& ProcessorPolyline::getLastKnownPolylines() const
+inline const FeatureBasePtrList& ProcessorPolyline::getLastKnownPolylines() const
 {
-    if (last_ptr_->getFramePtr()->isKey())
+    if (last_ptr_->getFrame()->isKey())
         return last_ptr_->getFeatureList();
     else
         return known_features_last_;
 }
 
-inline CaptureBasePtr ProcessorPolyline::getLastPtr()
+inline CaptureBasePtr ProcessorPolyline::getLast()
 {
     return last_ptr_;
 }
diff --git a/include/base/processor/processor_tracker_feature.h b/include/base/processor/processor_tracker_feature.h
index 97cbb23737fee4bb24a556243ddd24bccb408be9..2f0e7399ca78b42ff2ad5ddb4a4f53a1280fbbdf 100644
--- a/include/base/processor/processor_tracker_feature.h
+++ b/include/base/processor/processor_tracker_feature.h
@@ -169,7 +169,7 @@ class ProcessorTrackerFeature : public ProcessorTracker
 
         /** \brief Establish factors between features in Captures \b last and \b origin
          */
-        virtual void establishConstraints();
+        virtual void establishFactors();
 
     public:
         Track track(SizeStd _trk_id);
diff --git a/include/base/processor/processor_tracker_feature_corner.h b/include/base/processor/processor_tracker_feature_corner.h
index 63ef69245d53cb14520d8f5b16759b8b34b230d2..05e3723e3f75e49a8e6917ef8c2c9aa4cd1858f8 100644
--- a/include/base/processor/processor_tracker_feature_corner.h
+++ b/include/base/processor/processor_tracker_feature_corner.h
@@ -115,7 +115,7 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
          * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
          * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out);
+        virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out);
 
         virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
 
diff --git a/include/base/processor/processor_tracker_feature_dummy.h b/include/base/processor/processor_tracker_feature_dummy.h
index 419c31f5d31b81211c2b9cf15048c559103356ae..d517431b134026fdebeb91d17748bb49471b38cc 100644
--- a/include/base/processor/processor_tracker_feature_dummy.h
+++ b/include/base/processor/processor_tracker_feature_dummy.h
@@ -65,7 +65,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
          * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
          * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out);
+        virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out);
 
         virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
 
diff --git a/include/base/processor/processor_tracker_feature_image.h b/include/base/processor/processor_tracker_feature_image.h
index 25e2c629157033508b32bba6e80c910510ca8e16..3f730b60dfd540cf9fd2a44da40b61240a4d6f9d 100644
--- a/include/base/processor/processor_tracker_feature_image.h
+++ b/include/base/processor/processor_tracker_feature_image.h
@@ -121,7 +121,7 @@ class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature
          * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
          * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out);
+        virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out);
 
         virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
 
diff --git a/include/base/processor/processor_tracker_feature_polyline_2D.h b/include/base/processor/processor_tracker_feature_polyline_2D.h
index f883668806fd6501a42b5ad473f65d436b32de8f..7ac9687dfbb5aa8541bdc74306f38d98a2c75696 100644
--- a/include/base/processor/processor_tracker_feature_polyline_2D.h
+++ b/include/base/processor/processor_tracker_feature_polyline_2D.h
@@ -16,8 +16,8 @@
 #include "base/capture/capture_laser_2D.h"
 #include "base/feature/feature_polyline_2D.h"
 #include "base/feature/feature_match_polyline_2D.h"
-#include "base/constraint/constraint_point_2D.h"
-#include "base/constraint/constraint_point_to_line_2D.h"
+#include "base/factor/factor_point_2D.h"
+#include "base/factor/factor_point_to_line_2D.h"
 
 //laser_scan_utils
 #include "laser_scan_utils/laser_scan.h"
@@ -29,8 +29,8 @@ namespace wolf
 
 WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeaturePolyline2D);
 WOLF_PTR_TYPEDEFS(ProcessorTrackerFeaturePolyline2D);
-typedef std::list<FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DList;
-typedef std::list<LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DList;
+typedef std::list<FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DPtrList;
+typedef std::list<LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DPtrList;
 typedef std::map<Scalar,FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DScalarMap;
 typedef std::map<Scalar,LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DScalarMap;
 
@@ -52,10 +52,10 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
         ProcessorParamsTrackerFeaturePolyline2DPtr params_tracker_feature_polyline_;
         laserscanutils::LineFinderIterative line_finder_;
 
-        FeatureBaseList all_features_incoming_, all_features_last_;
+        FeatureBasePtrList all_features_incoming_, all_features_last_;
         LandmarkMatchPolyline2DMap landmark_match_map_;
-        LandmarkPolyline2DList   modified_lmks_;
-        std::list<LandmarkPolyline2DList> merge_candidates_list_;
+        LandmarkPolyline2DPtrList   modified_lmks_;
+        std::list<LandmarkPolyline2DPtrList> merge_candidates_list_;
 
         Eigen::Matrix2s R_sensor_world_last_, R_world_sensor_last_;
         Eigen::Vector2s t_sensor_world_last_, t_world_sensor_last_;
@@ -80,8 +80,8 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
          * \param _features_incoming_out returned list of features found in \b incoming
          * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
          */
-        virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in,
-                                           FeatureBaseList& _features_incoming_out,
+        virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in,
+                                           FeatureBasePtrList& _features_incoming_out,
                                            FeatureMatchMap& _feature_correspondences);
 
         /** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
@@ -108,38 +108,40 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
         virtual unsigned int processNew(const unsigned int& _max_features);
 
         /** \brief Detect new Features
-         * \param _max_features maximum number of features detected
+         * \param _max_features maximum number of features detected (-1: unlimited. 0: none)
+         * \param _features_last_out The list of detected Features.
          * \return The number of detected Features.
          *
          * This function detects Features that do not correspond to known Features/Landmarks in the system.
          *
-         * The function sets _features_last_out, the list of newly detected features in Capture last.
+         * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
+         * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features,
-                                               FeatureBaseList& _features_last_out);
+        virtual unsigned int detectNewFeatures(const int& _max_new_features,
+                                               FeatureBasePtrList& _features_last_out);
 
-        /** \brief Create a new constraint and link it to the wolf tree
+        /** \brief Create a new factor and link it to the wolf tree
          * \param _feature_ptr pointer to the parent Feature
          * \param _feature_other_ptr pointer to the other feature constrained.
          *
          * Implement this method in derived classes.
          *
-         * This function creates a constraint of the appropriate type for the derived processor.
+         * This function creates a factor of the appropriate type for the derived processor.
          */
-        virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr,
+        virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr,
                                                    FeatureBasePtr _feature_other_ptr){ return nullptr; };
 
-        /** \brief Establish constraints between features in Captures \b last and \b origin
+        /** \brief Establish factors between features in Captures \b last and \b origin
          */
-        virtual void establishConstraints();
+        virtual void establishFactors();
 
-        void emplaceConstraintPointToLine(FeaturePolyline2DPtr _polyline_feature,
+        void emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature,
                                           LandmarkPolyline2DPtr _polyline_landmark,
                                           const int& _ftr_point_id,
                                           const int& _lmk_point_id,
                                           const int& _lmk_prev_point_id);
 
-        void emplaceConstraintPoint(FeaturePolyline2DPtr _polyline_feature,
+        void emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature,
                                     LandmarkPolyline2DPtr _polyline_landmark,
                                     const int& _ftr_point_id,
                                     const int& _lmk_point_id);
@@ -196,7 +198,7 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
                                        const ProcessorParamsBasePtr _params,
                                        const SensorBasePtr _sensor_ptr);
 
-        const FeatureBaseList& getLastNewFeatures() const
+        const FeatureBasePtrList& getLastNewFeatures() const
         {
             return all_features_last_;
         }
diff --git a/include/base/processor/processor_tracker_feature_trifocal.h b/include/base/processor/processor_tracker_feature_trifocal.h
index 0fc73588391bc5e13bd5a66763cf07b88eebc6e9..03758dbdaecaa24414abfcbf6df272783029f63e 100644
--- a/include/base/processor/processor_tracker_feature_trifocal.h
+++ b/include/base/processor/processor_tracker_feature_trifocal.h
@@ -94,7 +94,7 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
          * The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
          * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out) override;
+        virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out) override;
 
         /** \brief Create a new factor and link it to the wolf tree
          * \param _feature_ptr pointer to the parent Feature
diff --git a/include/base/processor/processor_tracker_landmark.h b/include/base/processor/processor_tracker_landmark.h
index 35a99280c4a2fc9b72bce7dbc00f6f937bcfa993..b4e079eb723b60aa28e873167f4f4d7fb409cdeb 100644
--- a/include/base/processor/processor_tracker_landmark.h
+++ b/include/base/processor/processor_tracker_landmark.h
@@ -140,7 +140,7 @@ class ProcessorTrackerLandmark : public ProcessorTracker
          * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
          * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out) = 0;
+        virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out) = 0;
 
         /** \brief Creates a landmark for each of new_features_last_
          **/
diff --git a/include/base/processor/processor_tracker_landmark_corner.h b/include/base/processor/processor_tracker_landmark_corner.h
index 83be3c837aa8c2f26f57ceb8322a3897303096ad..903f14490f3dcb2c2f79e0900a35fd3e26a71173 100644
--- a/include/base/processor/processor_tracker_landmark_corner.h
+++ b/include/base/processor/processor_tracker_landmark_corner.h
@@ -144,7 +144,7 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
          * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
          * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out);
+        virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out);
 
         /** \brief Create one landmark
          *
diff --git a/include/base/processor/processor_tracker_landmark_dummy.h b/include/base/processor/processor_tracker_landmark_dummy.h
index c0c3424cb0983f352d6976a7bee0ec07af9bdaa2..a312ce4a08b8b71a2cc9d8519932d5c4074fc653 100644
--- a/include/base/processor/processor_tracker_landmark_dummy.h
+++ b/include/base/processor/processor_tracker_landmark_dummy.h
@@ -57,7 +57,7 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
          * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
          * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out);
+        virtual unsigned int detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out);
 
         /** \brief Create one landmark
          *
diff --git a/include/base/processor/processor_tracker_landmark_image.h b/include/base/processor/processor_tracker_landmark_image.h
index afdc8eff9200218c2856ee397aac879960a146d2..9765efd4be3584479e4322ca268d4de3438a28ba 100644
--- a/include/base/processor/processor_tracker_landmark_image.h
+++ b/include/base/processor/processor_tracker_landmark_image.h
@@ -137,7 +137,7 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
          * The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
          * the list of newly detected features of the capture last_ptr_.
          */
-        virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out);
+        virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out);
 
         /** \brief Create one landmark
          *
diff --git a/include/base/sensor/sensor_gnss.h b/include/base/sensor/sensor_gnss.h
index 1781dd90023d27430668d4ed2c1b7256b7450e1e..80367f52dcfc0b5ea38ba122835f5e2143696df6 100644
--- a/include/base/sensor/sensor_gnss.h
+++ b/include/base/sensor/sensor_gnss.h
@@ -43,10 +43,10 @@ class SensorGnss : public SensorBase
         bool isEnuMapInitialized() const;
 
         // ENU_MAP
-        StateBlockPtr getEnuMapTranslationStatePtr() const;
-        StateBlockPtr getEnuMapRollStatePtr() const;
-        StateBlockPtr getEnuMapPitchStatePtr() const;
-        StateBlockPtr getEnuMapYawStatePtr() const;
+        StateBlockPtr getEnuMapTranslationState() const;
+        StateBlockPtr getEnuMapRollState() const;
+        StateBlockPtr getEnuMapPitchState() const;
+        StateBlockPtr getEnuMapYawState() const;
         void initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, const Eigen::Vector3s& _t_ECEF_antenaENU,
                               const Eigen::VectorXs& _pose_MAP_frame2, const Eigen::Vector3s& _t_ECEF_antena2);
 
@@ -72,22 +72,22 @@ inline bool SensorGnss::isEnuMapInitialized() const
     return ENU_MAP_initialized_;
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapTranslationStatePtr() const
+inline StateBlockPtr SensorGnss::getEnuMapTranslationState() const
 {
     return getStateBlockPtrStatic(3);
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapRollStatePtr() const
+inline StateBlockPtr SensorGnss::getEnuMapRollState() const
 {
     return getStateBlockPtrStatic(4);
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapPitchStatePtr() const
+inline StateBlockPtr SensorGnss::getEnuMapPitchState() const
 {
     return getStateBlockPtrStatic(5);
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapYawStatePtr() const
+inline StateBlockPtr SensorGnss::getEnuMapYawState() const
 {
     return getStateBlockPtrStatic(6);
 }
@@ -113,7 +113,7 @@ inline Eigen::Matrix<T,3,3> SensorGnss::computeREnuMap(const T& _r,const T& _p,c
 
     // TODO: store R's when T=Scalar
     //Eigen::Matrix<T,3,3> R_roll_T, R_pitch_T, R_yaw_T;
-    //if (!getRollStatePtr()->isFixed())
+    //if (!getRollState()->isFixed())
     //    *R_roll_ptr_ = Eigen::AngleAxis<Scalar>(0.25*M_PI, Eigen::Vector3s::UnitX());
 }
 
diff --git a/src/examples/test_processor_tracker_feature.cpp b/src/examples/test_processor_tracker_feature.cpp
index e7e5752b0e381d8ede98d7086d843c78b71bfa71..44d3e44f200b5a0b0858c181c160d96e284cc051 100644
--- a/src/examples/test_processor_tracker_feature.cpp
+++ b/src/examples/test_processor_tracker_feature.cpp
@@ -20,22 +20,22 @@ void print_prc(wolf::ProcessorTrackerFeaturePtr _prc)
 {
     using namespace wolf;
 
-    auto o = _prc->getOriginPtr();
-    auto l = _prc->getLastPtr();
-    auto i = _prc->getIncomingPtr();
+    auto o = _prc->getOrigin();
+    auto l = _prc->getLast();
+    auto i = _prc->getIncoming();
 
     std::cout <<   "o: C" << ( int)( o ? o->id() : -99 )
             << " || l: C" << ( int)( l ? l->id() : -99 )
             << " || i: C" << ( int)( i ? i->id() : -99 )
             << std::endl;
-    for (auto ftr : _prc->getLastPtr()->getFeatureList())
+    for (auto ftr : _prc->getLast()->getFeatureList())
     {
         SizeStd trk_id = ftr->trackId();
         std::cout << "Track " << trk_id << " ---------------------" << std::endl;
         for (auto ftr_pair : _prc->track(trk_id))
         {
             auto f = ftr_pair.second;
-            auto C = f->getCapturePtr();
+            auto C = f->getCapture();
             std::cout << "f" << f->id() << " C" << (int)(C ? C->id() : -99) << std::endl;
         }
     }
diff --git a/src/examples/test_trifocal_optimization.cpp b/src/examples/test_trifocal_optimization.cpp
index 93cc5eed6b0fdd7541f36dcf5153c887b59e7c47..4e6ae7113cca24c723ddbd791db1426625950fef 100644
--- a/src/examples/test_trifocal_optimization.cpp
+++ b/src/examples/test_trifocal_optimization.cpp
@@ -10,14 +10,14 @@
 #include <vision_utils.h>
 
 //Wolf includes
-#include "../processors/processor_tracker_feature_trifocal.h"
-#include "../capture_image.h"
-#include "../sensor_camera.h"
-#include "../ceres_wrapper/ceres_manager.h"
-#include "../rotations.h"
-#include "../capture_pose.h"
-#include "../capture_void.h"
-#include "../constraints/constraint_autodiff_distance_3D.h"
+#include "base/processor/processor_tracker_feature_trifocal.h"
+#include "base/capture/capture_image.h"
+#include "base/sensor/sensor_camera.h"
+#include "base/ceres_wrapper/ceres_manager.h"
+#include "base/rotations.h"
+#include "base/capture/capture_pose.h"
+#include "base/capture/capture_void.h"
+#include "base/factor/factor_autodiff_distance_3D.h"
 
 Eigen::VectorXs get_random_state(const double& _LO, const double& _HI)
 {
@@ -122,7 +122,7 @@ int main(int argc, char** argv)
     camera->process(capture_1);
 
     // Verify KFs
-    FrameBasePtr kf1_check = capture_1->getFramePtr();
+    FrameBasePtr kf1_check = capture_1->getFrame();
     assert(kf1->id()==kf1_check->id() && "Prior and KF1 are not the same!");
 
 //    problem->print(2,0,1,0);
@@ -159,12 +159,12 @@ int main(int argc, char** argv)
     }
 
     // ==================================================
-    // Establish Scale Constraint (to see results scaled)
+    // Establish Scale Factor (to see results scaled)
     // ==================================================
 
-    std::cout << "================== ADD Scale constraint ========================" << std::endl;
+    std::cout << "================== ADD Scale factor ========================" << std::endl;
 
-    // Distance constraint
+    // Distance factor
     Vector1s distance(0.2);      // 2x10cm distance -- this fixes the scale
     Matrix1s dist_cov(0.000001); // 1mm error
 
@@ -175,11 +175,11 @@ int main(int argc, char** argv)
     ftr_dist = cap_dist->addFeature(make_shared<FeatureBase>("DISTANCE",
                                                               distance,
                                                               dist_cov));
-    ConstraintBasePtr
-    ctr_dist  = ftr_dist->addConstraint(make_shared<ConstraintAutodiffDistance3D>(ftr_dist,
+    FactorBasePtr
+    fac_dist  = ftr_dist->addFactor(make_shared<FactorAutodiffDistance3D>(ftr_dist,
                                                                                   kfs.at(0),
                                                                                   nullptr));
-    kfs.at(0)->addConstrainedBy(ctr_dist);
+    kfs.at(0)->addConstrainedBy(fac_dist);
 
     problem->print(1,1,1,0);
 
@@ -195,8 +195,8 @@ int main(int argc, char** argv)
     problem->print(1,1,1,0);
 
     // Print orientation states for all KFs
-    for (auto kf : problem->getTrajectoryPtr()->getFrameList())
-        std::cout << "KF" << kf->id() << " Euler deg " << wolf::q2e(kf->getOPtr()->getState()).transpose()*180.0/3.14159 << std::endl;
+    for (auto kf : problem->getTrajectory()->getFrameList())
+        std::cout << "KF" << kf->id() << " Euler deg " << wolf::q2e(kf->getO()->getState()).transpose()*180.0/3.14159 << std::endl;
 
 
     // ===============================================
@@ -205,10 +205,11 @@ int main(int argc, char** argv)
     // GET COVARIANCES of all states
     WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
     ceres_manager->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
-    for (auto kf : problem->getTrajectoryPtr()->getFrameList())
+    for (auto kf : problem->getTrajectory()->getFrameList())
         if (kf->isKey())
         {
-            Eigen::MatrixXs cov = kf->getCovariance();
+            Eigen::MatrixXs cov;
+            kf->getCovariance(cov);
             WOLF_TRACE("KF", kf->id(), "_std (sigmas) = ", cov.diagonal().transpose().array().sqrt());
         }
     std::cout << std::endl;
@@ -220,7 +221,7 @@ int main(int argc, char** argv)
     // ADD PERTURBATION
     std::cout << "================== ADD PERTURBATION ========================" << std::endl;
 
-    for (auto kf : problem->getTrajectoryPtr()->getFrameList())
+    for (auto kf : problem->getTrajectory()->getFrameList())
     {
         if (kf != kf1)
         {
@@ -228,7 +229,7 @@ int main(int argc, char** argv)
             Eigen::Vector7s state_perturbed = kf->getState() + perturbation;
             state_perturbed.segment(3,4).normalize();
             kf->setState(state_perturbed);
-            std::cout << "KF" << kf->id() << " Euler deg " << wolf::q2e(kf->getOPtr()->getState()).transpose()*180.0/3.14159 << std::endl;
+            std::cout << "KF" << kf->id() << " Euler deg " << wolf::q2e(kf->getO()->getState()).transpose()*180.0/3.14159 << std::endl;
         }
     }
 
@@ -246,8 +247,8 @@ int main(int argc, char** argv)
     std::cout << "================== AFTER SOLVE 2nd TIME ========================" << std::endl;
     problem->print(1,1,1,0);
 
-    for (auto kf : problem->getTrajectoryPtr()->getFrameList())
-        std::cout << "KF" << kf->id() << " Euler deg " << wolf::q2e(kf->getOPtr()->getState()).transpose()*180.0/3.14159 << std::endl;
+    for (auto kf : problem->getTrajectory()->getFrameList())
+        std::cout << "KF" << kf->id() << " Euler deg " << wolf::q2e(kf->getO()->getState()).transpose()*180.0/3.14159 << std::endl;
 
     cv::waitKey(0); // Wait for a keystroke in the window
 
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp
index 6cc41a85db65fdf5a88e8b8d7a38cab019d94d9b..7abfae36142847df7511e5fd19e94f5f25ab85c0 100644
--- a/src/landmark/landmark_polyline_2D.cpp
+++ b/src/landmark/landmark_polyline_2D.cpp
@@ -83,8 +83,8 @@ StateBlockPtr LandmarkPolyline2D::getPointStateBlock(int _i)
 Eigen::MatrixXs LandmarkPolyline2D::computePointsGlobal() const
 {
     Eigen::MatrixXs points_global = Eigen::MatrixXs::Ones(3,getNPoints());
-    Eigen::Matrix2s R_world_points = Eigen::Rotation2Ds(getOPtr()->getState()(0)).matrix();
-    Eigen::Vector2s t_world_points = getPPtr()->getState();
+    Eigen::Matrix2s R_world_points = Eigen::Rotation2Ds(getO()->getState()(0)).matrix();
+    Eigen::Vector2s t_world_points = getP()->getState();
 
     for (int i = 0, valid_id = getFirstId(); i < getNPoints(); i++, valid_id = getNextValidId(valid_id))
     {
@@ -457,7 +457,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
             // If landmark point constrained -> new factor
             if (fac_point_ptr->getLandmarkPointId() == _remove_id)
             {
-                new_fac_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeaturePtr()),
+                new_fac_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
                                                               std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
                                                               fac_point_ptr->getProcessor(),
                                                               fac_point_ptr->getFeaturePointId(),
@@ -483,7 +483,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
             // If landmark point is aux point -> new factor
             else if (fac_point_line_ptr->getLandmarkPointAuxId() == _remove_id)
             {
-                new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeaturePtr()),
+                new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
                                                                     std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
                                                                     fac_point_line_ptr->getProcessor(),
                                                                     fac_point_line_ptr->getFeaturePointId(),
@@ -692,20 +692,20 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli
         classify(pl_class);
 
         // Move origin to to the center
-        getPPtr()->setState((getPointVector(points_ids[0]) + getPointVector(points_ids[2])) / 2.0);
+        getP()->setState((getPointVector(points_ids[0]) + getPointVector(points_ids[2])) / 2.0);
         Eigen::Vector2s frame_x = (configuration ? getPointVector(points_ids[0])-getPointVector(points_ids[1]) : getPointVector(points_ids[2])-getPointVector(points_ids[1]));
-        getOPtr()->setState(Eigen::Vector1s::Constant(atan2(frame_x(1),frame_x(0))));
+        getO()->setState(Eigen::Vector1s::Constant(atan2(frame_x(1),frame_x(0))));
 
         // Unfix origin
-        getPPtr()->unfix();
-        getOPtr()->unfix();
+        getP()->unfix();
+        getO()->unfix();
 
         //std::cout << "A: " << getPointVector(points_ids[0]).transpose() << std::endl;
         //std::cout << "B: " << getPointVector(points_ids[1]).transpose() << std::endl;
         //std::cout << "C: " << getPointVector(points_ids[2]).transpose() << std::endl;
         //std::cout << "frame_x:           " << frame_x.transpose() << std::endl;
-        //std::cout << "frame position:    " << getPPtr()->getState().transpose() << std::endl;
-        //std::cout << "frame orientation: " << getOPtr()->getState() << std::endl;
+        //std::cout << "frame position:    " << getP()->getState().transpose() << std::endl;
+        //std::cout << "frame orientation: " << getO()->getState() << std::endl;
 
         // Set polyline points to its respective relative positions
         if (configuration)
@@ -819,16 +819,16 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk,
     }
 
     // ------------------------- COPY FACTORS
-    FactorBaseList old_factors_list = _merged_lmk->getConstrainedByList();
+    FactorBasePtrList old_factors_list = _merged_lmk->getConstrainedByList();
     //std::cout << "\tchanging " << old_factors_list.size() << " factors." << std::endl;
     FactorBasePtr new_fac_ptr = nullptr;
     for (auto fac_ptr : old_factors_list)
     {
-        //std::cout << "\t\tfactor " << fac_ptr->id() << " to landmark " << fac_ptr->getLandmarkOtherPtr()->id() << std::endl;
-        assert(fac_ptr->getLandmarkOtherPtr() == _merged_lmk);
+        //std::cout << "\t\tfactor " << fac_ptr->id() << " to landmark " << fac_ptr->getLandmarkOther()->id() << std::endl;
+        assert(fac_ptr->getLandmarkOther() == _merged_lmk);
 
         FactorPoint2DPtr fac_point_ptr = std::dynamic_pointer_cast<FactorPoint2D>(fac_ptr);
-        FactortPointToLine2DPtr fac_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr);
+        FactorPointToLine2DPtr fac_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr);
 
         // point 2 point
         if (fac_point_ptr)
@@ -836,7 +836,7 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk,
             //std::cout << "\t\tpoint-point factor to id: " << fac_point_ptr->getLandmarkPointId() << std::endl;
             assert(merged_id_to_id.find(fac_point_ptr->getLandmarkPointId()) != merged_id_to_id.end());
 
-            new_fac_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeaturePtr()),
+            new_fac_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
                                                           std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
                                                           fac_point_ptr->getProcessor(),
                                                           fac_point_ptr->getFeaturePointId(),
@@ -852,7 +852,7 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk,
             assert(merged_id_to_id.find(fac_point_line_ptr->getLandmarkPointId()) != merged_id_to_id.end());
             assert(merged_id_to_id.find(fac_point_line_ptr->getLandmarkPointAuxId()) != merged_id_to_id.end());
 
-            new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeaturePtr()),
+            new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
                                                                 std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
                                                                 fac_point_line_ptr->getProcessor(),
                                                                 fac_point_line_ptr->getFeaturePointId(),
@@ -873,7 +873,7 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk,
         //std::cout << "\t\tdeleting factor: " << fac_ptr->id() << std::endl;
 
         // add new factor
-        fac_ptr->getFeaturePtr()->addFactor(new_fac_ptr);
+        fac_ptr->getFeature()->addFactor(new_fac_ptr);
         addConstrainedBy(new_fac_ptr);
 
         // remove factor
@@ -968,7 +968,7 @@ YAML::Node LandmarkPolyline2D::saveToYaml() const
     return node;
 }
 
-void LandmarkPolyline2D::tryMergeLandmarks(LandmarkPolyline2DList& _lmk_list, const Scalar& _sq_dist_tol)
+void LandmarkPolyline2D::tryMergeLandmarks(LandmarkPolyline2DPtrList& _lmk_list, const Scalar& _sq_dist_tol)
 {
     std::cout << "LandmarkPolyline2D::tryMergeLandmarks\n";
     assert(_lmk_list.size() >= 2);
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index cb5577914e4632f0725126cedb350a01ae63acf7..b8251c3aa78637fdaa55b2a670a774bbadf9fa5d 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -6,14 +6,15 @@
 #include "base/capture/capture_velocity.h"
 
 #include "base/rotations.h"
-//#include "base/factor/factor_odom_2D.h"
+#include "base/factor/factor_odom_2D.h"
+#include "base/factor/factor_diff_drive.h"
 #include "base/feature/feature_diff_drive.h"
 
 namespace wolf
 {
 
 ProcessorDiffDrive::ProcessorDiffDrive(SensorDiffDrivePtr _sensor_diff_drive_ptr, ProcessorParamsDiffDrivePtr _params) :
-  ProcessorMotion("DIFF DRIVE", 3, 3, 3, 2, (_sensor_diff_drive_ptr->getIntrinsicPtr()->isFixed() ? 0 : 3), _params),
+  ProcessorMotion("DIFF DRIVE", 3, 3, 3, 2, (_sensor_diff_drive_ptr->getIntrinsic()->isFixed() ? 0 : 3), _params),
   sensor_diff_drive_ptr_(_sensor_diff_drive_ptr),
   params_motion_diff_drive_(_params)
 {
@@ -218,12 +219,12 @@ CaptureMotionPtr ProcessorDiffDrive::createCapture(const TimeStamp& _ts,
 
 FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin)
 {
-    FactorOdom2DPtr fac_odom;
+    FactorBasePtr fac_odom;
 
     if (hasCalibration())
-        fac_odom = std::make_shared<FactorDiffDrive>(_feature, _capture_origin->getFramePtr(), shared_from_this());
+        fac_odom = std::make_shared<FactorDiffDrive>(_feature, _capture_origin->getFrame(), shared_from_this());
     else
-        fac_odom = std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFramePtr(), shared_from_this());
+        fac_odom = std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(), shared_from_this());
 
     _feature->addFactor(fac_odom);
     _capture_origin->getFrame()->addConstrainedBy(fac_odom);
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index 6ee88052df6ff322990abc8cd8688dc2031b50e1..1e8cc96d941d5fe3f47ea91ff8e11a3925ecf25d 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -1,5 +1,5 @@
-#include "base/constraint/constraint_gnss_fix_2D.h"
-#include "base/constraint/constraint_gnss_fix_3D.h"
+#include "base/factor/factor_gnss_fix_2D.h"
+#include "base/factor/factor_gnss_fix_3D.h"
 #include "base/feature/feature_gnss_fix.h"
 #include "base/processor/processor_gnss_fix.h"
 #include "base/capture/capture_gnss_fix.h"
@@ -34,7 +34,7 @@ void ProcessorGnssFix::process(CaptureBasePtr _capture_ptr)
     //std::cout << "WOLF: gnss fix in ENU coordinates: " << p_enu.transpose() << std::endl;
 
     FrameBasePtr new_frame_ptr = nullptr;
-    bool new_ctr_created = false;
+    bool new_fac_created = false;
 
     // ALREADY CREATED KF
     PackKeyFramePtr KF_pack = kf_pack_buffer_.selectPack( _capture_ptr, params_->time_tolerance);
@@ -62,25 +62,25 @@ void ProcessorGnssFix::process(CaptureBasePtr _capture_ptr)
         FeatureBasePtr ftr_ptr = last_capture_ptr_->addFeature(std::make_shared<FeatureGnssFix>(last_capture_ptr_->getData(),last_capture_ptr_->getDataCovariance()));
 
         // EMPLACE CONSTRAINT
-        emplaceConstraint(ftr_ptr);
-        new_ctr_created = true;
+        emplaceFactor(ftr_ptr);
+        new_fac_created = true;
 
         // outlier rejection
         if (sensor_gnss_ptr_->isEnuDefined() && sensor_gnss_ptr_->isEnuMapInitialized())
-            if (rejectOutlier(ftr_ptr->getConstraintList().front()))
-                new_ctr_created = false;
+            if (rejectOutlier(ftr_ptr->getFactorList().front()))
+                new_fac_created = false;
 
         // store last KF
-        if (new_ctr_created)
+        if (new_fac_created)
             last_gnss_fix_KF_= new_frame_ptr;
 
-        //WOLF_DEBUG("Constraint established");
+        //WOLF_DEBUG("Factor established");
     }
 
     // INITIALIZE ENU IF:
     //      1 - not initialized
-    //      2 - constraint established
-    if (!sensor_gnss_ptr_->isEnuDefined() && new_ctr_created)
+    //      2 - factor established
+    if (!sensor_gnss_ptr_->isEnuDefined() && new_fac_created)
     {
         sensor_gnss_ptr_->setEcefEnu(last_capture_ptr_->getData());
 
@@ -89,62 +89,62 @@ void ProcessorGnssFix::process(CaptureBasePtr _capture_ptr)
 
     // INITIALIZE ENU_MAP IF 2 NECESSARY CONDITIONS:
     //      1 - not initialized
-    //      2 - two constraints established (first and current)
-    if (!sensor_gnss_ptr_->isEnuMapInitialized() && first_capture_ptr_ && new_ctr_created)
+    //      2 - two factors established (first and current)
+    if (!sensor_gnss_ptr_->isEnuMapInitialized() && first_capture_ptr_ && new_fac_created)
     {
-        sensor_gnss_ptr_->initializeEnuMap(first_capture_ptr_->getFramePtr()->getState(), first_capture_ptr_->getData(),
-                                           last_capture_ptr_->getFramePtr()->getState(), last_capture_ptr_->getData());
+        sensor_gnss_ptr_->initializeEnuMap(first_capture_ptr_->getFrame()->getState(), first_capture_ptr_->getData(),
+                                           last_capture_ptr_->getFrame()->getState(), last_capture_ptr_->getData());
 
         WOLF_INFO("ProcessorGnssFix: ENU-MAP initialized.")
     }
 
-    // Store the first capture that established a constraint
-    if (new_ctr_created && first_capture_ptr_ == nullptr)
+    // Store the first capture that established a factor
+    if (new_fac_created && first_capture_ptr_ == nullptr)
         first_capture_ptr_ = last_capture_ptr_;
 }
 
-void ProcessorGnssFix::emplaceConstraint(FeatureBasePtr& ftr_ptr)
+void ProcessorGnssFix::emplaceFactor(FeatureBasePtr& ftr_ptr)
 {
     // CREATE CONSTRAINT --------------------
-    //WOLF_DEBUG("creating the constraint...");
-    ConstraintBasePtr new_ctr_ptr = nullptr;
+    //WOLF_DEBUG("creating the factor...");
+    FactorBasePtr new_fac_ptr = nullptr;
     // 2D
     if (getProblem()->getDim() == 2)
-        new_ctr_ptr = std::make_shared<ConstraintGnssFix2D>(ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false);
+        new_fac_ptr = std::make_shared<FactorGnssFix2D>(ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false);
     // 3D
     else
-        new_ctr_ptr = std::make_shared<ConstraintGnssFix3D>(ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false);
+        new_fac_ptr = std::make_shared<FactorGnssFix3D>(ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false);
 
     // ADD CONSTRAINT --------------------
-    //WOLF_DEBUG("adding the constraint...");
-    ftr_ptr->addConstraint(new_ctr_ptr);
+    //WOLF_DEBUG("adding the factor...");
+    ftr_ptr->addFactor(new_fac_ptr);
 }
 
-bool ProcessorGnssFix::rejectOutlier(ConstraintBasePtr ctr_ptr)
+bool ProcessorGnssFix::rejectOutlier(FactorBasePtr fac_ptr)
 {
     // Cast feature
-    auto gnss_ftr_ptr = std::static_pointer_cast<FeatureGnssFix>(ctr_ptr->getFeaturePtr());
+    auto gnss_ftr_ptr = std::static_pointer_cast<FeatureGnssFix>(fac_ptr->getFeature());
     // copy states
-    Eigen::VectorXs x(gnss_ftr_ptr->getCapturePtr()->getFramePtr()->getPPtr()->getState());
-    Eigen::VectorXs o(gnss_ftr_ptr->getCapturePtr()->getFramePtr()->getPPtr()->getState());
-    Eigen::VectorXs x_antena(sensor_gnss_ptr_->getStateBlockPtr(0)->getState());
-    Eigen::VectorXs t_ENU_map(sensor_gnss_ptr_->getEnuMapTranslationStatePtr()->getState());
-    Eigen::VectorXs roll_ENU_map(sensor_gnss_ptr_->getEnuMapRollStatePtr()->getState());
-    Eigen::VectorXs pitch_ENU_map(sensor_gnss_ptr_->getEnuMapPitchStatePtr()->getState());
-    Eigen::VectorXs yaw_ENU_map(sensor_gnss_ptr_->getEnuMapYawStatePtr()->getState());
+    Eigen::VectorXs x(gnss_ftr_ptr->getCapture()->getFrame()->getP()->getState());
+    Eigen::VectorXs o(gnss_ftr_ptr->getCapture()->getFrame()->getP()->getState());
+    Eigen::VectorXs x_antena(sensor_gnss_ptr_->getStateBlock(0)->getState());
+    Eigen::VectorXs t_ENU_map(sensor_gnss_ptr_->getEnuMapTranslationState()->getState());
+    Eigen::VectorXs roll_ENU_map(sensor_gnss_ptr_->getEnuMapRollState()->getState());
+    Eigen::VectorXs pitch_ENU_map(sensor_gnss_ptr_->getEnuMapPitchState()->getState());
+    Eigen::VectorXs yaw_ENU_map(sensor_gnss_ptr_->getEnuMapYawState()->getState());
     // create Scalar* array of a copy of the state
     Scalar* parameters[7] = {x.data(), o.data(), x_antena.data(), t_ENU_map.data(), roll_ENU_map.data(),
                              pitch_ENU_map.data(), yaw_ENU_map.data()};
     // create residuals pointer
     Eigen::VectorXs residuals(3);
-    // evaluate the constraint in this state
-    ctr_ptr->evaluate(parameters, residuals.data(), nullptr);
+    // evaluate the factor in this state
+    fac_ptr->evaluate(parameters, residuals.data(), nullptr);
     // discard if residual too high evaluated at the current estimation
     if (residuals.norm() > 1e3)
     {
-        WOLF_WARN("Discarding GNSS FIX Constraint, considered OUTLIER");
-        WOLF_TRACE("Feature: ", ctr_ptr->getMeasurement().transpose(),"\nError: ",(ctr_ptr->getMeasurementSquareRootInformationUpper().inverse()*residuals).transpose());
-        ctr_ptr->remove();
+        WOLF_WARN("Discarding GNSS FIX Factor, considered OUTLIER");
+        WOLF_TRACE("Feature: ", fac_ptr->getMeasurement().transpose(),"\nError: ",(fac_ptr->getMeasurementSquareRootInformationUpper().inverse()*residuals).transpose());
+        fac_ptr->remove();
         return true;
     }
     return false;
@@ -165,7 +165,7 @@ bool ProcessorGnssFix::voteForKeyFrame()
 void ProcessorGnssFix::configure(SensorBasePtr _sensor)
 {
     sensor_gnss_ptr_ = std::static_pointer_cast<SensorGnss>(_sensor);
-    setSensorPtr(_sensor);
+    setSensor(_sensor);
 };
 
 ProcessorBasePtr ProcessorGnssFix::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr)
diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp
index 9a2600a9eab3768bc1cad6c852607505b43fc183..179933a0789e5bc268351593f3fba3eaaf6349ef 100644
--- a/src/processor/processor_gnss_single_diff.cpp
+++ b/src/processor/processor_gnss_single_diff.cpp
@@ -1,4 +1,4 @@
-#include "base/constraint/constraint_gnss_single_diff_2D.h"
+#include "base/factor/factor_gnss_single_diff_2D.h"
 #include "base/feature/feature_gnss_single_diff.h"
 #include "base/processor/processor_gnss_single_diff.h"
 #include "base/capture/capture_gnss_single_diff.h"
@@ -30,7 +30,7 @@ void ProcessorGnssSingleDiff::process(CaptureBasePtr _capture_ptr)
 
     // ALREADY CREATED KF
     PackKeyFramePtr KF_pack = kf_pack_buffer_.selectPack( _capture_ptr, params_->time_tolerance);
-    if (KF_pack && KF_pack->key_frame != last_capture_ptr_->getOriginFramePtr())
+    if (KF_pack && KF_pack->key_frame != last_capture_ptr_->getOriginFrame())
     {
         new_frame_ptr = KF_pack->key_frame;
         WOLF_DEBUG( "PR ",getName()," - capture ", _capture_ptr->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp());
@@ -54,20 +54,20 @@ void ProcessorGnssSingleDiff::process(CaptureBasePtr _capture_ptr)
         FeatureBasePtr ftr_ptr = last_capture_ptr_->addFeature(std::make_shared<FeatureGnssSingleDiff>(last_capture_ptr_->getData(),last_capture_ptr_->getDataCovariance()));
 
         // ADD CONSTRAINT
-        ConstraintBasePtr ctr_ptr;
-        //WOLF_DEBUG("adding the constraint...");
+        FactorBasePtr fac_ptr;
+        //WOLF_DEBUG("adding the factor...");
         // 2D
         if (getProblem()->getDim() == 2)
-            ctr_ptr = ftr_ptr->addConstraint(std::make_shared<ConstraintGnssSingleDiff2D>(ftr_ptr, last_capture_ptr_->getOriginFramePtr(), sensor_gnss_ptr_, shared_from_this()));
+            fac_ptr = ftr_ptr->addFactor(std::make_shared<FactorGnssSingleDiff2D>(ftr_ptr, last_capture_ptr_->getOriginFrame(), sensor_gnss_ptr_, shared_from_this()));
         // 3D TODO
         else
             std::runtime_error("Single Differences in 3D not implemented yet.");
-        //    ctr_ptr = ftr_ptr->addConstraint(std::make_shared<ConstraintGnssSingleDiff3D>(ftr_ptr, sensor_gnss_ptr_, shared_from_this()));
+        //    fac_ptr = ftr_ptr->addFactor(std::make_shared<FactorGnssSingleDiff3D>(ftr_ptr, sensor_gnss_ptr_, shared_from_this()));
 
         // add constrained-by to the origin
-        last_capture_ptr_->getOriginFramePtr()->addConstrainedBy(ctr_ptr);
+        last_capture_ptr_->getOriginFrame()->addConstrainedBy(fac_ptr);
 
-        //WOLF_DEBUG("Constraint established");
+        //WOLF_DEBUG("Factor established");
     }
 }
 
@@ -80,7 +80,7 @@ bool ProcessorGnssSingleDiff::voteForKeyFrame()
 void ProcessorGnssSingleDiff::configure(SensorBasePtr _sensor)
 {
     sensor_gnss_ptr_ = std::static_pointer_cast<SensorGnss>(_sensor);
-    setSensorPtr(_sensor);
+    setSensor(_sensor);
 };
 
 ProcessorBasePtr ProcessorGnssSingleDiff::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr)
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index ca17b1ea6a5d64f6f1869d1c78426a398859e304..a556c3e8328daacd044b6cf22f90e48d456f97a8 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -554,7 +554,7 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
         {
             // Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
             capture_motion = std::static_pointer_cast<CaptureMotion>(capture);
-            if (capture_motion->getOriginFramePtr() && !capture_motion->getBuffer().get().empty() && _ts >= capture_motion->getBuffer().get().front().ts_)
+            if (capture_motion->getOriginFrame() && !capture_motion->getBuffer().get().empty() && _ts >= capture_motion->getBuffer().get().front().ts_)
                 // Found time stamp satisfying rule 3 above !! ==> break for loop
                 break;
             else
diff --git a/src/processor/processor_polyline.cpp b/src/processor/processor_polyline.cpp
index 901543c14c5603dd74fe1b4208c6b2f2b0bf96d9..38c9794c87182363cbca8eb56346ecf0d830ca39 100644
--- a/src/processor/processor_polyline.cpp
+++ b/src/processor/processor_polyline.cpp
@@ -70,19 +70,19 @@ void ProcessorPolyline::process(CaptureBasePtr _incoming_ptr)
         // Create landmarks from new_features_last_
         processNew();
 
-        // Establish constraints from last
-        establishConstraints();
+        // Establish factors from last
+        establishFactors();
     }
     // OTHER TIMES
     else
     {
-        assert(last_ptr_->getFramePtr() && "last_ not linked to any frame in OTHER TIMES");
+        assert(last_ptr_->getFrame() && "last_ not linked to any frame in OTHER TIMES");
         //WOLF_DEBUG("OTHER TIMES");
 
-        // 1. First we track the known Features and create new constraints as needed
+        // 1. First we track the known Features and create new factors as needed
 
         // Find landmarks in new_features_incoming_ (and extract from it)
-        findLandmarks(getProblem()->getMapPtr()->getLandmarkList(), known_features_incoming_, matches_landmark_from_incoming_);
+        findLandmarks(getProblem()->getMap()->getLandmarkList(), known_features_incoming_, matches_landmark_from_incoming_);
         //WOLF_DEBUG("incoming new_features: " , new_features_incoming_.size());
         //WOLF_DEBUG("incoming known features: ", known_features_incoming_.size());
 
@@ -93,11 +93,11 @@ void ProcessorPolyline::process(CaptureBasePtr _incoming_ptr)
         //   - Last is not KF
         //   - KF has not another capture of the same sensor
         PackKeyFramePtr KF_pack   = kf_pack_buffer_.selectPack( last_ptr_, params_->time_tolerance);
-        bool KF_has_capture = KF_pack && (KF_pack->key_frame->getCaptureOf(getSensorPtr()) != nullptr);
-        bool create_KF      = !KF_has_capture && !last_ptr_->getFramePtr()->isKey() && voteForKeyFrame() && permittedKeyFrame();
-        bool existing_KF    = !KF_has_capture && !last_ptr_->getFramePtr()->isKey() && KF_pack;
+        bool KF_has_capture = KF_pack && (KF_pack->key_frame->getCaptureOf(getSensor()) != nullptr);
+        bool create_KF      = !KF_has_capture && !last_ptr_->getFrame()->isKey() && voteForKeyFrame() && permittedKeyFrame();
+        bool existing_KF    = !KF_has_capture && !last_ptr_->getFrame()->isKey() && KF_pack;
 
-        FrameBasePtr closest_key_frm_to_last = getProblem()->getTrajectoryPtr()->closestKeyFrameToTimeStamp(last_ptr_->getTimeStamp());
+        FrameBasePtr closest_key_frm_to_last = getProblem()->getTrajectory()->closestKeyFrameToTimeStamp(last_ptr_->getTimeStamp());
 
         //WOLF_DEBUG("last is KF? ", (last_is_KF ? "YES" : "NO"));
 
@@ -121,24 +121,24 @@ void ProcessorPolyline::process(CaptureBasePtr _incoming_ptr)
                 // Make the last Capture's Frame a KeyFrame
                 setKeyFrame(last_ptr_);
 
-                WOLF_DEBUG("PR ",getName() ," - Set KEY to last F", last_ptr_->getFramePtr()->id());
+                WOLF_DEBUG("PR ",getName() ," - Set KEY to last F", last_ptr_->getFrame()->id());
             }
             // existing KF within the time tolerance
             else
             {
                 // Move incomming to the last F
-                last_ptr_->getFramePtr()->addCapture(incoming_ptr_);
-                incoming_ptr_->getFramePtr()->setTimeStamp(incoming_ptr_->getTimeStamp());
+                last_ptr_->getFrame()->addCapture(incoming_ptr_);
+                incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp());
 
                 // Move last to the existing KF
-                last_ptr_->getFramePtr()->unlinkCapture(last_ptr_);
+                last_ptr_->getFrame()->unlinkCapture(last_ptr_);
                 KF_pack->key_frame->addCapture(last_ptr_);
 
-                WOLF_DEBUG("PR ",getName() ," - Last capture ", last_ptr_->id(), " appended to existing KF ", last_ptr_->getFramePtr()->id());
+                WOLF_DEBUG("PR ",getName() ," - Last capture ", last_ptr_->id(), " appended to existing KF ", last_ptr_->getFrame()->id());
             }
 
-            // Establish constraints for last matches
-            establishConstraints();
+            // Establish factors for last matches
+            establishFactors();
 
             // Reset the Tracker
             // Move matches and new features from incoming to last
@@ -159,7 +159,7 @@ void ProcessorPolyline::process(CaptureBasePtr _incoming_ptr)
             known_features_last_ = std::move(known_features_incoming_);
             //WOLF_DEBUG("new_features_last_ " , new_features_last_.size());
 
-            if (last_ptr_->getFramePtr()->isKey())
+            if (last_ptr_->getFrame()->isKey())
             {
                 //WOLF_DEBUG("Last is in a KF \n");
                 // Make a non-key-frame to hold incoming
@@ -171,8 +171,8 @@ void ProcessorPolyline::process(CaptureBasePtr _incoming_ptr)
             {
                 //WOLF_DEBUG("Last is NOT in a KF \n");
                 // Add incoming Capture to the tracker's last Frame
-                last_ptr_->getFramePtr()->addCapture(incoming_ptr_);
-                incoming_ptr_->getFramePtr()->setTimeStamp(incoming_ptr_->getTimeStamp());
+                last_ptr_->getFrame()->addCapture(incoming_ptr_);
+                incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp());
                 // discard last capture
                 last_ptr_->remove();
             }
@@ -191,7 +191,7 @@ void ProcessorPolyline::process(CaptureBasePtr _incoming_ptr)
     while (!merge_candidates_list_.empty())
     {
         // load next list of candidates
-        LandmarkPolyline2DList merge_candidates = std::move(merge_candidates_list_.front());
+        LandmarkPolyline2DPtrList merge_candidates = std::move(merge_candidates_list_.front());
         merge_candidates_list_.pop_front();
 
         // change already merged lmks with the corresponding remaining lmk
@@ -242,7 +242,7 @@ bool ProcessorPolyline::voteForKeyFrame()
     {
         assert(matches_landmark_from_last_.find(new_ftr) != matches_landmark_from_last_.end());
         assert(matches_landmark_from_last_[new_ftr]->landmark_ptr_->getConstrainedByList().size() > 0);
-        if (std::fabs(last_ptr_->getTimeStamp() - matches_landmark_from_last_[new_ftr]->landmark_ptr_->getConstrainedByList().back()->getCapturePtr()->getTimeStamp()) > params_->loop_time_th)
+        if (std::fabs(last_ptr_->getTimeStamp() - matches_landmark_from_last_[new_ftr]->landmark_ptr_->getConstrainedByList().back()->getCapture()->getTimeStamp()) > params_->loop_time_th)
         {
             WOLF_DEBUG("PR ",getName(),"------------- NEW KEY FRAME: Option 2 - Loop closure");
             return true;
@@ -253,13 +253,13 @@ bool ProcessorPolyline::voteForKeyFrame()
 
 // IMPLEMENTATION /////////////////////////////////////////////////////////////////////////////////////////////////
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-void ProcessorPolyline::extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, FeatureBaseList& _polyline_list)
+void ProcessorPolyline::extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _polyline_list)
 {
     //WOLF_DEBUG("ProcessorPolyline::extractPolylines: ");
     // TODO: sort corners by bearing
     std::list<laserscanutils::Polyline> polylines;
 
-    line_finder_.findPolylines(_capture_laser_ptr->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensorPtr()))->getScanParams(), polylines);
+    line_finder_.findPolylines(_capture_laser_ptr->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(), polylines);
     //WOLF_DEBUG("Polylines extracted: ", polylines.size());
 
     for (auto&& pl : polylines)
@@ -271,8 +271,8 @@ void ProcessorPolyline::extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, F
     }
 }
 
-unsigned int ProcessorPolyline::findLandmarks(const LandmarkBaseList& _landmarks_searched,
-                                              FeatureBaseList& _features_found,
+unsigned int ProcessorPolyline::findLandmarks(const LandmarkBasePtrList& _landmarks_searched,
+                                              FeatureBasePtrList& _features_found,
                                               LandmarkMatchMap& _feature_landmark_correspondences)
 {
     //std::cout << "ProcessorPolyline::findLandmarks: " << _landmarks_searched.size() << " features: " << new_features_incoming_.size()  << std::endl;
@@ -295,7 +295,7 @@ unsigned int ProcessorPolyline::findLandmarks(const LandmarkBaseList& _landmarks
     LandmarkMatchPolyline2DPtr best_match = nullptr;
     FeaturePolyline2DPtr pl_ftr;
     LandmarkPolyline2DPtr pl_lmk;
-    LandmarkPolyline2DList merging_candidates;
+    LandmarkPolyline2DPtrList merging_candidates;
     auto feature_it = new_features_incoming_.begin();
 
     // iterate over all polylines features
@@ -372,7 +372,7 @@ unsigned int ProcessorPolyline::findLandmarks(const LandmarkBaseList& _landmarks
             merge_candidates_list_.push_back(std::move(merging_candidates));
     }
 
-    //std::cout << matches_landmark_from_incoming_.size() << " landmarks found from " << getProblem()->getMapPtr()->getLandmarkList().size() << std::endl;
+    //std::cout << matches_landmark_from_incoming_.size() << " landmarks found from " << getProblem()->getMap()->getLandmarkList().size() << std::endl;
     //std::cout << "done" << std::endl;
     return matches_landmark_from_incoming_.size();
 }
@@ -440,16 +440,16 @@ LandmarkBasePtr ProcessorPolyline::createLandmark(FeatureBasePtr _feature_ptr)
     //std::cout << "done" << std::endl;
 }
 
-void ProcessorPolyline::establishConstraints()
+void ProcessorPolyline::establishFactors()
 {
     //TODO: update with new index in landmarks
-    //std::cout << "ProcessorPolyline::establishConstraints" << std::endl;
-    //std::cout << "\tlast_ id: " << last_ptr_->id() << " linked to frame: " << last_ptr_->getFramePtr()->id() << " is linked to problem? " << (last_ptr_->getProblem() ? "YES" : "NO") << std::endl;
-    unsigned int N_constraints = 0;
+    //std::cout << "ProcessorPolyline::establishFactors" << std::endl;
+    //std::cout << "\tlast_ id: " << last_ptr_->id() << " linked to frame: " << last_ptr_->getFrame()->id() << " is linked to problem? " << (last_ptr_->getProblem() ? "YES" : "NO") << std::endl;
+    unsigned int N_factors = 0;
     LandmarkMatchPolyline2DPtr pl_match;
     FeaturePolyline2DPtr     pl_ftr;
     LandmarkPolyline2DPtr    pl_lmk;
-    LandmarkPolyline2DList   modified_lmks;
+    LandmarkPolyline2DPtrList   modified_lmks;
 
     for (auto last_ftr : last_ptr_->getFeatureList())
     {
@@ -647,15 +647,15 @@ void ProcessorPolyline::establishConstraints()
         assert(pl_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
         assert(pl_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
         assert((pl_lmk->getNPoints() >= pl_ftr->getNPoints() + pl_match->landmark_from_id_ || pl_lmk->isClosed()) && "Landmark didn't grow properly");
-        //std::cout << "Establishing constraint between" << std::endl;
-        //std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapturePtr() ? pl_ftr->getCapturePtr()->id() : 9999999999999999) << " is liked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl;
+        //std::cout << "Establishing factor between" << std::endl;
+        //std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapture() ? pl_ftr->getCapture()->id() : 9999999999999999) << " is liked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl;
         //std::cout << "\tlandmark " << pl_lmk->id() << std::endl;
         //std::cout << "\tmatch from feature point " << pl_match->feature_from_id_ << std::endl;
         //std::cout << "\tmatch to feature point " << pl_match->feature_to_id_ << std::endl;
         //std::cout << "\tmatch from landmark point " << pl_match->landmark_from_id_ << std::endl;
         //std::cout << "\tmatch to landmark point " << pl_match->landmark_to_id_ << std::endl;
 
-        // Constraints for all feature points
+        // Factors for all feature points
         int lmk_point_id = pl_match->landmark_from_id_;
 
         for (int ftr_point_id = 0; ftr_point_id < pl_ftr->getNPoints(); ftr_point_id++)
@@ -665,47 +665,47 @@ void ProcessorPolyline::establishConstraints()
 
             // First not defined point
             if (ftr_point_id == 0 && !pl_ftr->isFirstDefined())
-                // first point to line constraint
+                // first point to line factor
             {
                 //std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getNextValidId(lmk_point_id) << std::endl;
-                assert(!(lmk_point_id == pl_lmk->getLastId() && !pl_lmk->isClosed()) && "Trying to establish a constraint for first not defined point of a feature with the last landmark point without being closed. A landmark didn't grow correctly.");
+                assert(!(lmk_point_id == pl_lmk->getLastId() && !pl_lmk->isClosed()) && "Trying to establish a factor for first not defined point of a feature with the last landmark point without being closed. A landmark didn't grow correctly.");
 
-                // emplace constraint
-                emplaceConstraintPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getNextValidId(lmk_point_id));
-                N_constraints++;
-                //std::cout << "constraint added" << std::endl;
+                // emplace factor
+                emplaceFactorPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getNextValidId(lmk_point_id));
+                N_factors++;
+                //std::cout << "factor added" << std::endl;
             }
 
             // Last not defined point
             else if (ftr_point_id == pl_ftr->getNPoints()-1 && !pl_ftr->isLastDefined())
-                // last point to line constraint
+                // last point to line factor
             {
                 //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_prev_point_id << std::endl;
-                assert(!(lmk_point_id == pl_lmk->getFirstId() && !pl_lmk->isClosed()) && "Trying to establish a constraint for last not defined point of a feature with the first landmark point without being closed. A landmark didn't grow correctly.");
+                assert(!(lmk_point_id == pl_lmk->getFirstId() && !pl_lmk->isClosed()) && "Trying to establish a factor for last not defined point of a feature with the first landmark point without being closed. A landmark didn't grow correctly.");
 
-                // emplace constraint
-                emplaceConstraintPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getPrevValidId(lmk_point_id));
-                N_constraints++;
-                //std::cout << "constraint added" << std::endl;
+                // emplace factor
+                emplaceFactorPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getPrevValidId(lmk_point_id));
+                N_factors++;
+                //std::cout << "factor added" << std::endl;
             }
 
             // Defined point
             else
-                // point to point constraint
+                // point to point factor
             {
                 //std::cout << "point-point: landmark point " << lmk_point_id << std::endl;
                 //std::cout << "landmark first id:" << pl_lmk->getFirstId() << std::endl;
                 //std::cout << "landmark last id:" << pl_lmk->getLastId() << std::endl;
                 //std::cout << "landmark n points:" << pl_lmk->getNPoints() << std::endl;
-                emplaceConstraintPoint(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id);
-                //std::cout << "constraint added" << std::endl;
+                emplaceFactorPoint(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id);
+                //std::cout << "factor added" << std::endl;
             }
 
             if (ftr_point_id < pl_ftr->getNPoints()-1)
                 lmk_point_id=pl_lmk->getNextValidId(lmk_point_id);
         }
     }
-    //std::cout << N_constraints << " constraints established, trying to close and classify " << modified_lmks.size() << std::endl;
+    //std::cout << N_factors << " factors established, trying to close and classify " << modified_lmks.size() << std::endl;
 
     // Try to close & classify modified landmarks
     for (auto lmk_ptr : modified_lmks)
@@ -721,15 +721,15 @@ void ProcessorPolyline::establishConstraints()
     }
 
     // Outlier rejection
-    ConstraintBaseList new_ctr_list;
-    last_ptr_->getConstraintList(new_ctr_list);
-    for (auto ctr : new_ctr_list)
-        rejectOutlier(ctr);
+    FactorBasePtrList new_fac_list;
+    last_ptr_->getFactorList(new_fac_list);
+    for (auto fac : new_fac_list)
+        rejectOutlier(fac);
 
-    //std::cout << "ProcessorPolyline::establishConstraints: done\n";
+    //std::cout << "ProcessorPolyline::establishFactors: done\n";
 }
 
-void ProcessorPolyline::classifyPolylineLandmarks(LandmarkBaseList& _lmk_list)
+void ProcessorPolyline::classifyPolylineLandmarks(LandmarkBasePtrList& _lmk_list)
 {
     //std::cout << "ProcessorPolyline::classifyPolylineLandmarks: " << _lmk_list.size() << std::endl;
 
@@ -795,23 +795,23 @@ LandmarkMatchPolyline2DPtr ProcessorPolyline::tryMatch(const Eigen::MatrixXs& _l
     return lmk_match;
 }
 
-bool ProcessorPolyline::rejectOutlier(ConstraintBasePtr& ctr_ptr)
+bool ProcessorPolyline::rejectOutlier(FactorBasePtr& fac_ptr)
 {
-    // Cast Point2Line/Point2Point constraint
-    auto ctr_p2l_ptr = std::dynamic_pointer_cast<ConstraintPointToLine2D>(ctr_ptr);
-    auto ctr_p2p_ptr = std::dynamic_pointer_cast<ConstraintPoint2D>(ctr_ptr);
+    // Cast Point2Line/Point2Point factor
+    auto fac_p2l_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr);
+    auto fac_p2p_ptr = std::dynamic_pointer_cast<FactorPoint2D>(fac_ptr);
 
     // Cast feature and landmark
-    auto pl_ftr_ptr = std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeaturePtr());
-    auto pl_lmk_ptr = std::static_pointer_cast<LandmarkPolyline2D>(ctr_ptr->getLandmarkOtherPtr());
+    auto pl_ftr_ptr = std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature());
+    auto pl_lmk_ptr = std::static_pointer_cast<LandmarkPolyline2D>(fac_ptr->getLandmarkOther());
 
     // copy states
-    Eigen::VectorXs frameP(pl_ftr_ptr->getCapturePtr()->getFramePtr()->getPPtr()->getState());
-    Eigen::VectorXs frameO(pl_ftr_ptr->getCapturePtr()->getFramePtr()->getOPtr()->getState());
-    Eigen::VectorXs lmkOriginP(pl_lmk_ptr->getPPtr()->getState());
-    Eigen::VectorXs lmkOriginO(pl_lmk_ptr->getOPtr()->getState());
-    Eigen::VectorXs lmkPoint(pl_lmk_ptr->getPointVector((ctr_p2l_ptr ? ctr_p2l_ptr->getLandmarkPointId() : ctr_p2p_ptr->getLandmarkPointId())));
-    Eigen::VectorXs lmkPointAux((ctr_p2l_ptr ? pl_lmk_ptr->getPointVector(ctr_p2l_ptr->getLandmarkPointAuxId()) : Eigen::VectorXs::Zero(1)));
+    Eigen::VectorXs frameP(pl_ftr_ptr->getCapture()->getFrame()->getP()->getState());
+    Eigen::VectorXs frameO(pl_ftr_ptr->getCapture()->getFrame()->getO()->getState());
+    Eigen::VectorXs lmkOriginP(pl_lmk_ptr->getP()->getState());
+    Eigen::VectorXs lmkOriginO(pl_lmk_ptr->getO()->getState());
+    Eigen::VectorXs lmkPoint(pl_lmk_ptr->getPointVector((fac_p2l_ptr ? fac_p2l_ptr->getLandmarkPointId() : fac_p2p_ptr->getLandmarkPointId())));
+    Eigen::VectorXs lmkPointAux((fac_p2l_ptr ? pl_lmk_ptr->getPointVector(fac_p2l_ptr->getLandmarkPointAuxId()) : Eigen::VectorXs::Zero(1)));
 
     // create Scalar* array of a copy of the state
     Scalar* parameters[6] = {frameP.data(),
@@ -822,16 +822,16 @@ bool ProcessorPolyline::rejectOutlier(ConstraintBasePtr& ctr_ptr)
                              lmkPointAux.data()}; // last wont be used in p2p evaluate()
 
     // create residuals pointer
-    Eigen::VectorXs residuals(ctr_p2l_ptr ? 1 : 2);
+    Eigen::VectorXs residuals(fac_p2l_ptr ? 1 : 2);
 
-    // evaluate the constraint in this state (without jacobians, no need)
-    ctr_ptr->evaluate(parameters, residuals.data(), nullptr);
+    // evaluate the factor in this state (without jacobians, no need)
+    fac_ptr->evaluate(parameters, residuals.data(), nullptr);
 
     // discard if residual too high evaluated at the current estimation
     if (residuals.norm() > 1e3)
     {
-        WOLF_WARN((ctr_p2l_ptr ? "Discarding Point 2 Line Polyline Constraint, considered OUTLIER" : "Discarding Point 2 Point Polyline Constraint, considered OUTLIER"));
-        ctr_ptr->remove();
+        WOLF_WARN((fac_p2l_ptr ? "Discarding Point 2 Line Polyline Factor, considered OUTLIER" : "Discarding Point 2 Point Polyline Factor, considered OUTLIER"));
+        fac_ptr->remove();
         return true;
     }
     return false;
@@ -843,15 +843,15 @@ bool ProcessorPolyline::rejectOutlier(ConstraintBasePtr& ctr_ptr)
 void ProcessorPolyline::setKeyFrame(CaptureBasePtr _capture_ptr)
 {
 
-    assert(_capture_ptr != nullptr && _capture_ptr->getFramePtr() != nullptr && "ProcessorTracker::setKeyFrame: null capture or capture without frame");
-    if (!_capture_ptr->getFramePtr()->isKey())
+    assert(_capture_ptr != nullptr && _capture_ptr->getFrame() != nullptr && "ProcessorTracker::setKeyFrame: null capture or capture without frame");
+    if (!_capture_ptr->getFrame()->isKey())
     {
         // Set key
-        _capture_ptr->getFramePtr()->setKey();
+        _capture_ptr->getFrame()->setKey();
         // Set state to the keyframe
-        _capture_ptr->getFramePtr()->setState(getProblem()->getState(_capture_ptr->getTimeStamp()));
-        // Call the new keyframe callback in order to let the other processors to establish their constraints
-        getProblem()->keyFrameCallback(_capture_ptr->getFramePtr(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_->time_tolerance);
+        _capture_ptr->getFrame()->setState(getProblem()->getState(_capture_ptr->getTimeStamp()));
+        // Call the new keyframe callback in order to let the other processors to establish their factors
+        getProblem()->keyFrameCallback(_capture_ptr->getFrame(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_->time_tolerance);
     }
 }
 
@@ -867,11 +867,11 @@ void ProcessorPolyline::computeTransformations(const TimeStamp& _ts)
     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 (getSensorPtr()->isExtrinsicDynamic() || !getSensorPtr()->getPPtr()->isFixed()
-            || !getSensorPtr()->getOPtr()->isFixed() || !extrinsics_transformation_computed_)
+    if (getSensor()->isExtrinsicDynamic() || !getSensor()->getP()->isFixed()
+            || !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_)
     {
-        t_robot_sensor_ = getSensorPtr()->getPPtr()->getState();
-        R_robot_sensor_ = Eigen::Rotation2Ds(getSensorPtr()->getOPtr()->getState()(0)).matrix();
+        t_robot_sensor_ = getSensor()->getP()->getState();
+        R_robot_sensor_ = Eigen::Rotation2Ds(getSensor()->getO()->getState()(0)).matrix();
         extrinsics_transformation_computed_ = true;
     }
 
diff --git a/src/processor/processor_tracker_feature_corner.cpp b/src/processor/processor_tracker_feature_corner.cpp
index 9218dba63738e88a65e2fcf3572ed2a1b5757237..9a90676b67408ba352704ecb89ce0e06fc0b7838 100644
--- a/src/processor/processor_tracker_feature_corner.cpp
+++ b/src/processor/processor_tracker_feature_corner.cpp
@@ -107,14 +107,14 @@ bool ProcessorTrackerFeatureCorner::voteForKeyFrame()
     return incoming_ptr_->getFeatureList().size() < params_tracker_feature_corner_->n_corners_th;
 }
 
-unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out)
 {
     // in corners_last_ remain all not tracked corners
-    _features_incoming_out = std::move(corners_last_);
-    if (_max_features != -1 && _features_incoming_out.size() > _max_features)
-        _features_incoming_out.resize(_max_features);
+    _features_last_out = std::move(corners_last_);
+    if (_max_features != -1 && _features_last_out.size() > _max_features)
+        _features_last_out.resize(_max_features);
 
-    return _features_incoming_out.size();
+    return _features_last_out.size();
 }
 
 FactorBasePtr ProcessorTrackerFeatureCorner::createFactor(FeatureBasePtr _feature_ptr,
diff --git a/src/processor/processor_tracker_feature_dummy.cpp b/src/processor/processor_tracker_feature_dummy.cpp
index 87655896dd2cc378378dd7a76b6acacf3579dfa3..aea025a2daea5962cf98ed8e0afea2330a8759f8 100644
--- a/src/processor/processor_tracker_feature_dummy.cpp
+++ b/src/processor/processor_tracker_feature_dummy.cpp
@@ -49,7 +49,7 @@ bool ProcessorTrackerFeatureDummy::voteForKeyFrame()
     return vote;
 }
 
-unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out)
 {
     unsigned int max_features = _max_features;
 
diff --git a/src/processor/processor_tracker_feature_image.cpp b/src/processor/processor_tracker_feature_image.cpp
index e6188cc896ce5406480c90478aa617b70147dc11..e8092dafb1454e53bd6ab3eb22345c76e468026a 100644
--- a/src/processor/processor_tracker_feature_image.cpp
+++ b/src/processor/processor_tracker_feature_image.cpp
@@ -244,7 +244,7 @@ bool ProcessorTrackerFeatureImage::correctFeatureDrift(const FeatureBasePtr _ori
     }
 }
 
-unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out)
 {
     cv::Rect roi;
     KeyPointVector new_keypoints;
diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp
index b90877de97f2289987ee405096c1830340e1dd9c..9c2fa6ac19d996cc682613de624af5bdae655846 100644
--- a/src/processor/processor_tracker_feature_polyline_2D.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2D.cpp
@@ -23,8 +23,8 @@ ProcessorTrackerFeaturePolyline2D::~ProcessorTrackerFeaturePolyline2D()
 {
 }
 
-unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseList& _features_last_in,
-                                                            FeatureBaseList& _features_incoming_out,
+unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBasePtrList& _features_last_in,
+                                                            FeatureBasePtrList& _features_incoming_out,
                                                             FeatureMatchMap& _feature_correspondences)
 {
     WOLF_DEBUG("PTFP ", getName(), "::trackFeatures ", _features_last_in.size());
@@ -174,7 +174,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const unsigned int& _
     T_sensor_world_last.topRightCorner(2,1) = t_sensor_world_last_;
 
     // For each new feature: Either create a landmark or match with an existent landmark
-    LandmarkBaseList new_landmarks;
+    LandmarkBasePtrList new_landmarks;
     for (auto ftr_it = std::prev(last_ptr_->getFeatureList().end(),n);
               ftr_it != last_ptr_->getFeatureList().end();
               ftr_it++)
@@ -184,7 +184,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const unsigned int& _
         WOLF_DEBUG("Processing feature: ", pl_ftr->id());
 
         // Check matching with all existing polyline landmarks
-        for (auto lmk : getProblem()->getMapPtr()->getLandmarkList())
+        for (auto lmk : getProblem()->getMap()->getLandmarkList())
             // Check for polylines landmarks that hasn't been just created
             if (lmk->getType() == "POLYLINE 2D" && std::find(new_landmarks.begin(), new_landmarks.end(), lmk) == new_landmarks.end())
                 // Store all matches consistent with T_sensor_world_last in best_lmk_matches sorted by difference from T_sensor_world_last
@@ -225,7 +225,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const unsigned int& _
             // store landmark candidates to be merged
             if (best_lmk_matches.size() > 1)
             {
-                LandmarkPolyline2DList merge_candidates;
+                LandmarkPolyline2DPtrList merge_candidates;
                 for (auto lmk_match_pair_it : best_lmk_matches)
                     merge_candidates.push_back(std::static_pointer_cast<LandmarkPolyline2D>(lmk_match_pair_it.second->landmark_ptr_));
                 merge_candidates_list_.push_back(merge_candidates);
@@ -261,38 +261,26 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const unsigned int& _
     return n;
 }
 
-unsigned int ProcessorTrackerFeaturePolyline2D::detectNewFeatures(const unsigned int& _max_new_features,
-                                                                  FeatureBaseList& _features_last_out)
+unsigned int ProcessorTrackerFeaturePolyline2D::detectNewFeatures(const int& _max_new_features,
+                                                                  FeatureBasePtrList& _features_last_out)
 {
     WOLF_DEBUG("PTFP ", getName(), "::detectNewFeatures (", all_features_last_.size(), " in all_features_last_)");
-    int prev_size = _features_last_out.size();
 
-    if (_features_last_out == new_features_last_)
-    {
-        _features_last_out.splice(_features_last_out.end(), all_features_last_);
+    _features_last_out = std::move(all_features_last_);
+    if (_max_new_features != -1 && _features_last_out.size() > _max_new_features)
+        _features_last_out.resize(_max_new_features);
 
-        // 0 means unlimited new features
-        if (_max_new_features != 0 && _features_last_out.size() > prev_size + _max_new_features)
-        {
-            _features_last_out.resize(prev_size + _max_new_features);
-            WOLF_DEBUG(_max_new_features, " were provided (max new features)");
-            return _max_new_features;
-        }
-        WOLF_DEBUG(_features_last_out.size()-prev_size, " were provided");
-        return _features_last_out.size()-prev_size;
-    }
-    else
-        std::runtime_error("asking for new features not from last");
+    WOLF_DEBUG(_features_last_out.size(), " were provided");
 
-    return 0;
+    return _features_last_out.size();
 }
 
-void ProcessorTrackerFeaturePolyline2D::establishConstraints()
+void ProcessorTrackerFeaturePolyline2D::establishFactors()
 {
-    WOLF_DEBUG("PTFP ", getName(), "::establishConstraints: ");
-    unsigned int N_constraints = 0;
+    WOLF_DEBUG("PTFP ", getName(), "::establishFactors: ");
+    unsigned int N_factors = 0;
 
-    // Create a constraint for each match in last features
+    // Create a factor for each match in last features
     auto ftr_it = last_ptr_->getFeatureList().begin();
     while (ftr_it != last_ptr_->getFeatureList().end())
     {
@@ -343,22 +331,22 @@ void ProcessorTrackerFeaturePolyline2D::establishConstraints()
                 modified_lmks_.push_back(pl_lmk);
 
         // ESTABLISH CONSTRAINTS
-        WOLF_DEBUG("\tEstablishing constraints..");
+        WOLF_DEBUG("\tEstablishing factors..");
         // checks
         assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
         assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
         assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly");
         assert(lmk_match->check() && "Landmark didn't grow properly");
 
-        std::cout << "Establishing constraint between" << std::endl;
-        std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapturePtr() ? pl_ftr->getCapturePtr()->id() : 9999999999999999) << " is linked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl;
+        std::cout << "Establishing factor between" << std::endl;
+        std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapture() ? pl_ftr->getCapture()->id() : 9999999999999999) << " is linked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl;
         std::cout << "\tlandmark " << pl_lmk->id() << std::endl;
         std::cout << "\tmatch from feature point " << lmk_match->feature_from_id_ << std::endl;
         std::cout << "\tmatch to feature point " << lmk_match->feature_to_id_ << std::endl;
         std::cout << "\tmatch from landmark point " << lmk_match->landmark_from_id_ << std::endl;
         std::cout << "\tmatch to landmark point " << lmk_match->landmark_to_id_ << std::endl;
 
-        // Constraints for all feature points
+        // Factors for all feature points
         int lmk_point_id = lmk_match->landmark_from_id_;
         for (int ftr_point_id = 0; ftr_point_id < pl_ftr->getNPoints(); ftr_point_id++)
         {
@@ -367,41 +355,41 @@ void ProcessorTrackerFeaturePolyline2D::establishConstraints()
 
             // First not defined point
             if (ftr_point_id == 0 && !pl_ftr->isFirstDefined())
-                // first point to line constraint
+                // first point to line factor
             {
                 std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getNextValidId(lmk_point_id) << std::endl;
-                assert(!(lmk_point_id == pl_lmk->getLastId() && !pl_lmk->isClosed()) && "Trying to establish a constraint for first not defined point of a feature with the last landmark point without being closed. A landmark didn't grow correctly.");
+                assert(!(lmk_point_id == pl_lmk->getLastId() && !pl_lmk->isClosed()) && "Trying to establish a factor for first not defined point of a feature with the last landmark point without being closed. A landmark didn't grow correctly.");
 
-                // emplace constraint
-                emplaceConstraintPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getNextValidId(lmk_point_id));
-                N_constraints++;
-                std::cout << "constraint added" << std::endl;
+                // emplace factor
+                emplaceFactorPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getNextValidId(lmk_point_id));
+                N_factors++;
+                std::cout << "factor added" << std::endl;
             }
 
             // Last not defined point
             else if (ftr_point_id == pl_ftr->getNPoints()-1 && !pl_ftr->isLastDefined())
-                // last point to line constraint
+                // last point to line factor
             {
                 std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getPrevValidId(lmk_point_id) << std::endl;
-                assert(!(lmk_point_id == pl_lmk->getFirstId() && !pl_lmk->isClosed()) && "Trying to establish a constraint for last not defined point of a feature with the first landmark point without being closed. A landmark didn't grow correctly.");
+                assert(!(lmk_point_id == pl_lmk->getFirstId() && !pl_lmk->isClosed()) && "Trying to establish a factor for last not defined point of a feature with the first landmark point without being closed. A landmark didn't grow correctly.");
 
-                // emplace constraint
-                emplaceConstraintPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getPrevValidId(lmk_point_id));
-                N_constraints++;
-                std::cout << "constraint added" << std::endl;
+                // emplace factor
+                emplaceFactorPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getPrevValidId(lmk_point_id));
+                N_factors++;
+                std::cout << "factor added" << std::endl;
             }
 
             // Defined point
             else
-                // point to point constraint
+                // point to point factor
             {
                 std::cout << "point-point: landmark point " << lmk_point_id << std::endl;
                 //std::cout << "landmark first id:" << pl_lmk->getFirstId() << std::endl;
                 //std::cout << "landmark last id:" << pl_lmk->getLastId() << std::endl;
                 //std::cout << "landmark n points:" << pl_lmk->getNPoints() << std::endl;
-                emplaceConstraintPoint(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id);
-                N_constraints++;
-                std::cout << "constraint added" << std::endl;
+                emplaceFactorPoint(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id);
+                N_factors++;
+                std::cout << "factor added" << std::endl;
             }
 
             // Next lmk point
@@ -411,10 +399,10 @@ void ProcessorTrackerFeaturePolyline2D::establishConstraints()
         // next ftr
         ftr_it++;
     }
-    std::cout << N_constraints << " constraints established" << std::endl;
+    std::cout << N_factors << " factors established" << std::endl;
 }
 
-void ProcessorTrackerFeaturePolyline2D::emplaceConstraintPointToLine(FeaturePolyline2DPtr _polyline_feature,
+void ProcessorTrackerFeaturePolyline2D::emplaceFactorPointToLine(FeaturePolyline2DPtr _polyline_feature,
                                                                      LandmarkPolyline2DPtr _polyline_landmark,
                                                                      const int& _ftr_point_id,
                                                                      const int& _lmk_point_id,
@@ -423,24 +411,24 @@ void ProcessorTrackerFeaturePolyline2D::emplaceConstraintPointToLine(FeaturePoly
     assert(_polyline_landmark->isValidId(_lmk_point_id) && _polyline_landmark->isValidId(_lmk_prev_point_id));
 
     // CREATE CONSTRAINT --------------------
-    ConstraintBasePtr new_ctr = std::make_shared<ConstraintPointToLine2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id, _lmk_prev_point_id);
+    FactorBasePtr new_fac = std::make_shared<FactorPointToLine2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id, _lmk_prev_point_id);
 
     // ADD CONSTRAINT --------------------
-    _polyline_feature->addConstraint(new_ctr);
-    _polyline_landmark->addConstrainedBy(new_ctr);
+    _polyline_feature->addFactor(new_fac);
+    _polyline_landmark->addConstrainedBy(new_fac);
 }
 
-void ProcessorTrackerFeaturePolyline2D::emplaceConstraintPoint(FeaturePolyline2DPtr _polyline_feature,
+void ProcessorTrackerFeaturePolyline2D::emplaceFactorPoint(FeaturePolyline2DPtr _polyline_feature,
                                                                LandmarkPolyline2DPtr _polyline_landmark,
                                                                const int& _ftr_point_id,
                                                                const int& _lmk_point_id)
 {
     // CREATE CONSTRAINT --------------------
-    ConstraintBasePtr new_ctr = std::make_shared<ConstraintPoint2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id);
+    FactorBasePtr new_fac = std::make_shared<FactorPoint2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id);
 
     // ADD CONSTRAINT --------------------
-    _polyline_feature->addConstraint(new_ctr);
-    _polyline_landmark->addConstrainedBy(new_ctr);
+    _polyline_feature->addFactor(new_fac);
+    _polyline_landmark->addConstrainedBy(new_fac);
 }
 
 LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::createLandmark(FeatureBasePtr _feature_ptr)
@@ -696,7 +684,7 @@ void ProcessorTrackerFeaturePolyline2D::preProcess()
     // TODO: sort corners by bearing
     std::list<laserscanutils::Polyline> polylines;
 
-    line_finder_.findPolylines(std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_)->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensorPtr()))->getScanParams(), polylines);
+    line_finder_.findPolylines(std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_)->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(), polylines);
     WOLF_DEBUG("PTFP ", getName(), " Polylines extracted: ", polylines.size());
 
     for (auto&& pl : polylines)
@@ -739,7 +727,7 @@ void ProcessorTrackerFeaturePolyline2D::postProcess()
     while (!merge_candidates_list_.empty())
     {
         // load next list of candidates
-        LandmarkPolyline2DList merge_candidates = std::move(merge_candidates_list_.front());
+        LandmarkPolyline2DPtrList merge_candidates = std::move(merge_candidates_list_.front());
         merge_candidates_list_.pop_front();
 
         // change already merged lmks with the corresponding remaining lmk
@@ -817,7 +805,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline
 //    WOLF_DEBUG("last incomming pose ", T2pose2D(_T_last_incoming_prior).transpose());
 //    WOLF_DEBUG("incomming last pose ", T2pose2D(_T_last_incoming_prior.inverse()).transpose());
 //    // compute best rigid transformations
-//    MatchPolyline2DList matches = computeBestRigidTrans(_ftr_I->getPoints(),       // <--feature incoming points
+//    MatchPolyline2DPtrList matches = computeBestRigidTrans(_ftr_I->getPoints(),       // <--feature incoming points
 //                                                        _ftr_I->isFirstDefined(),
 //                                                        _ftr_I->isLastDefined(),
 //                                                        false,                     // <--feature is not closed for sure
@@ -920,7 +908,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
     }
 
 //    // compute best rigid transformation matching
-//    MatchPolyline2DList matches = computeBestRigidTrans(_lmk_ptr->computePointsGlobal(),  // <--landmark points in local coordinates
+//    MatchPolyline2DPtrList matches = computeBestRigidTrans(_lmk_ptr->computePointsGlobal(),  // <--landmark points in local coordinates
 //                                                        _lmk_ptr->isFirstDefined(),
 //                                                        _lmk_ptr->isLastDefined(),
 //                                                        _lmk_ptr->isClosed(),
@@ -1079,13 +1067,13 @@ void ProcessorTrackerFeaturePolyline2D::computeTransformations()
     Eigen::Matrix2s R_world_robot_last     = Eigen::Rotation2Ds(vehicle_pose_last(2)).matrix();
 
     // robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
-    if (getSensorPtr()->isExtrinsicDynamic() ||
-        !getSensorPtr()->getPPtr()->isFixed() ||
-        !getSensorPtr()->getOPtr()->isFixed() ||
+    if (getSensor()->isExtrinsicDynamic() ||
+        !getSensor()->getP()->isFixed() ||
+        !getSensor()->getO()->isFixed() ||
         !extrinsics_transformation_computed_)
     {
-        t_robot_sensor_ = getSensorPtr()->getPPtr()->getState();
-        R_robot_sensor_ = Eigen::Rotation2Ds(getSensorPtr()->getOPtr()->getState()(0)).matrix();
+        t_robot_sensor_ = getSensor()->getP()->getState();
+        R_robot_sensor_ = Eigen::Rotation2Ds(getSensor()->getO()->getState()(0)).matrix();
         extrinsics_transformation_computed_ = true;
     }
 
diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp
index fdcb3b1ab6f98ebc3a3cfef57c70abec963d78b7..622f733132daf2ffdf2bc00ce9f21e016da050af 100644
--- a/src/processor/processor_tracker_feature_trifocal.cpp
+++ b/src/processor/processor_tracker_feature_trifocal.cpp
@@ -81,7 +81,7 @@ bool ProcessorTrackerFeatureTrifocal::isInlier(const cv::KeyPoint& _kp_last, con
 }
 
 
-unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out)
 {
 //    // DEBUG =====================================
 //    clock_t debug_tStart;
@@ -267,7 +267,7 @@ void ProcessorTrackerFeatureTrifocal::resetDerived()
 void ProcessorTrackerFeatureTrifocal::preProcess()
 {
 //    //DEBUG
-//    WOLF_TRACE("-------- Image ", getIncomingPtr()->id(), " -- t = ", getIncomingPtr()->getTimeStamp(), " s ----------");
+//    WOLF_TRACE("-------- Image ", getIncoming()->id(), " -- t = ", getIncoming()->getTimeStamp(), " s ----------");
 
     if (!initialized_)
     {
@@ -395,8 +395,8 @@ void ProcessorTrackerFeatureTrifocal::establishFactors()
                 // get the middle feature of the track using the average of the time stamps
                 FeatureBasePtr ftr_mid = nullptr;
 
-                TimeStamp ts_first      = ftr_first->getCapturePtr()->getTimeStamp();
-                TimeStamp ts_last       = ftr_last->getCapturePtr()->getTimeStamp();
+                TimeStamp ts_first      = ftr_first->getCapture()->getTimeStamp();
+                TimeStamp ts_last       = ftr_last->getCapture()->getTimeStamp();
                 Scalar    Dt2           = (ts_last - ts_first) / 2.0;
                 TimeStamp ts_ave        = ts_first + Dt2;
 
@@ -404,9 +404,9 @@ void ProcessorTrackerFeatureTrifocal::establishFactors()
                 auto track = track_matrix_.track(trk_id);
                 for (auto ftr_it = track.begin() ; ftr_it != track.end() ; ftr_it ++)
                 {
-                    if ( ftr_it->second->getCapturePtr() != nullptr ) // have capture
+                    if ( ftr_it->second->getCapture() != nullptr ) // have capture
                     {
-                        if ( auto kf_mid = ftr_it->second->getCapturePtr()->getFramePtr() ) // have frame
+                        if ( auto kf_mid = ftr_it->second->getCapture()->getFrame() ) // have frame
                         {
                             TimeStamp ts_mid    = kf_mid->getTimeStamp();
 
@@ -424,7 +424,7 @@ void ProcessorTrackerFeatureTrifocal::establishFactors()
 
                 // Several safety checks
                 assert(ftr_mid != nullptr   && "Middle feature is nullptr!");
-                assert(ftr_mid->getCapturePtr()->getFramePtr() != nullptr   && "Middle feature's frame is nullptr!");
+                assert(ftr_mid->getCapture()->getFrame() != nullptr   && "Middle feature's frame is nullptr!");
                 assert(ftr_mid != ftr_first && "First and middle features are the same!");
                 assert(ftr_mid != ftr_last  && "Last and middle features are the same!");
 
@@ -437,7 +437,7 @@ void ProcessorTrackerFeatureTrifocal::establishFactors()
                                                                                          FAC_ACTIVE);
 
                 // link to wolf tree
-                fac         ->  setFeaturePtr   (ftr_last);
+                fac         ->  setFeature      (ftr_last);
                 ftr_first   ->  addConstrainedBy(fac);
                 ftr_mid     ->  addConstrainedBy(fac);
                 ftr_last    ->  addFactor       (fac);
diff --git a/src/processor/processor_tracker_landmark_corner.cpp b/src/processor/processor_tracker_landmark_corner.cpp
index c3cac8003660aa14149fe1fa4ba2e01927772312..24a59a5f535873ea0a7f7a8d8f8545fab9238092 100644
--- a/src/processor/processor_tracker_landmark_corner.cpp
+++ b/src/processor/processor_tracker_landmark_corner.cpp
@@ -375,14 +375,14 @@ LandmarkBasePtr ProcessorTrackerLandmarkCorner::createLandmark(FeatureBasePtr _f
                                               _feature_ptr->getMeasurement()(3));
 }
 
-unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out)
 {
     // in corners_last_ remain all not tracked corners, already computed in preprocess()
-    _features_incoming_out = std::move(corners_last_);
-    if (_max_features != -1 && _features_incoming_out.size() > _max_features)
-        _features_incoming_out.resize(_max_features);
+    _features_last_out = std::move(corners_last_);
+    if (_max_features != -1 && _features_last_out.size() > _max_features)
+        _features_last_out.resize(_max_features);
 
-    return _features_incoming_out.size();
+    return _features_last_out.size();
 }
 
 FactorBasePtr ProcessorTrackerLandmarkCorner::createFactor(FeatureBasePtr _feature_ptr,
diff --git a/src/processor/processor_tracker_landmark_image.cpp b/src/processor/processor_tracker_landmark_image.cpp
index 9977c26b643689c92bbd2183235cf4ca94d2bf90..967319385bd8e14adbe5e1e665d2a8232a308c2f 100644
--- a/src/processor/processor_tracker_landmark_image.cpp
+++ b/src/processor/processor_tracker_landmark_image.cpp
@@ -225,7 +225,7 @@ bool ProcessorTrackerLandmarkImage::voteForKeyFrame()
 //    return landmarks_tracked_ < params_tracker_landmark_image_->min_features_for_keyframe;
 }
 
-unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_incoming_out)
+unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out)
 {
     cv::Rect roi;
     KeyPointVector new_keypoints;
diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp
index 5412fd67251498cf3859d035366658fa85f5ef80..09c619c78c1a1f271cae9da86ba19ccd4cb9746c 100644
--- a/src/sensor/sensor_gnss.cpp
+++ b/src/sensor/sensor_gnss.cpp
@@ -129,23 +129,23 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, con
     if (getProblem()->getDim() == 2)
     {
         // compute antena vector (from 1 to 2) in MAP
-        Eigen::Vector2s t_MAP_antenaENU = _pose_MAP_frameENU.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frameENU(2)) * getPPtr()->getState().head<2>();
-        Eigen::Vector2s t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frame2(2)) * getPPtr()->getState().head<2>();
+        Eigen::Vector2s t_MAP_antenaENU = _pose_MAP_frameENU.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frameENU(2)) * getP()->getState().head<2>();
+        Eigen::Vector2s t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frame2(2)) * getP()->getState().head<2>();
         Eigen::Vector2s v_MAP =  t_MAP_antena2 - t_MAP_antenaENU;
 
         // initialize yaw
         Scalar theta_ENU = atan2(v_ENU(1),v_ENU(0));
         Scalar theta_MAP = atan2(v_MAP(1),v_MAP(0));
         Scalar yaw = theta_ENU-theta_MAP;
-        this->getEnuMapYawStatePtr()->setState(Eigen::Vector1s(yaw));
+        this->getEnuMapYawState()->setState(Eigen::Vector1s(yaw));
 
         // initialize translation
         Eigen::Vector3s t_ENU_MAP(Eigen::Vector3s::Zero());
-        Eigen::Matrix2s R_ENU_MAP = computeREnuMap(getEnuMapRollStatePtr()->getState()(0),
-                                                   getEnuMapPitchStatePtr()->getState()(0),
-                                                   getEnuMapYawStatePtr()->getState()(0)).topLeftCorner<2,2>();
+        Eigen::Matrix2s R_ENU_MAP = computeREnuMap(getEnuMapRollState()->getState()(0),
+                                                   getEnuMapPitchState()->getState()(0),
+                                                   getEnuMapYawState()->getState()(0)).topLeftCorner<2,2>();
         t_ENU_MAP.head<2>() = -R_ENU_MAP * t_MAP_antenaENU;
-        this->getEnuMapTranslationStatePtr()->setState(t_ENU_MAP);
+        this->getEnuMapTranslationState()->setState(t_ENU_MAP);
 
         //std::cout << "-----------------------------------------\n";
         //std::cout << "t_ECEF_antenaENU: " << _t_ECEF_antenaENU.transpose() << std::endl;
diff --git a/src/track_matrix.cpp b/src/track_matrix.cpp
index 2bab9abb565eb64b009eddb8a147c757b52440bc..51b06c703e716e055f2fff03429da6873ad84b4f 100644
--- a/src/track_matrix.cpp
+++ b/src/track_matrix.cpp
@@ -225,7 +225,7 @@ Track TrackMatrix::trackAtKeyframes(size_t _track_id)
 
 bool TrackMatrix::markKeyframe(CaptureBasePtr _capture)
 {
-    if (_capture->getFramePtr() && _capture->getFramePtr()->isKey())
+    if (_capture->getFrame() && _capture->getFrame()->isKey())
     {
         auto snap = snapshot(_capture);
 
@@ -236,7 +236,7 @@ bool TrackMatrix::markKeyframe(CaptureBasePtr _capture)
         {
             auto track_id = pair_trkid_ftr.first;
             auto ftr = pair_trkid_ftr.second;
-            auto ts = _capture->getFramePtr()->getTimeStamp();
+            auto ts = _capture->getFrame()->getTimeStamp();
             tracks_kf_[track_id][ts] = ftr;
         }
         return true;
diff --git a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
index 46f01d4b96546507e8aeb4ff4e0d76da9240b68f..db78505d9a69330770ab7fb693e572f01fb96765 100644
--- a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
+++ b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
@@ -51,7 +51,6 @@ static ProcessorParamsBasePtr createProcessorParamsTrackerFeatureTrifocal(const
         params->n_cells_h                       = algorithm["grid horiz cells"]               .as<int>();
         params->n_cells_v                       = algorithm["grid vert cells"]                .as<int>();
         params->min_response_new_feature        = algorithm["min response new features"]      .as<int>();
-        params->max_euclidean_distance          = algorithm["max euclidean distance"]         .as<Scalar>();
         params->min_track_length_for_factor     = algorithm["min track length for factor"].as<int>();
 
         YAML::Node noise                      = config["noise"];
diff --git a/test/gtest_factor_gnss_fix_2D.cpp b/test/gtest_factor_gnss_fix_2D.cpp
index 3b60dba80e48528337b7197285527758850040cc..a01314ae7b3a7b554178816db74b9a1dcfb20abf 100644
--- a/test/gtest_factor_gnss_fix_2D.cpp
+++ b/test/gtest_factor_gnss_fix_2D.cpp
@@ -27,29 +27,29 @@ void resetStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr,
                  const Vector1s& o_map_base)
 {
     // ENU-MAP
-    gnss_sensor_ptr->getEnuMapRollStatePtr()->setState(Eigen::Vector1s::Zero());
-    gnss_sensor_ptr->getEnuMapPitchStatePtr()->setState(Eigen::Vector1s::Zero());
-    gnss_sensor_ptr->getEnuMapYawStatePtr()->setState(o_enu_map);
-    gnss_sensor_ptr->getEnuMapTranslationStatePtr()->setState(t_enu_map);
+    gnss_sensor_ptr->getEnuMapRollState()->setState(Eigen::Vector1s::Zero());
+    gnss_sensor_ptr->getEnuMapPitchState()->setState(Eigen::Vector1s::Zero());
+    gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map);
+    gnss_sensor_ptr->getEnuMapTranslationState()->setState(t_enu_map);
     // Antena
-    gnss_sensor_ptr->getPPtr()->setState(t_base_antena);
+    gnss_sensor_ptr->getP()->setState(t_base_antena);
     // Frame
-    frame_ptr->getPPtr()->setState(t_map_base.head(2));
-    frame_ptr->getOPtr()->setState(o_map_base);
+    frame_ptr->getP()->setState(t_map_base.head(2));
+    frame_ptr->getO()->setState(o_map_base);
 }
 
 void fixAllStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr)
 {
     // ENU-MAP
-    gnss_sensor_ptr->getEnuMapRollStatePtr()->fix();
-    gnss_sensor_ptr->getEnuMapPitchStatePtr()->fix();
-    gnss_sensor_ptr->getEnuMapYawStatePtr()->fix();
-    gnss_sensor_ptr->getEnuMapTranslationStatePtr()->fix();
+    gnss_sensor_ptr->getEnuMapRollState()->fix();
+    gnss_sensor_ptr->getEnuMapPitchState()->fix();
+    gnss_sensor_ptr->getEnuMapYawState()->fix();
+    gnss_sensor_ptr->getEnuMapTranslationState()->fix();
     // Antena
-    gnss_sensor_ptr->getPPtr()->fix();
+    gnss_sensor_ptr->getP()->fix();
     // Frame
-    frame_ptr->getPPtr()->fix();
-    frame_ptr->getOPtr()->fix();
+    frame_ptr->getP()->fix();
+    frame_ptr->getO()->fix();
  }
 
 void computeParamSizes(const CeresManagerPtr& ceres_mgr_ptr, int& num_params_reduced, int& num_param_blocks_reduced )
@@ -100,10 +100,10 @@ TEST(FactorGnssFix2DTest, configure_tree)
     ceres_mgr_ptr->getSolverOptions().max_num_iterations = 100;
 
     // Configure sensor and processor
-    gnss_sensor_ptr->getEnuMapTranslationStatePtr()->setState(t_enu_map);
-    gnss_sensor_ptr->getEnuMapYawStatePtr()->setState(o_enu_map);
-    gnss_sensor_ptr->getEnuMapRollStatePtr()->fix();
-    gnss_sensor_ptr->getEnuMapPitchStatePtr()->fix();
+    gnss_sensor_ptr->getEnuMapTranslationState()->setState(t_enu_map);
+    gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map);
+    gnss_sensor_ptr->getEnuMapRollState()->fix();
+    gnss_sensor_ptr->getEnuMapPitchState()->fix();
     gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
 
     ProcessorParamsGnssFixPtr gnss_params_ptr = std::make_shared<ProcessorParamsGnssFix>();
@@ -138,7 +138,7 @@ TEST(FactorGnssFix2DTest, gnss_1_map_base_position)
 
     // --------------------------- fixing/unfixing states
     fixAllStates(gnss_sensor_ptr, frame_ptr);
-    frame_ptr->getPPtr()->unfix();
+    frame_ptr->getP()->unfix();
 
     // --------------------------- distort: map base position
     Vector3s frm_dist = frame_ptr->getState();
@@ -184,12 +184,12 @@ TEST(FactorGnssFix2DTest, gnss_1_map_base_orientation)
 
     // --------------------------- fixing/unfixing states
     fixAllStates(gnss_sensor_ptr, frame_ptr);
-    frame_ptr->getOPtr()->unfix();
+    frame_ptr->getO()->unfix();
 
     // --------------------------- distort: map base orientation
-    Vector1s frm_dist = frame_ptr->getOPtr()->getState();
+    Vector1s frm_dist = frame_ptr->getO()->getState();
     frm_dist(0) += 0.18;
-    frame_ptr->getOPtr()->setState(frm_dist);
+    frame_ptr->getO()->setState(frm_dist);
 
     // --------------------------- update solver
     ceres_mgr_ptr->update();
@@ -211,7 +211,7 @@ TEST(FactorGnssFix2DTest, gnss_1_map_base_orientation)
     ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3);
 
     // --------------------------- check solver solution
-    ASSERT_MATRIX_APPROX(frame_ptr->getOPtr()->getState(), o_map_base, 1e-6);
+    ASSERT_MATRIX_APPROX(frame_ptr->getO()->getState(), o_map_base, 1e-6);
 }
 
 /*
@@ -227,12 +227,12 @@ TEST(FactorGnssFix2DTest, gnss_1_enu_map_yaw)
 
     // --------------------------- fixing/unfixing states
     fixAllStates(gnss_sensor_ptr, frame_ptr);
-    gnss_sensor_ptr->getEnuMapYawStatePtr()->unfix();
+    gnss_sensor_ptr->getEnuMapYawState()->unfix();
 
     // --------------------------- distort: yaw enu_map
-    Vector1s o_enu_map_dist = gnss_sensor_ptr->getEnuMapYawStatePtr()->getState();
+    Vector1s o_enu_map_dist = gnss_sensor_ptr->getEnuMapYawState()->getState();
     o_enu_map_dist(0) += 0.18;
-    gnss_sensor_ptr->getEnuMapYawStatePtr()->setState(o_enu_map_dist);
+    gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map_dist);
 
     // --------------------------- update solver
     ceres_mgr_ptr->update();
@@ -254,7 +254,7 @@ TEST(FactorGnssFix2DTest, gnss_1_enu_map_yaw)
     ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3);
 
     // --------------------------- check solver solution
-    ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYawStatePtr()->getState(), o_enu_map, 1e-6);
+    ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYawState()->getState(), o_enu_map, 1e-6);
 }
 
 /*
@@ -270,13 +270,13 @@ TEST(FactorGnssFix2DTest, gnss_1_enu_map_position)
 
     // --------------------------- fixing/unfixing states
     fixAllStates(gnss_sensor_ptr, frame_ptr);
-    gnss_sensor_ptr->getEnuMapTranslationStatePtr()->unfix();// enu-map yaw translation
+    gnss_sensor_ptr->getEnuMapTranslationState()->unfix();// enu-map yaw translation
 
     // --------------------------- distort: position enu_map
-    Vector3s t_enu_map_dist = gnss_sensor_ptr->getEnuMapTranslationStatePtr()->getState();
+    Vector3s t_enu_map_dist = gnss_sensor_ptr->getEnuMapTranslationState()->getState();
     t_enu_map_dist(0) += 0.86;
     t_enu_map_dist(1) += -0.34;
-    gnss_sensor_ptr->getEnuMapTranslationStatePtr()->setState(t_enu_map_dist);
+    gnss_sensor_ptr->getEnuMapTranslationState()->setState(t_enu_map_dist);
 
     // --------------------------- update solver
     ceres_mgr_ptr->update();
@@ -298,7 +298,7 @@ TEST(FactorGnssFix2DTest, gnss_1_enu_map_position)
     ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3);
 
     // --------------------------- check solver solution
-    ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapTranslationStatePtr()->getState().head(2), t_enu_map.head(2), 1e-6);
+    ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapTranslationState()->getState().head(2), t_enu_map.head(2), 1e-6);
 }
 
 /*
@@ -314,13 +314,13 @@ TEST(FactorGnssFix2DTest, gnss_1_base_antena)
 
     // --------------------------- fixing/unfixing states
     fixAllStates(gnss_sensor_ptr, frame_ptr);
-    gnss_sensor_ptr->getPPtr()->unfix();
+    gnss_sensor_ptr->getP()->unfix();
 
     // --------------------------- distort: base_antena
-    Vector3s base_antena_dist = gnss_sensor_ptr->getPPtr()->getState();
+    Vector3s base_antena_dist = gnss_sensor_ptr->getP()->getState();
     base_antena_dist(0) += 1.68;
     base_antena_dist(0) += -0.48;
-    gnss_sensor_ptr->getPPtr()->setState(base_antena_dist);
+    gnss_sensor_ptr->getP()->setState(base_antena_dist);
 
     // --------------------------- update solver
     ceres_mgr_ptr->update();
@@ -342,7 +342,7 @@ TEST(FactorGnssFix2DTest, gnss_1_base_antena)
     ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3);
 
     // --------------------------- check solver solution
-    ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getPPtr()->getState().head(2), t_base_antena.head(2), 1e-6);
+    ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getP()->getState().head(2), t_base_antena.head(2), 1e-6);
 }
 
 int main(int argc, char **argv)
diff --git a/test/gtest_factor_gnss_single_diff_2D.cpp b/test/gtest_factor_gnss_single_diff_2D.cpp
index 55443ed3404c2f020d760856e53fe199e3721662..c0f0021f60a48be7c3e20b3ccad5d021161fa855 100644
--- a/test/gtest_factor_gnss_single_diff_2D.cpp
+++ b/test/gtest_factor_gnss_single_diff_2D.cpp
@@ -65,8 +65,8 @@ class FactorGnssSingleDiff2DTest : public testing::Test
 
             // gnss sensor
             gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("GNSS", "gnss", t_base_antena, std::make_shared<IntrinsicsBase>()));
-            gnss_sensor_ptr->getEnuMapRollStatePtr()->fix();
-            gnss_sensor_ptr->getEnuMapPitchStatePtr()->fix();
+            gnss_sensor_ptr->getEnuMapRollState()->fix();
+            gnss_sensor_ptr->getEnuMapPitchState()->fix();
             gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
             std::cout << "gnss sensor installed" << std::endl;
 
@@ -109,7 +109,7 @@ class FactorGnssSingleDiff2DTest : public testing::Test
             t_ecef_antena2 = R_ecef_enu * R_enu_map * (R_map_base2 * t_base_antena + t_map_base2) + t_ecef_enu;
 
             // Reset antena extrinsics
-            gnss_sensor_ptr->getPPtr()->setState(t_base_antena);
+            gnss_sensor_ptr->getP()->setState(t_base_antena);
 
             // Reset ENU_ECEF
             gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
@@ -136,25 +136,25 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_position)
     gnss_sensor_ptr->process(cap_gnss_ptr);
 
     // fixing things
-    gnss_sensor_ptr->getEnuMapYawStatePtr()->setState(o_enu_map); // enu-map yaw orientation
-    gnss_sensor_ptr->getEnuMapYawStatePtr()->fix();
-    cap_gnss_ptr->getFramePtr()->getOPtr()->setState(o_map_base2); // frame orientation
-    cap_gnss_ptr->getFramePtr()->getOPtr()->fix();
+    gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map); // enu-map yaw orientation
+    gnss_sensor_ptr->getEnuMapYawState()->fix();
+    cap_gnss_ptr->getFrame()->getO()->setState(o_map_base2); // frame orientation
+    cap_gnss_ptr->getFrame()->getO()->fix();
 
     std::cout << "frame1: " << prior_frame_ptr->getState().transpose() << std::endl;
 
     // distort frm position
-    Vector3s frm_dist = cap_gnss_ptr->getFramePtr()->getState();
+    Vector3s frm_dist = cap_gnss_ptr->getFrame()->getState();
     frm_dist(0) += 18;
     frm_dist(1) += -58;
-    cap_gnss_ptr->getFramePtr()->setState(frm_dist);
+    cap_gnss_ptr->getFrame()->setState(frm_dist);
 
     // solve for frm
     std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
     std::cout << report << std::endl;
 
-    ASSERT_TRUE(cap_gnss_ptr->getFramePtr()->isKey());
-    ASSERT_MATRIX_APPROX(cap_gnss_ptr->getFramePtr()->getState().head(2), t_map_base2.head(2), 1e-6);
+    ASSERT_TRUE(cap_gnss_ptr->getFrame()->isKey());
+    ASSERT_MATRIX_APPROX(cap_gnss_ptr->getFrame()->getState().head(2), t_map_base2.head(2), 1e-6);
 }
 
 /*
@@ -170,21 +170,21 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_orientation)
     gnss_sensor_ptr->process(cap_gnss_ptr);
 
     // fixing things
-    gnss_sensor_ptr->getEnuMapYawStatePtr()->setState(o_enu_map); // enu-map yaw orientation
-    gnss_sensor_ptr->getEnuMapYawStatePtr()->fix();
-    cap_gnss_ptr->getFramePtr()->getPPtr()->setState(t_map_base2.head(2)); // frame position
-    cap_gnss_ptr->getFramePtr()->getPPtr()->fix();
+    gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map); // enu-map yaw orientation
+    gnss_sensor_ptr->getEnuMapYawState()->fix();
+    cap_gnss_ptr->getFrame()->getP()->setState(t_map_base2.head(2)); // frame position
+    cap_gnss_ptr->getFrame()->getP()->fix();
 
     // distort frm position
-    Vector1s frm_dist = cap_gnss_ptr->getFramePtr()->getOPtr()->getState();
+    Vector1s frm_dist = cap_gnss_ptr->getFrame()->getO()->getState();
     frm_dist(0) += 1.8;
-    cap_gnss_ptr->getFramePtr()->getOPtr()->setState(frm_dist);
+    cap_gnss_ptr->getFrame()->getO()->setState(frm_dist);
 
     // solve for frm
     std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
 
-    ASSERT_TRUE(cap_gnss_ptr->getFramePtr()->isKey());
-    ASSERT_MATRIX_APPROX(cap_gnss_ptr->getFramePtr()->getOPtr()->getState(), o_map_base2, 1e-6);
+    ASSERT_TRUE(cap_gnss_ptr->getFrame()->isKey());
+    ASSERT_MATRIX_APPROX(cap_gnss_ptr->getFrame()->getO()->getState(), o_map_base2, 1e-6);
 }
 
 /*
@@ -200,23 +200,23 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_enu_map_yaw)
     gnss_sensor_ptr->process(cap_gnss_ptr);
 
     // unfixing things
-    gnss_sensor_ptr->getEnuMapYawStatePtr()->unfix();
+    gnss_sensor_ptr->getEnuMapYawState()->unfix();
     // fixing things
-    cap_gnss_ptr->getFramePtr()->getPPtr()->setState(t_map_base2.head(2)); // frame position
-    cap_gnss_ptr->getFramePtr()->getPPtr()->fix();
-    cap_gnss_ptr->getFramePtr()->getOPtr()->setState(o_map_base2); // frame orientation
-    cap_gnss_ptr->getFramePtr()->getOPtr()->fix();
+    cap_gnss_ptr->getFrame()->getP()->setState(t_map_base2.head(2)); // frame position
+    cap_gnss_ptr->getFrame()->getP()->fix();
+    cap_gnss_ptr->getFrame()->getO()->setState(o_map_base2); // frame orientation
+    cap_gnss_ptr->getFrame()->getO()->fix();
 
     // distort yaw enu_map
-    Vector1s o_enu_map_dist = gnss_sensor_ptr->getEnuMapYawStatePtr()->getState();
+    Vector1s o_enu_map_dist = gnss_sensor_ptr->getEnuMapYawState()->getState();
     o_enu_map_dist(0) += 1.8;
-    gnss_sensor_ptr->getEnuMapYawStatePtr()->setState(o_enu_map_dist);
+    gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map_dist);
 
     // solve for frm
     std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
 
-    ASSERT_TRUE(cap_gnss_ptr->getFramePtr()->isKey());
-    ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYawStatePtr()->getState(), o_enu_map, 1e-6);
+    ASSERT_TRUE(cap_gnss_ptr->getFrame()->isKey());
+    ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYawState()->getState(), o_enu_map, 1e-6);
 }
 
 /*
@@ -232,26 +232,26 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_base_antena)
     gnss_sensor_ptr->process(cap_gnss_ptr);
 
     // unfixing things
-    gnss_sensor_ptr->getPPtr()->unfix();
+    gnss_sensor_ptr->getP()->unfix();
     // fixing things
-    gnss_sensor_ptr->getEnuMapYawStatePtr()->setState(o_enu_map); // enu-map yaw orientation
-    gnss_sensor_ptr->getEnuMapYawStatePtr()->fix();
-    cap_gnss_ptr->getFramePtr()->getPPtr()->setState(t_map_base2.head(2)); // frame position
-    cap_gnss_ptr->getFramePtr()->getPPtr()->fix();
-    cap_gnss_ptr->getFramePtr()->getOPtr()->setState(o_map_base2); // frame orientation
-    cap_gnss_ptr->getFramePtr()->getOPtr()->fix();
+    gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map); // enu-map yaw orientation
+    gnss_sensor_ptr->getEnuMapYawState()->fix();
+    cap_gnss_ptr->getFrame()->getP()->setState(t_map_base2.head(2)); // frame position
+    cap_gnss_ptr->getFrame()->getP()->fix();
+    cap_gnss_ptr->getFrame()->getO()->setState(o_map_base2); // frame orientation
+    cap_gnss_ptr->getFrame()->getO()->fix();
 
     // distort base_antena
-    Vector3s base_antena_dist = gnss_sensor_ptr->getPPtr()->getState();
+    Vector3s base_antena_dist = gnss_sensor_ptr->getP()->getState();
     base_antena_dist(0) += 16.8;
     base_antena_dist(0) += -40.8;
-    gnss_sensor_ptr->getPPtr()->setState(base_antena_dist);
+    gnss_sensor_ptr->getP()->setState(base_antena_dist);
 
     // solve for frm
     std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
 
-    ASSERT_TRUE(cap_gnss_ptr->getFramePtr()->isKey());
-    ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getPPtr()->getState().head(2), t_base_antena.head(2), 1e-6);
+    ASSERT_TRUE(cap_gnss_ptr->getFrame()->isKey());
+    ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getP()->getState().head(2), t_base_antena.head(2), 1e-6);
 }
 
 int main(int argc, char **argv)
diff --git a/test/gtest_landmark_polyline.cpp b/test/gtest_landmark_polyline.cpp
index 2f01d06b0d6b882418d57e9e5f99389cc98ccddb..1ead19daedc9b85bb4ef797309b24778ed4bdefe 100644
--- a/test/gtest_landmark_polyline.cpp
+++ b/test/gtest_landmark_polyline.cpp
@@ -120,14 +120,14 @@ TEST_F(LandmarkPolylineTest, Classify)
         ASSERT_TRUE(lmk_ptr->isClosed());
 
         // test points fixed and center unfixed
-        ASSERT_FALSE(lmk_ptr->getPPtr()->isFixed());
-        ASSERT_FALSE(lmk_ptr->getOPtr()->isFixed());
+        ASSERT_FALSE(lmk_ptr->getP()->isFixed());
+        ASSERT_FALSE(lmk_ptr->getO()->isFixed());
         for (auto st_pair : lmk_ptr->getPointStatePtrMap())
             ASSERT_TRUE(st_pair.second->isFixed());
 
         // test points are in the same place (compoping with object pose)
-        Matrix2s R = Rotation2D<Scalar>(lmk_ptr->getOPtr()->getState()(0)).matrix();
-        VectorXs t = lmk_ptr->getPPtr()->getState();
+        Matrix2s R = Rotation2D<Scalar>(lmk_ptr->getO()->getState()(0)).matrix();
+        VectorXs t = lmk_ptr->getP()->getState();
         for (unsigned int point_i = 0; point_i < 4; point_i++)
             ASSERT_MATRIX_APPROX(R*lmk_ptr->getPointVector(point_i)+t, points_groundtruth[i].col(point_i), 1e-9);
     }