diff --git a/CMakeLists.txt b/CMakeLists.txt
index fdc81d7a6ed05ce6f166e14f198009c63ad64dad..f5137909142765fc7d0b817976ce2f70b60857bf 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -376,7 +376,7 @@ SET(SRCS_YAML
 #optional HDRS and SRCS
 IF (Ceres_FOUND)
     SET(HDRS_WRAPPER
-      #ceres_wrapper/qr_manager.h
+      include/core/ceres_wrapper/qr_manager.h
       include/core/ceres_wrapper/ceres_manager.h
       include/core/ceres_wrapper/cost_function_wrapper.h
       include/core/ceres_wrapper/create_numeric_diff_cost_function.h
@@ -385,7 +385,7 @@ IF (Ceres_FOUND)
       include/core/solver_suitesparse/sparse_utils.h
       )
     SET(SRCS_WRAPPER
-      #ceres_wrapper/qr_manager.cpp
+      src/ceres_wrapper/qr_manager.cpp
       src/ceres_wrapper/ceres_manager.cpp
       src/ceres_wrapper/local_parametrization_wrapper.cpp
       src/solver/solver_manager.cpp
diff --git a/include/core/ceres_wrapper/qr_manager.h b/include/core/ceres_wrapper/qr_manager.h
index 691c44166da222b893c1d8acd428c7e63da71539..855b5dab5691c7a951de72a169e44ec279a5f348 100644
--- a/include/core/ceres_wrapper/qr_manager.h
+++ b/include/core/ceres_wrapper/qr_manager.h
@@ -9,7 +9,7 @@
 #define SRC_CERES_WRAPPER_QR_MANAGER_H_
 
 #include "core/solver/solver_manager.h"
-#include "core/solver_suitesparse/sparse_utils.h"
+#include "core/ceres_wrapper/sparse_utils.h"
 
 namespace wolf
 {
@@ -36,7 +36,7 @@ class QRManager : public SolverManager
 
         virtual std::string solve(const unsigned int& _report_level);
 
-        virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS);
+        virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS);
 
         virtual void computeCovariances(const std::vector<StateBlockPtr>& _sb_list);
 
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index b68b4ad7dd482f071faf04339656879635232dd2..1e94d116b3829224e3e7f647888b82e8ed8d3500 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -74,6 +74,10 @@ public:
          **/
         Eigen::VectorXd getState() const;
 
+        /** \brief Returns the state vector data pointer
+         **/
+        double* getStateData();
+
         /** \brief Sets the state vector
          **/
         void setState(const Eigen::VectorXd& _state, const bool _notify = true);
@@ -198,6 +202,12 @@ inline Eigen::VectorXd StateBlock::getState() const
     return state_;
 }
 
+inline double* StateBlock::getStateData()
+{
+    std::lock_guard<std::mutex> lock(mut_state_);
+    return state_.data();
+}
+
 inline SizeEigen StateBlock::getSize() const
 {
     return state_size_.load();
diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp
index 0d7746f929f427d3e2bc9f6bcce53e4147b7924d..2a7c21aa363f364a294528dd7a1a1abed7c71177 100644
--- a/src/ceres_wrapper/qr_manager.cpp
+++ b/src/ceres_wrapper/qr_manager.cpp
@@ -5,7 +5,7 @@
  *      Author: jvallve
  */
 
-#include "qr_manager.h"
+#include "core/ceres_wrapper/qr_manager.h"
 
 namespace wolf {
 
@@ -57,7 +57,7 @@ void QRManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
     // TODO
 }
 
-void QRManager::computeCovariances(const StateBlockPtrList& _sb_list)
+void QRManager::computeCovariances(const std::vector<StateBlockPtr>& _sb_list)
 {
     //std::cout << "computing covariances.." << std::endl;
     update();
@@ -206,8 +206,10 @@ void QRManager::relinearizeFactor(FactorBasePtr _fac_ptr)
 {
     // evaluate factor
     std::vector<const double*> fac_states_ptr;
-    for (auto sb : _fac_ptr->getStateBlockPtrVector())
-        fac_states_ptr.push_back(sb->get());
+    for (auto sb : _fac_ptr->getStateBlockPtrVector()){
+        fac_states_ptr.push_back(sb->getStateData());
+    }
+
     Eigen::VectorXd residual(_fac_ptr->getSize());
     std::vector<Eigen::MatrixXd> jacobians;
     _fac_ptr->evaluate(fac_states_ptr,residual,jacobians);
@@ -215,13 +217,16 @@ void QRManager::relinearizeFactor(FactorBasePtr _fac_ptr)
     // Fill jacobians
     Eigen::SparseMatrixd A_block_row(_fac_ptr->getSize(), A_.cols());
     for (auto i = 0; i < jacobians.size(); i++)
-        if (!_fac_ptr->getStateBlockPtrVector()[i]->isFixed())
+    {
+    if (!_fac_ptr->getStateBlockPtrVector()[i]->isFixed())
         {
             assert(sb_2_col_.find(_fac_ptr->getStateBlockPtrVector()[i]) != sb_2_col_.end() && "factor involving a state block not added");
             assert(A_.cols() >= sb_2_col_[_fac_ptr->getStateBlockPtrVector()[i]] + jacobians[i].cols() - 1 && "bad A number of cols");
             // insert since A_block_row has just been created so it's empty for sure
             insertSparseBlock(jacobians[i], A_block_row, 0, sb_2_col_[_fac_ptr->getStateBlockPtrVector()[i]]);
         }
+    }
+    
     assignBlockRow(A_, A_block_row, fac_2_row_[_fac_ptr]);
 
     // Fill residual