fixed laser prior

src/processor/processor_odom_icp.cpp

@@ -41,8 +41,14 @@ void ProcessorOdomIcp::preProcess()
     else if (params_odom_icp_->initial_guess == "state" )
     {
         odom_incoming_  = getProblem()->getState("PO").vector("PO");
-        odom_last_      = getProblem()->getState(last_ptr_->getTimeStamp(), "PO").vector("PO");
-        odom_origin_    = getProblem()->getState(origin_ptr_->getTimeStamp(), "PO").vector("PO");
+        if(last_ptr_)
+        {
+            odom_last_      = getProblem()->getState(last_ptr_->getTimeStamp(), "PO").vector("PO");
+        }
+        if(origin_ptr_)
+        {
+            odom_origin_    = getProblem()->getState(origin_ptr_->getTimeStamp(), "PO").vector("PO");
+        }
     }
 
     assert(odom_incoming_.size() == 3);
@@ -63,15 +69,14 @@ unsigned int ProcessorOdomIcp::processKnown()
         CaptureLaser2dPtr origin_ptr    = std::static_pointer_cast<CaptureLaser2d>(origin_ptr_);
         CaptureLaser2dPtr incoming_ptr  = std::static_pointer_cast<CaptureLaser2d>(incoming_ptr_);
 
-        Eigen::Vector3d initial_guess;
+        Eigen::Vector3d initial_guess = this->trf_origin_last_.res_transf;
         if (params_odom_icp_->initial_guess == "odom" or params_odom_icp_->initial_guess == "state" )
         {
-            initial_guess.head(2) = Eigen::Rotation2Dd(-odom_incoming_(2)) * (odom_origin_.head(2) - odom_incoming_.head(2));
-            initial_guess(2) = -(odom_incoming_(2) - odom_origin_(2));
+            //TODO: Add sensor extrinsics
+            initial_guess.head(2) += Eigen::Rotation2Dd(-odom_origin_(2)) * (odom_last_.head(2) - odom_incoming_.head(2));
+            initial_guess(2) += -(odom_incoming_(2) - odom_last_(2));
         }
-        else if (params_odom_icp_->initial_guess == "zero")
-            initial_guess = this->trf_origin_last_.res_transf;
-        else
+        else if (params_odom_icp_->initial_guess != "zero")
             throw std::runtime_error("unknown value for param 'initial_guess'. Should be 'odom', 'state' or 'zero'");
 
         trf_origin_incoming_ = icp_tools_ptr_->align(incoming_ptr->getScan(),
@@ -102,6 +107,8 @@ unsigned int ProcessorOdomIcp::processNew(const int& _max_features)
           initial_guess.head(2) = Eigen::Rotation2Dd(-odom_incoming_(2)) * (odom_last_.head(2) - odom_incoming_.head(2));
           initial_guess(2) = -(odom_incoming_(2) - odom_last_(2));
     }
+    else if (params_odom_icp_->initial_guess != "zero")
+      throw std::runtime_error("unknown value for param 'initial_guess'. Should be 'odom', 'state' or 'zero'");
 
     trf_last_incoming_ =  icp_tools_ptr_->align(incoming_ptr->getScan(),
                                                 last_ptr->getScan(),