Merge branch 'devel' into refactor-odometry

cmake_modules/Findcsm.cmake

@@ -47,7 +47,7 @@ macro(csm_report_not_found REASON_MSG)
   # FindPackage() use the camelcase library name, not uppercase.
   if (csm_FIND_QUIETLY)
     message(STATUS "Failed to find csm- " ${REASON_MSG} ${ARGN})
-  else (csm_FIND_REQUIRED)
+  elseif (csm_FIND_REQUIRED)
     message(FATAL_ERROR "Failed to find csm - " ${REASON_MSG} ${ARGN})
   else()
     # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error

include/laser/state_block/local_parametrization_polyline_extreme.h

@@ -31,6 +31,11 @@ class LocalParametrizationPolylineExtreme : public LocalParametrizationBase
         bool minus(Eigen::Map<const Eigen::VectorXd>& _point1,
                            Eigen::Map<const Eigen::VectorXd>& _point2,
                            Eigen::Map<Eigen::VectorXd>& _delta_theta) override;
+        bool isValid(const Eigen::VectorXd& _state, double tolerance) override
+        {
+            return _state.size() == global_size_;
+        }
+
 };
 
 } // namespace wolf