diff --git a/include/laser/processor/processor_odom_icp.h b/include/laser/processor/processor_odom_icp.h index 7f24ed1ee76ee56c7809d2c55373f9cc3704b3b7..1895c713c9714118c9aa7c4b2ef50b03644a2d83 100644 --- a/include/laser/processor/processor_odom_icp.h +++ b/include/laser/processor/processor_odom_icp.h @@ -169,9 +169,9 @@ class ProcessorOdomIcp : public ProcessorTracker, public IsMotion ~ProcessorOdomIcp() override; void configure(SensorBasePtr _sensor) override; - VectorComposite getState() const override; + VectorComposite getState(const StateStructure& _structure = "") const override; TimeStamp getTimeStamp( ) const override; - VectorComposite getState(const TimeStamp& _ts) const override; + VectorComposite getState(const TimeStamp& _ts, const StateStructure& _structure = "") const override; void setProblem(ProblemPtr _problem_ptr) override; protected: diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp index 8ea5e822a278b3f7d4018d06b267652bccdf497b..0cdbe27ec52f9bbd710793f8909d3d70255dd81c 100644 --- a/src/processor/processor_odom_icp.cpp +++ b/src/processor/processor_odom_icp.cpp @@ -319,7 +319,7 @@ FactorBasePtr ProcessorOdomIcp::emplaceFactor(FeatureBasePtr _feature) params_->apply_loss_function); } -VectorComposite ProcessorOdomIcp::getState( ) const +VectorComposite ProcessorOdomIcp::getState( const StateStructure& _structure ) const { Isometry2d w_T_ro = Translation2d(origin_ptr_->getFrame()->getP()->getState()) * Rotation2Dd(origin_ptr_->getFrame()->getO()->getState()(0)); Isometry2d ro_T_so = Translation2d(origin_ptr_->getSensorP()->getState()) * Rotation2Dd(origin_ptr_->getSensorO()->getState()(0)); @@ -348,7 +348,7 @@ TimeStamp ProcessorOdomIcp::getTimeStamp() const return last_ptr_->getTimeStamp(); } -VectorComposite ProcessorOdomIcp::getState(const TimeStamp& _ts) const +VectorComposite ProcessorOdomIcp::getState(const TimeStamp& _ts, const StateStructure& _structure) const { // todo fix this code to get any state in the whole trajectory!