diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp index a5ad7357fef93368c8cc047ed8231964ca8073d4..a1f0aee499cf8a3080d7ded796d792544a7c78c8 100644 --- a/src/processor/processor_odom_icp.cpp +++ b/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(); }