diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp index 0cdbe27ec52f9bbd710793f8909d3d70255dd81c..4ec5e9adb4de8d885f00ae012e0565e0590e8781 100644 --- a/src/processor/processor_odom_icp.cpp +++ b/src/processor/processor_odom_icp.cpp @@ -236,7 +236,7 @@ void ProcessorOdomIcp::advanceDerived() // overwrite last frame's state - Isometry2d w_T_ro = Translation2d(origin_ptr_->getFrame()->getState().at("P")) * Rotation2Dd(origin_ptr_->getFrame()->getState().at("O")(0)); + Isometry2d w_T_ro = Translation2d(origin_ptr_->getFrame()->getState().at('P')) * Rotation2Dd(origin_ptr_->getFrame()->getState().at('O')(0)); Isometry2d ro_T_so = Translation2d(origin_ptr_->getSensorP()->getState()) * Rotation2Dd(origin_ptr_->getSensorO()->getState()(0)); Isometry2d& ri_T_si = ro_T_so; // A reference just to have nice names without computing overhead diff --git a/src/processor/processor_tracker_feature_polyline_2d.cpp b/src/processor/processor_tracker_feature_polyline_2d.cpp index fefda45e831f4b454a63960d7b4f8736cc5dad26..fc7ef54669b0bfec3fee40c3e1477d214660bedc 100644 --- a/src/processor/processor_tracker_feature_polyline_2d.cpp +++ b/src/processor/processor_tracker_feature_polyline_2d.cpp @@ -1310,8 +1310,8 @@ void ProcessorTrackerFeaturePolyline2d::computeTransformations() Eigen::Matrix2d R_world_robot_last = Eigen::Rotation2Dd(vehicle_pose_last(2)).matrix(); // robot_sensor (to be computed once if extrinsics are fixed and not dynamic) - if (getSensor()->isStateBlockDynamic("P") || - getSensor()->isStateBlockDynamic("O") || + if (getSensor()->isStateBlockDynamic('P') || + getSensor()->isStateBlockDynamic('O') || !getSensor()->getP()->isFixed() || !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_)