diff --git a/CMakeLists.txt b/CMakeLists.txt
index df6f82b72c8463f7e0b6919114e36bf6969e3b19..e9e3f77c36652f9c83b95a31e9f138d27b1a13a0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -506,6 +506,7 @@ TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${YAMLCPP_LIBRARY})
 
 #Link the created libraries
 #=============================================================
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CMAKE_THREAD_LIBS_INIT} dl)
 IF (Ceres_FOUND)
     TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CERES_LIBRARIES})
 ENDIF(Ceres_FOUND)
diff --git a/cmake_modules/wolfConfig.cmake b/cmake_modules/wolfConfig.cmake
index 54a4b4edf5f01dd03e237eeb97d30178ccb14645..a72b121bde57c392cf9f13fe4a5ed73e309ddf98 100644
--- a/cmake_modules/wolfConfig.cmake
+++ b/cmake_modules/wolfConfig.cmake
@@ -11,7 +11,7 @@ ENDIF(wolf_INCLUDE_DIR)
 
 FIND_LIBRARY(
     wolf_LIBRARY
-    NAMES libwolf.so
+    NAMES libwolf.so libwolf.dylib
     PATHS /usr/local/lib/iri-algorithms)
 IF(wolf_LIBRARY)
   MESSAGE("Found wolf lib: ${wolf_LIBRARY}")
diff --git a/hello_wolf/CMakeLists.txt b/hello_wolf/CMakeLists.txt
index f8f41bd407e9e4d9fc9eaf71f92b7111b0772feb..b998b6f1c15ccb59d84e0a70ebbb6051a7301220 100644
--- a/hello_wolf/CMakeLists.txt
+++ b/hello_wolf/CMakeLists.txt
@@ -20,7 +20,10 @@ SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
     ${CMAKE_CURRENT_SOURCE_DIR}/processor_range_bearing.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/sensor_range_bearing.cpp
     )
+    
+
 add_library(hellowolf SHARED ${SRCS_HELLOWOLF})
+TARGET_LINK_LIBRARIES(hellowolf ${PROJECT_NAME})
 ADD_EXECUTABLE(hello_wolf hello_wolf.cpp)
 TARGET_LINK_LIBRARIES(hello_wolf ${PROJECT_NAME} hellowolf)
 add_library(sensor_odom SHARED ../src/sensor/sensor_odom_2D.cpp ../src/processor/processor_odom_2D.cpp)
diff --git a/include/core/ceres_wrapper/ceres_manager.h b/include/core/ceres_wrapper/ceres_manager.h
index c65b27415643c2c54b4c5a5144465b4a0767884b..413ecce13ebd2bb179134e255fe1d1af228efe13 100644
--- a/include/core/ceres_wrapper/ceres_manager.h
+++ b/include/core/ceres_wrapper/ceres_manager.h
@@ -90,9 +90,9 @@ class CeresManager : public SolverManager
 
         ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _fac_ptr);
 
-        virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr);
+        virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override;
 
-        virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr);
+        virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) override;
 };
 
 inline ceres::Solver::Summary CeresManager::getSummary()
@@ -105,7 +105,7 @@ inline ceres::Solver::Options& CeresManager::getSolverOptions()
     return ceres_options_;
 }
 
-inline bool CeresManager::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr)
+inline bool CeresManager::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const
 {
     return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end()
             && fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end();
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index 049cb951d07c5e1c0b70cdf9be9a1d9dea992261..f5ed62680e815ec182a9a0db28efe1c3894c6090 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -69,7 +69,7 @@ public:
 
   virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr);
 
-  virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr);
+  virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const;
 
 protected:
 
@@ -94,7 +94,7 @@ protected:
 
   virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) = 0;
 
-  virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) = 0;
+  virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0;
 };
 
 } // namespace wolf
diff --git a/include/core/utils/converter.h b/include/core/utils/converter.h
index 1824bbf870095023d17789374d4e244bf9e2434a..96e64575a653cef6f9f01d0c0d9b372e0540a170 100644
--- a/include/core/utils/converter.h
+++ b/include/core/utils/converter.h
@@ -6,6 +6,7 @@
 #include <eigen3/Eigen/Geometry>
 #include <regex>
 #include <iostream>
+#include <array>
 
 namespace utils{
     template <typename A>
@@ -249,4 +250,4 @@ struct converter<std::map<std::string,A>>{
     }
 };
 }
-#endif
\ No newline at end of file
+#endif
diff --git a/include/core/yaml/parser_yaml.hpp b/include/core/yaml/parser_yaml.hpp
index 9b678e1b5fe2823877695c02dedaaa3784f67e98..78ff43caa595ff4f8db37b0b860b3aef4ebf66fd 100644
--- a/include/core/yaml/parser_yaml.hpp
+++ b/include/core/yaml/parser_yaml.hpp
@@ -6,6 +6,7 @@
 #include <map>
 #include <iostream>
 #include <algorithm>
+#include <numeric>
 
 using namespace std;
 namespace {
@@ -321,4 +322,4 @@ map<string, string> parserYAML::fetchAsMap(YAML::Node n){
     }
     return m;
 }
-#endif
\ No newline at end of file
+#endif
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index f6d2914da4ff77e594ee0b2722627897f6fb23e5..e03be2d6ce6c5a7dac729efeef040a4d312ae11e 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -165,7 +165,7 @@ bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr)
     return state_blocks_.find(state_ptr) != state_blocks_.end() && isStateBlockRegisteredDerived(state_ptr);
 }
 
-bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr)
+bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const
 {
     return isFactorRegisteredDerived(fac_ptr);
 }
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index eb4b405f46a0cf0b429171f1c8c51b050321fe4f..92e42e4405b0cdd427af740a239849cdb8a53079 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -83,10 +83,8 @@ TEST(Emplace, Feature)
     ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
     ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem());
     ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame());
-    auto cov = Eigen::MatrixXs(2,2);
-    cov(0,0) = 1;
-    cov(1,1) = 1;
-    FeatureBase::emplace<FeatureBase>(cpt, "Dummy", Eigen::VectorXs(5), cov);
+    auto cov = Eigen::MatrixXs::Identity(2,2);
+    FeatureBase::emplace<FeatureBase>(cpt, "Dummy", Eigen::VectorXs(2), cov);
     ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
     ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
     ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture());
@@ -103,10 +101,8 @@ TEST(Emplace, Factor)
     ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
     ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem());
     ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame());
-    auto cov = Eigen::MatrixXs(2,2);
-    cov(0,0) = 1;
-    cov(1,1) = 1;
-    auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXs(5), cov);
+    auto cov = Eigen::MatrixXs::Identity(2,2);
+    auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXs(2), cov);
     ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
     ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
     ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture());
@@ -121,17 +117,15 @@ TEST(Emplace, EmplaceDerived)
     auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),  KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
     // LandmarkBase::emplace<LandmarkBase>(MapBaseWPtr(P->getMap()),"Dummy", nullptr, nullptr);
     auto sen = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Eigen::VectorXs(3), IntrinsicsOdom2D());
-    auto cov = Eigen::MatrixXs(2,2);
-    cov(0,0) = 1;
-    cov(1,1) = 1;
-    auto cpt = CaptureBase::emplace<CaptureOdom2D>(frm, TimeStamp(0), sen, Eigen::Vector6s(), cov, frm);
+    auto cov = Eigen::MatrixXs::Identity(2,2);
+    auto cpt = CaptureBase::emplace<CaptureOdom2D>(frm, TimeStamp(0), sen, Eigen::VectorXs(2), cov, frm);
     auto cpt2 = std::static_pointer_cast<CaptureOdom2D>(cpt);
     auto m = Eigen::Matrix<Scalar,9,6>();
     for(int i = 0; i < 9; i++)
         for(int j = 0; j < 6; j++)
             m(i,j) = 1;
 
-    auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXs(5), cov);
+    auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXs(2), cov);
     ASSERT_EQ(sen, P->getHardware()->getSensorList().front());
     ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
 }
@@ -143,4 +137,4 @@ int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);
   return RUN_ALL_TESTS();
-}
\ No newline at end of file
+}
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 942e49086acba86cbd74101bd21ec1ec14195e66..e25ba4baef1ff16bb61fcb399c9c7c6d35744700 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -46,9 +46,9 @@ public:
   virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr){};
   virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr){};
   virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr){return true;};
-  virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr){return true;};
+  virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const {return true;};
   virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
-  virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr){return true;};
+  virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;};
 };
 
 TEST(Problem, create)
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 496dab16c21dc0de603c0b2169d47a89a85514ed..92fc07b84ba8228a4c1b3ef90790a35dd28405b1 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -45,7 +45,7 @@ class SolverManagerWrapper : public SolverManager
             return state_block_fixed_.at(st);
         };
 
-        bool isFactorRegistered(const FactorBasePtr& fac_ptr)
+        bool isFactorRegistered(const FactorBasePtr& fac_ptr) const
         {
             return std::find(factors_.begin(), factors_.end(), fac_ptr) != factors_.end();
         };
@@ -63,7 +63,7 @@ class SolverManagerWrapper : public SolverManager
         virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
         virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
         virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
-        virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr){return true;};
+        virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;};
 
         // The following are dummy implementations
         bool    hasConverged()  { return true;      }
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index 50a78bc0f7b4705a9e17ddf1429ad6ee81aafe48..f71bc579b49ced7d6e389d917c690eed06e00566 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -38,9 +38,9 @@ struct DummySolverManager : public SolverManager
   virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr){};
   virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr){};
   virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr){return true;};
-  virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr){return true;};
+  virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const {return true;};
   virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr){return true;};
-  virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr){return true;};
+  virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const {return true;};
 };
 
 /// Set to true if you want debug info