diff --git a/include/laser/processor/processor_closeloop_icp.h b/include/laser/processor/processor_closeloop_icp.h
index edc50d4bcd5ebcdaa14e283ec39354775c01e3ec..481ab6a42ab0552176291fc8859b18b26623d37c 100644
--- a/include/laser/processor/processor_closeloop_icp.h
+++ b/include/laser/processor/processor_closeloop_icp.h
@@ -189,20 +189,20 @@ class ProcessorCloseLoopICP : public ProcessorBase
     protected:
         virtual void processCapture(CaptureBasePtr) override;
 
-        virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) override;
+        virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override;
 
         virtual bool triggerInCapture(CaptureBasePtr) const override;
 
-        virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) const override;
+        virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override;
 
         virtual bool storeKeyFrame(FrameBasePtr) override;
 
         virtual bool storeCapture(CaptureBasePtr) override;
 
         virtual bool voteForKeyFrame() const override;
-        FrameBasePtrList selectCandidates(FrameBasePtr _keyframe_ptr, const Scalar &_time_tolerance);
-        std::map<Scalar, CapturesAligned> evaluateCandidates(FrameBasePtr _keyframe_own, FrameBasePtrList& _keyframe_candidates);
-        CapturesAligned bestCandidate(std::map<Scalar, CapturesAligned>& _capture_candidates);
+        FrameBasePtrList selectCandidates(FrameBasePtr _keyframe_ptr, const double &_time_tolerance);
+        std::map<double, CapturesAligned> evaluateCandidates(FrameBasePtr _keyframe_own, FrameBasePtrList& _keyframe_candidates);
+        CapturesAligned bestCandidate(std::map<double, CapturesAligned>& _capture_candidates);
         FactorBasePtr emplaceFeatureAndFactor(CapturesAligned& _captures_aligned);
 
 };
diff --git a/include/laser/state_block/local_parametrization_polyline_extreme.h b/include/laser/state_block/local_parametrization_polyline_extreme.h
index f57dd3a4a50d70843cf9e2307fd34c656e0ee8a1..8abe58ce3419dd0aa4c053d3ee302ae3efeb3a90 100644
--- a/include/laser/state_block/local_parametrization_polyline_extreme.h
+++ b/include/laser/state_block/local_parametrization_polyline_extreme.h
@@ -27,7 +27,7 @@ class LocalParametrizationPolylineExtreme : public LocalParametrizationBase
         virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _point,
                           Eigen::Map<const Eigen::VectorXd>& _delta_theta,
                           Eigen::Map<Eigen::VectorXd>& _point_plus_delta_theta) const;
-        virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _point, Eigen::Map<Eigen::MatrixXd>& _jacobian) const;
+        virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _point, Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const;
         virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _point1,
                            Eigen::Map<const Eigen::VectorXd>& _point2,
                            Eigen::Map<Eigen::VectorXd>& _delta_theta);
diff --git a/src/processor/processor_closeloop_icp.cpp b/src/processor/processor_closeloop_icp.cpp
index 8e5417f4cf92817f25d9acd8671e0a835db070da..fe491488daed46ed31b51febb65e0420bfcbd9e6 100644
--- a/src/processor/processor_closeloop_icp.cpp
+++ b/src/processor/processor_closeloop_icp.cpp
@@ -84,7 +84,7 @@ void ProcessorCloseLoopICP::processCapture(CaptureBasePtr _capture_ptr)
     // _origin_cpt = cpt_ptr;
 }
 
-void ProcessorCloseLoopICP::processKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance)
+void ProcessorCloseLoopICP::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
 {
     if(key_frames_skipped_ > key_frames_to_wait_)
         {
@@ -115,7 +115,7 @@ bool ProcessorCloseLoopICP::triggerInCapture(CaptureBasePtr) const
     return true;
 }
 
-bool ProcessorCloseLoopICP::triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) const
+bool ProcessorCloseLoopICP::triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const
 {
     return true;
 }
@@ -135,7 +135,7 @@ bool ProcessorCloseLoopICP::voteForKeyFrame() const
     return false;
 }
 
-FrameBasePtrList ProcessorCloseLoopICP::selectCandidates(FrameBasePtr _keyframe_ptr, const Scalar &_time_tolerance)
+FrameBasePtrList ProcessorCloseLoopICP::selectCandidates(FrameBasePtr _keyframe_ptr, const double &_time_tolerance)
 {
     FrameBasePtrList candidates;
     int key_frames_counter = 0;
@@ -154,9 +154,9 @@ FrameBasePtrList ProcessorCloseLoopICP::selectCandidates(FrameBasePtr _keyframe_
     WOLF_INFO("%%%%%%%%%%%%%%%%%% CANDIDATES SIZE ", candidates.size());
     return candidates;
 }
-std::map<Scalar, CapturesAligned> ProcessorCloseLoopICP::evaluateCandidates(FrameBasePtr _keyframe_own, FrameBasePtrList &_keyframe_candidates)
+std::map<double, CapturesAligned> ProcessorCloseLoopICP::evaluateCandidates(FrameBasePtr _keyframe_own, FrameBasePtrList &_keyframe_candidates)
 {
-    std::map<Scalar, CapturesAligned> captures_candidates;
+    std::map<double, CapturesAligned> captures_candidates;
     CaptureLaser2DPtr capture_current = std::static_pointer_cast<wolf::CaptureLaser2D>(_keyframe_own->getCaptureOf(this->getSensor()));
     for(const auto& capture_own : _keyframe_own->getCaptureList())
         {
@@ -171,19 +171,19 @@ std::map<Scalar, CapturesAligned> ProcessorCloseLoopICP::evaluateCandidates(Fram
                             CaptureLaser2DPtr capture_laser_other = std::static_pointer_cast<wolf::CaptureLaser2D>(capture_other);
 
                             // Initial guess for alignment
-                            Eigen::Vector3s transform_guess;
-                            Eigen::Isometry2ds T_w_rown, T_w_rother, T_r_s, T_sother_sown;
+                            Eigen::Vector3d transform_guess;
+                            Eigen::Isometry2d T_w_rown, T_w_rother, T_r_s, T_sother_sown;
 
                             // Transformations
-                            T_w_rown = Eigen::Translation2ds(_keyframe_own->getP()->getState()) * Eigen::Rotation2Ds(_keyframe_own->getO()->getState()(0));
+                            T_w_rown = Eigen::Translation2d(_keyframe_own->getP()->getState()) * Eigen::Rotation2Dd(_keyframe_own->getO()->getState()(0));
                             T_w_rother =
-                                Eigen::Translation2ds(_keyframe_other->getP()->getState()) * Eigen::Rotation2Ds(_keyframe_other->getO()->getState()(0));
-                            T_r_s = Eigen::Translation2ds(this->getSensor()->getP()->getState()) * Eigen::Rotation2Ds(this->getSensor()->getO()->getState()(0));
+                                Eigen::Translation2d(_keyframe_other->getP()->getState()) * Eigen::Rotation2Dd(_keyframe_other->getO()->getState()(0));
+                            T_r_s = Eigen::Translation2d(this->getSensor()->getP()->getState()) * Eigen::Rotation2Dd(this->getSensor()->getO()->getState()(0));
                             T_sother_sown = T_r_s.inverse() * T_w_rother.inverse() * T_w_rown * T_r_s;
 
                             // Fill up initial guess
                             transform_guess.head(2) = T_sother_sown.translation();
-                            Eigen::Rotation2Ds R(T_sother_sown.rotation());
+                            Eigen::Rotation2Dd R(T_sother_sown.rotation());
                             transform_guess(2) = R.angle();
 
                             SensorLaser2DPtr sensor_own, sensor_other;
@@ -206,9 +206,9 @@ std::map<Scalar, CapturesAligned> ProcessorCloseLoopICP::evaluateCandidates(Fram
                             laserscanutils::icpOutput trf =
                                 icp_tools_ptr_->align(capture_laser_own->getScan(), capture_laser_other->getScan(), sensor_own->getScanParams(),
                                                       sensor_other->getScanParams(), this->icp_params_, transform_guess);
-                            Scalar points_coeff_used =
-                                ((Scalar)trf.nvalid / (Scalar)fmin(capture_laser_own->getScan().ranges_raw_.size(), capture_laser_other->getScan().ranges_raw_.size()));
-                            Scalar mean_error = trf.error / trf.nvalid;
+                            double points_coeff_used =
+                                ((double)trf.nvalid / (double)fmin(capture_laser_own->getScan().ranges_raw_.size(), capture_laser_other->getScan().ranges_raw_.size()));
+                            double mean_error = trf.error / trf.nvalid;
                             WOLF_INFO("DBG ------------------------------");
                             WOLF_INFO("DBG valid? ", trf.valid, " m_er ", mean_error, " ", points_coeff_used*100, "% own_id: ", _keyframe_own->id(), " other_id: ", _keyframe_other->id());
                             WOLF_INFO("DBG own_POSE: ", _keyframe_own->getState().transpose(), " other_POSE: ", _keyframe_other->getState().transpose(), " ICP_guess: ", transform_guess.transpose(), " ICP_trf: ", trf.res_transf.transpose());
@@ -223,7 +223,7 @@ std::map<Scalar, CapturesAligned> ProcessorCloseLoopICP::evaluateCandidates(Fram
         }
     return captures_candidates;
 }
-CapturesAligned ProcessorCloseLoopICP::bestCandidate(std::map<Scalar, CapturesAligned> &_capture_candidates)
+CapturesAligned ProcessorCloseLoopICP::bestCandidate(std::map<double, CapturesAligned> &_capture_candidates)
 {
     return _capture_candidates.begin()->second;
 }
diff --git a/src/state_block/local_parametrization_polyline_extreme.cpp b/src/state_block/local_parametrization_polyline_extreme.cpp
index 741c708a8f9c4f7e08bcd768456aca0d71cf25b4..50999a48da61aeb39b75fa81d0e5b33e94f062c7 100644
--- a/src/state_block/local_parametrization_polyline_extreme.cpp
+++ b/src/state_block/local_parametrization_polyline_extreme.cpp
@@ -29,7 +29,7 @@ bool LocalParametrizationPolylineExtreme::plus(Eigen::Map<const Eigen::VectorXd>
 }
 
 bool LocalParametrizationPolylineExtreme::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _point,
-                                                     Eigen::Map<Eigen::MatrixXd>& _jacobian) const
+                                                     Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const
 {
     assert(_point.size() == global_size_ && "Wrong size of input point.");
     assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");