improved processor odom icp

src/processor/processor_odom_icp.cpp

@@ -77,13 +77,30 @@ void ProcessorOdomIcp::preProcess()
            ("Capture type mismatch. " + getName() + " can only process captures of type CaptureLaser2d").c_str());
 
     if (initial_guess_ == "odom")
-        odom_incoming_ = getProblem()->getOdometry("PO").vector("PO");
+    {
+        auto odom_incoming_composite = getProblem()->getOdometry("PO");
+        if (odom_incoming_composite.has("PO"))
+            odom_incoming_ = odom_incoming_composite.vector("PO");
+        else
+            odom_incoming_ = odom_last_;  // if no odometry available, use identity as initial guess
+    }
     else if (initial_guess_ == "state")
     {
-        odom_incoming_ = getProblem()->getState("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");
+        auto state_incoming_composite = getProblem()->getState("PO");
+        if (state_incoming_composite.has("PO"))
+            odom_incoming_ = state_incoming_composite.vector("PO");
+        else
+            odom_incoming_ = odom_last_;
+        if (last_ptr_)
+        {
+            auto state_last_composite = getProblem()->getState(last_ptr_->getTimeStamp(), "PO");
+            if (state_last_composite.has("PO")) odom_last_ = state_last_composite.vector("PO");
+        }
+        if (origin_ptr_)
+        {
+            auto state_origin_composite = getProblem()->getState(origin_ptr_->getTimeStamp(), "PO");
+            if (state_origin_composite.has("PO")) odom_origin_ = state_origin_composite.vector("PO");
+        }
     }
 }
 
@@ -121,7 +138,7 @@ void ProcessorOdomIcp::updateExtrinsicsIsometries()
         ro_T_so_ = laser::trf2isometry(origin_ptr_->getSensorP()->getState(), origin_ptr_->getSensorP()->getState());
         rl_T_sl_ = laser::trf2isometry(last_ptr_->getSensorP()->getState(), last_ptr_->getSensorP()->getState());
     }
-    // statics not fixed (otherwise nothing to do)
+    // estimated
     else if (not sensor_laser_->getP()->isFixed() or not sensor_laser_->getO()->isFixed())
     {
         ro_T_so_ = laser::trf2isometry(sensor_laser_->getP()->getState(), sensor_laser_->getO()->getState());
@@ -465,7 +482,7 @@ VectorComposite ProcessorOdomIcp::getState(StateKeys _keys) const
                 if (state_types_.has(key)) keys_available += key;
             }
             _keys = keys_available;
-            WOLF_WARN("Processor has no all keys asked (", _keys, "). Returning the available ones: ", _keys);
+            WOLF_DEBUG("Processor has no all keys asked (", _keys, "). Returning the available ones: ", _keys);
         }
     }
 
@@ -531,7 +548,13 @@ VectorComposite ProcessorOdomIcp::getState(const TimeStamp& _ts, StateKeys _keys
     if (origin_ptr_ and fabs(_ts - origin_ptr_->getTimeStamp()) < getTimeTolerance())
         return origin_ptr_->getFrame()->getState("PO");
 
-    WOLF_WARN(getName(), ": Requested state with time stamp out of tolerance. Returning empty VectorComposite");
+    WOLF_WARN(getName(),
+              ": Requested state with time stamp out of tolerance. Returning empty VectorComposite. Requested ts: ",
+              _ts,
+              " - last: ",
+              last_ptr_->getTimeStamp(),
+              " - origin: ",
+              origin_ptr_->getTimeStamp());
     return VectorComposite();
 }