Merge branch '9-adapt-to-state-block-map' into 'devel'

Resolve "Adapt to  state block map"

Closes #9

See merge request !15

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());

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_)