diff --git a/src/processor/processor_closeloop_icp.cpp b/src/processor/processor_closeloop_icp.cpp index 6b56ed474bce3a127470877a5ce983b228c37c2f..721619894428beab720720aa312fea2c8ca9ddbd 100644 --- a/src/processor/processor_closeloop_icp.cpp +++ b/src/processor/processor_closeloop_icp.cpp @@ -207,7 +207,7 @@ std::map<Scalar, CapturesAligned> ProcessorCloseLoopICP::evaluateCandidates(Fram icp_tools_ptr_->align(capture_laser_own->getScan(), capture_laser_other->getScan(), sensor_own->getScanParams(), sensor_other->getScanParams(), this->icp_params_, transform_guess); Scalar points_coeff_used = - ((Scalar)trf.nvalid / (Scalar)min(capture_laser_own->getScan().ranges_raw_.size(), capture_laser_other->getScan().ranges_raw_.size())); + ((Scalar)trf.nvalid / (Scalar)fmin(capture_laser_own->getScan().ranges_raw_.size(), capture_laser_other->getScan().ranges_raw_.size())); Scalar mean_error = trf.error / trf.nvalid; WOLF_INFO("DBG ------------------------------"); WOLF_INFO("DBG valid? ", trf.valid, " m_er ", mean_error, " ", points_coeff_used*100, "% own_id: ", _keyframe_own->id(), " other_id: ", _keyframe_other->id()); diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp index 9f463ca44dec39f288eca06c1bab708993907bfa..1edad9f4ae6276b3c73ef34a62226bbd4de8ee59 100644 --- a/src/processor/processor_tracker_feature_polyline_2D.cpp +++ b/src/processor/processor_tracker_feature_polyline_2D.cpp @@ -1311,7 +1311,8 @@ void ProcessorTrackerFeaturePolyline2D::computeTransformations() Eigen::Matrix2s R_world_robot_last = Eigen::Rotation2Ds(vehicle_pose_last(2)).matrix(); // robot_sensor (to be computed once if extrinsics are fixed and not dynamic) - if (getSensor()->isExtrinsicDynamic() || + if (getSensor()->isStateBlockDynamic("P") || + getSensor()->isStateBlockDynamic("O") || !getSensor()->getP()->isFixed() || !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_)