Merge branch '23-icp-develop-matching-tools' of...

Merge branch '23-icp-develop-matching-tools' of https://jvallve@gitlab.iri.upc.edu/mobile_robotics/laser_scan_utils.git into 23-icp-develop-matching-tools

src/CMakeLists.txt

@@ -10,10 +10,12 @@ IF(faramotics_FOUND)
     MESSAGE("Faramotics Library FOUND: Tests requiring it will be built.")
 ENDIF(faramotics_FOUND)
 
-FIND_PACKAGE(csm REQUIRED)
+FIND_PACKAGE(csm QUIET)
 #include directories
 INCLUDE_DIRECTORIES(.)
-INCLUDE_DIRECTORIES(${csm_INCLUDE_DIR})
+IF(csm_FOUND)
+    INCLUDE_DIRECTORIES(${csm_INCLUDE_DIR})
+ENDIF(csm_FOUND)
 IF(Ceres_FOUND)
     INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
 ENDIF(Ceres_FOUND)
@@ -41,7 +43,11 @@ SET(HDRS
     point_set.h
     polyline.h
     scan_segment.h
-    icp.h)
+    )
+  IF(csm_FOUND)
+    SET(HDRS ${HDRS}
+      icp.h)
+  ENDIF(csm_FOUND)
 
 #sources
 SET(SRCS
@@ -60,8 +66,11 @@ SET(SRCS
     point_set.cpp
     polyline.cpp
     scan_segment.cpp
-    icp.cpp)
-
+    )
+  IF(csm_FOUND)
+    SET(SRCS ${SRCS}
+      icp.cpp)
+  ENDIF(csm_FOUND)
 # create the shared library
 ADD_LIBRARY(${PROJECT_NAME} SHARED ${SRCS})
 target_link_libraries(${PROJECT_NAME} ${csm_LIBRARY})

src/icp.cpp

@@ -88,6 +88,7 @@ icpOutput ICP::align(const LaserScan &_last_ls, const LaserScan &_origin_ls, con
     csm_input.outliers_adaptive_order   = icp_params.outliers_adaptive_order;
     csm_input.outliers_adaptive_mult    = icp_params.outliers_adaptive_mult;
 
+
     csm_input.do_compute_covariance     = 1;
 
     sm_icp(&csm_input, &csm_output);