diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0242a96b77867854aead97822c282920234509fd..9c71d5edb9d94e4b0fd4d475ad675605da295374 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -27,18 +27,14 @@ message(STATUS "Configured to compile in ${CMAKE_BUILD_TYPE} mode.")
 SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
 SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
 
-#Set compiler according C++11 support
+#Set compiler according C++14 support
 include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-if(COMPILER_SUPPORTS_CXX11)
-		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++11 support.")
-        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-elseif(COMPILER_SUPPORTS_CXX0X)
-		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++0x support.")
-        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
+if(COMPILER_SUPPORTS_CXX14)
+		message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++14 support.")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
 else()
-        message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+  message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
 endif()
 
 if(UNIX)
diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt
index 93d17fbe0befb99c22622c7f48f5fd67511467df..191653f92527669418d058a7c2a7f6fa399d0b73 100644
--- a/demos/CMakeLists.txt
+++ b/demos/CMakeLists.txt
@@ -1,26 +1,10 @@
 INCLUDE_DIRECTORIES(
     ${CMAKE_CURRENT_SOURCE_DIR}
-    ${catkin_INCLUDE_DIRS}
 )
 
-FIND_PACKAGE(catkin REQUIRED COMPONENTS
-  rosbag
-  rosconsole
-  roscpp
-  roslib
-  sensor_msgs
-  std_msgs
-  geometry_msgs
-  tf
-  wolf_ros_objectslam
-  wolf_ros_node
-)
 FIND_PACKAGE(wolfimu REQUIRED)
 
 INCLUDE_DIRECTORIES(
-    ${rosbag_INCLUDE_DIRS}
-    ${wolf_ros_objectslam_INCLUDE_DIRS}
-    ${std_msgs_INCLUDE_DIRS}
     ${wolfimu_INCLUDE_DIRS}
 )
 
@@ -29,22 +13,22 @@ ADD_EXECUTABLE(demo_toy_pbe demo_toy_pbe.cpp)
 TARGET_LINK_LIBRARIES(demo_toy_pbe 
     ${wolfcore_LIBRARIES}
     ${PLUGIN_NAME}
-    ${catkin_LIBRARIES}
-)
-
-ADD_EXECUTABLE(cosyslam cosyslam.cpp)
-TARGET_LINK_LIBRARIES(cosyslam 
-    ${wolfcore_LIBRARIES}
-    ${PLUGIN_NAME}
-    ${catkin_LIBRARIES}
+    # ${catkin_LIBRARIES}
 )
 
-ADD_EXECUTABLE(cosyslam_imu cosyslam_imu.cpp)
-TARGET_LINK_LIBRARIES(cosyslam_imu 
-    ${wolfcore_LIBRARIES}
-    ${PLUGIN_NAME}
-    ${catkin_LIBRARIES}
-    ${wolfimu_LIBRARIES}
-)
+# ADD_EXECUTABLE(cosyslam cosyslam.cpp)
+# TARGET_LINK_LIBRARIES(cosyslam 
+#     ${wolfcore_LIBRARIES}
+#     ${PLUGIN_NAME}
+#     ${catkin_LIBRARIES}
+# )
+
+# ADD_EXECUTABLE(cosyslam_imu cosyslam_imu.cpp)
+# TARGET_LINK_LIBRARIES(cosyslam_imu 
+#     ${wolfcore_LIBRARIES}
+#     ${PLUGIN_NAME}
+#     ${catkin_LIBRARIES}
+#     ${wolfimu_LIBRARIES}
+# )
 
 
diff --git a/demos/cosyslam.cpp b/demos/cosyslam.cpp
index fcbd5093572b46bf9d9b6751e5b3e6136f3a848c..2a4de756d05d8672edeb08ba534a7fdece72b00c 100644
--- a/demos/cosyslam.cpp
+++ b/demos/cosyslam.cpp
@@ -84,7 +84,7 @@ int main()
     ObjectDetections dets;
     VectorComposite x0("PO", {Vector3d::Zero(),  Quaterniond::Identity().coeffs()});
     VectorComposite sig0("PO", {Vector3d::Ones()*0.001, Vector3d::Ones()*0.1});
-    FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0, 0.1);
+    FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0);
 
     // Variables init
     Matrix6d cov;
diff --git a/demos/cosyslam_imu.cpp b/demos/cosyslam_imu.cpp
index 3bacab812af0eda3aa268a67c9e00ff2dbb7fc66..d783e93fe6c5be14199800e865f87706b848111d 100644
--- a/demos/cosyslam_imu.cpp
+++ b/demos/cosyslam_imu.cpp
@@ -157,7 +157,7 @@ int main()
 
     VectorComposite x0("POV", { Vector3d::Zero(), q_init, Vector3d::Zero()});
     VectorComposite sig0("POV", {Vector3d::Ones()*0.001, Vector3d::Ones()*0.1, Vector3d::Ones()*0.01});
-    FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0, 0.1);
+    FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0);
 
     foreach(rosbag::MessageInstance const m, view)
     {
diff --git a/demos/demo_toy_pbe.cpp b/demos/demo_toy_pbe.cpp
index b8941930d607e418f9827416b471016a026c6c1f..57b9b7bc7b8bca93282251e26c8163ea3b24d0e7 100644
--- a/demos/demo_toy_pbe.cpp
+++ b/demos/demo_toy_pbe.cpp
@@ -85,7 +85,7 @@ int main() {
 
     VectorComposite x0("PO", {Vector3d(0,0,0), Quaterniond::Identity().coeffs()});
     VectorComposite sig0("PO", {Vector3d(1,1,1), Vector3d(1,1,1)});
-    FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0, 0.1);
+    FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0);
 
     // define measurements according to "detections" represented in the graph above
     Matrix6d cov = Matrix6d::Identity();
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index c3d6dae0f40144640a339c044dd79f63005d9cfa..22e324bd6ced20ba2ebcebaf68792656ed8ce0f3 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -1,6 +1,6 @@
 #include "core/math/rotations.h"
 #include <core/factor/factor_distance_3d.h>
-#include "core/factor/factor_kf_lmk_pose_3d_with_extrinsics.h"
+#include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
 
 // Wolf object plugin
 #include "objectslam/processor/processor_tracker_landmark_object.h"
@@ -85,14 +85,11 @@ FactorBasePtr ProcessorTrackerLandmarkObject::emplaceFactor(FeatureBasePtr _feat
     // A keyframe is voted 
     keyframe_voted_ = true;
 
-    return FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(_feature_ptr,
-                                                                getSensor(),
-                                                                getLast()->getFrame(),
-                                                                std::static_pointer_cast<LandmarkObject>(_landmark_ptr),
-                                                                std::static_pointer_cast<FeatureObject> (_feature_ptr ),
-                                                                shared_from_this(),
-                                                                params_->apply_loss_function,
-                                                                FAC_ACTIVE);
+    return FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(_feature_ptr,
+                                                                   _feature_ptr,
+                                                                   _landmark_ptr,
+                                                                   shared_from_this(),
+                                                                   params_->apply_loss_function);
 }
 
 LandmarkBasePtr ProcessorTrackerLandmarkObject::emplaceLandmark(FeatureBasePtr _feature_ptr)
@@ -264,9 +261,6 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
 }
 
 
-
-
-
 FeatureBasePtrList ProcessorTrackerLandmarkObject::getIncomingDetections() const
 {
     return detections_incoming_;
@@ -278,7 +272,6 @@ FeatureBasePtrList ProcessorTrackerLandmarkObject::getLastDetections() const
 }
 
 
-
 void ProcessorTrackerLandmarkObject::advanceDerived()
 {
     ProcessorTrackerLandmark::advanceDerived();
@@ -287,153 +280,10 @@ void ProcessorTrackerLandmarkObject::advanceDerived()
 
 void ProcessorTrackerLandmarkObject::resetDerived()
 {   
-    // Add 3d distance constraint between 2 frames
-    if (getProblem()->getProcessorIsMotionMap().empty() && add_3d_cstr_){
-        if ((getOrigin() != nullptr) && 
-            (getOrigin()->getFrame() != nullptr) && 
-            (getOrigin() != getLast()) &&
-            (getOrigin()->getFrame() != getLast()->getFrame()) 
-            )
-        {
-            FrameBasePtr ori_frame = getOrigin()->getFrame();
-            Eigen::Vector1d dist_meas; dist_meas << 0.0;
-            double dist_std = 0.5;
-            Eigen::Matrix1d cov0(dist_std*dist_std);
- 
-            auto capt3d = CaptureBase::emplace<CaptureBase>(getLast()->getFrame(),"Dist",getLast()->getTimeStamp());
-            auto feat_dist = FeatureBase::emplace<FeatureBase>(capt3d, "Dist", dist_meas, cov0);
-            auto cstr = FactorBase::emplace<FactorDistance3d>(feat_dist, feat_dist, ori_frame, shared_from_this(), params_->apply_loss_function, FAC_ACTIVE);
-        }
-    }
-
-    if (getProblem()->getProcessorIsMotionMap().empty() && reestimate_last_frame_){
-        reestimateLastFrame();
-    }
-
     ProcessorTrackerLandmark::resetDerived();
     detections_last_ = std::move(detections_incoming_);
 }
 
-void ProcessorTrackerLandmarkObject::reestimateLastFrame(){
-    // !!!!!!!!!!!!!!!!!!!!!!!!!!!
-    // !! TOO COMPLEX: to rewrite
-    // !!!!!!!!!!!!!!!!!!!!!!!!!!!
-
-
-    // // Rewrite the last frame state and landmark state initialised during last frame creation to account
-    // // for a better estimate of the current state using the last landmark detection.
-    // // Otherwise, default behaviour is to take the last KF as an initialization of the new KF which can lead
-    // // to the solver finding local minima
-
-    // When called for the first time, no feature list initialized in ori/last(?)
-    if (n_reset_ < 1){
-        n_reset_ += 1;
-        return;
-    }
-
-    // // A TESTER
-    // // (getOrigin() != nullptr)
-
-    // // Retrieve camera extrinsics
-    // Eigen::Quaterniond last_q_cam(getSensor()->getO()->getState().data());
-    // Eigen::Isometry3d last_M_cam = Eigen::Translation3d(getSensor()->getP()->getState()) * last_q_cam;
-
-    // // get features list of KF linked to origin capture and last capture
-    // FeatureBasePtrList ori_feature_list = getOrigin()->getFeatureList();
-    // FeatureBasePtrList last_feature_list = getLast()->getFeatureList();
-
-    // if (last_feature_list.size() == 0 || ori_feature_list.size() == 0){
-    //     return;
-    // }
-    
-    // // Among landmarks detected in origin and last, find the one that has the smallest error ratio (best confidence)
-    // double lowest_ration = 1;  // rep_error1/rep_error2 cannot be higher than 1
-    // FeatureObjectPtr best_feature;
-    // bool useable_feature = false;
-    // for (auto it_last = last_feature_list.begin(); it_last != last_feature_list.end(); it_last++){
-    //     FeatureObjectPtr last_feat_ptr = std::static_pointer_cast<FeatureObject>(*it_last);
-    //     for (auto it_ori = ori_feature_list.begin(); it_ori != ori_feature_list.end(); it_ori++){
-    //         FeatureObjectPtr ori_feat_ptr =  std::static_pointer_cast<FeatureObject>(*it_ori);
-    //         if (ori_feat_ptr->getTagId() == last_feat_ptr->getTagId()){
-    //             double ratio = ori_feat_ptr->getRepError1() / ori_feat_ptr->getRepError2();
-    //             //if (ratio < lowest_ration){
-    //             if (last_feat_ptr->getUserotation() && (ratio < lowest_ration)){
-    //                 useable_feature = true;
-    //                 lowest_ration = ratio;
-    //                 best_feature = last_feat_ptr;
-    //             }
-    //         }
-    //     }
-    // }
-    // // If there is no common feature between the two images, the continuity is broken and 
-    // // nothing can be estimated. In the case of objslam alone, this result in a broken factor map
-    // if (!useable_feature){
-    //     return;
-    // }
-    
-    // // std::cout << "Best feature id after: " << best_feature->getTagId() << std::endl;
-    // // Retrieve cam to landmark transform
-    // Eigen::Vector7d cam_pose_lmk = best_feature->getMeasurement();
-    // Eigen::Quaterniond cam_q_lmk(cam_pose_lmk.segment<4>(3).data());
-    // Eigen::Isometry3d cam_M_lmk = Eigen::Translation3d(cam_pose_lmk.head(3)) * cam_q_lmk;
-    
-    // // Get corresponding landmarks in origin/last landmark list
-    // Eigen::Isometry3d w_M_lmk;
-    // LandmarkBasePtrList lmk_list = getProblem()->getMap()->getLandmarkList();
-    // // Iterate in reverse order because landmark detected in last are at the end of the list (while still landmarks to discovers)
-    // for (std::list<LandmarkBasePtr>::reverse_iterator it_lmk = lmk_list.rbegin(); it_lmk != lmk_list.rend(); ++it_lmk){
-    //     LandmarkObjectPtr lmk_ptr = std::dynamic_pointer_cast<LandmarkObject>(*it_lmk);
-    //     // the map might contain other types of landmarks so check if the cast is valid
-    //     if (lmk_ptr == nullptr){
-    //         continue;
-    //     }
-    
-    //     if (lmk_ptr->getTagId() == best_feature->getTagId()){
-    //         Eigen::Vector3d w_t_lmk = lmk_ptr->getP()->getState();
-    //         Eigen::Quaterniond w_q_lmk(lmk_ptr->getO()->getState().data());
-    //         w_M_lmk = Eigen::Translation<double,3>(w_t_lmk) * w_q_lmk;
-    //     }
-    // }
-
-    // // Compute last frame estimate
-    // Eigen::Isometry3d w_M_last = w_M_lmk * (last_M_cam * cam_M_lmk).inverse();
-
-    // // Use the w_M_last to overide last key frame state estimation and keyframe estimation
-    // Eigen::Vector3d pos_last  = w_M_last.translation();
-    // Eigen::Quaterniond quat_last(w_M_last.linear());
-    // getLast()->getFrame()->getP()->setState(pos_last);
-    // getLast()->getFrame()->getO()->setState(quat_last.coeffs());
-
-    // // if (!best_feature->getUserotation()){
-    // //     return;
-    // // }
-    // ///////////////////
-    // // Reestimate position of landmark new in Last
-    // ///////////////////
-    // for (auto it_feat = new_features_last_.begin(); it_feat != new_features_last_.end(); it_feat++){
-    //     FeatureObjectPtr new_feature_last = std::static_pointer_cast<FeatureObject>(*it_feat);
-       
-    //     Eigen::Vector7d cam_pose_lmk = new_feature_last->getMeasurement();
-    //     Eigen::Quaterniond cam_q_lmk(cam_pose_lmk.segment<4>(3).data());
-    //     Eigen::Isometry3d cam_M_lmk_new = Eigen::Translation3d(cam_pose_lmk.head(3)) * cam_q_lmk;
-    //     Eigen::Isometry3d w_M_lmk =  w_M_last * last_M_cam * cam_M_lmk_new;
-
-    //     for (auto it_lmk = new_landmarks_.begin(); it_lmk != new_landmarks_.end(); ++it_lmk){
-    //         LandmarkObjectPtr lmk_ptr = std::dynamic_pointer_cast<LandmarkObject>(*it_lmk);
-    //         if (lmk_ptr == nullptr){
-    //             continue;
-    //         }
-    //         if (lmk_ptr->getTagId() == new_feature_last->getTagId()){
-    //             Eigen::Vector3d pos_lmk_last  = w_M_lmk.translation();
-    //             Eigen::Quaterniond quat_lmk_last(w_M_lmk.linear());
-    //             lmk_ptr->getP()->setState(pos_lmk_last);
-    //             lmk_ptr->getO()->setState(quat_lmk_last.coeffs());
-    //             break;
-    //         }
-    //     }
-    // }
-}
-
 
 } // namespace wolf
 
diff --git a/test/gtest_processor_tracker_landmark_object.cpp b/test/gtest_processor_tracker_landmark_object.cpp
index 6ee563efecfb1b0ffd520eece02b483f589cd2d8..d8dd187fe3f86212dbc90d0218f5b6819d336a84 100644
--- a/test/gtest_processor_tracker_landmark_object.cpp
+++ b/test/gtest_processor_tracker_landmark_object.cpp
@@ -82,7 +82,7 @@ class ProcessorTrackerLandmarkObject_class : public testing::Test{
             // set prior
             VectorComposite x0("PO", {Vector3d(0,0,0), Quaterniond::Identity().coeffs()});
             VectorComposite s0("PO", {Vector3d(1,1,1), Vector3d(1,1,1)});
-            F1 = problem->setPriorFactor(x0, s0, 0.0, 0.1);
+            F1 = problem->setPriorFactor(x0, s0, 0.0);
 
             // minimal config for the processor to be operative
             C1 = CaptureBase::emplace<CapturePose>(F1, 1.0, sen, Vector7d(), Matrix6d());