diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 077d37acdb368f0e7cd6dd9e79476ac6347c0d06..2d763fbd8be467c230417270bac24788ed06a0e7 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -34,9 +34,11 @@ stages:
   - cd $CI_PROJECT_DIR
 
   # configure git
-  - export CI_NEW_BRANCH=ci_processing$RANDOM
+  - if [ `git rev-parse --verify ci_processing 2>/dev/null` ]; then
+  -   git branch --delete ci_processing
+  - fi
+  - export CI_NEW_BRANCH=ci_processing
   - echo creating new temporary branch... $CI_NEW_BRANCH
-  #- export CI_NEW_BRANCH=ci_processing$CI_COMMIT_SHORT_SHA
   - git config --global user.email "${CI_EMAIL}"
   - git config --global user.name "${CI_USERNAME}"
   - git checkout -b $CI_NEW_BRANCH # temporary branch
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 11db1957193ec28d5a6b61b62204137f7e085148..c83af3bd66ab395c465600fc2f604f04c1d453ed 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,5 @@
 # Pre-requisites about cmake itself
-CMAKE_MINIMUM_REQUIRED(VERSION 3.10)
+CMAKE_MINIMUM_REQUIRED(VERSION 3.16)
 
 # The project name and the type of project
 PROJECT(laser_scan_utils)
diff --git a/examples/polyline_demo.cpp b/examples/polyline_demo.cpp
index 634f8eb915d4859916dc88399417c147f88ba32d..4bb848cab67a6a968fb76aaf79f7c57798ea43f9 100644
--- a/examples/polyline_demo.cpp
+++ b/examples/polyline_demo.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/corner_falko_2d.h b/include/laser_scan_utils/corner_falko_2d.h
index 6c951fdf2809e3b123bbf952ae22d77ddaafc65d..94911a42cfc9a1344da03498eecfebf93dba936e 100644
--- a/include/laser_scan_utils/corner_falko_2d.h
+++ b/include/laser_scan_utils/corner_falko_2d.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/corner_finder.h b/include/laser_scan_utils/corner_finder.h
index c4a7d3aa2a2daf88a509cdef7a7f3ff562faa46d..fdab44af6e660639049b2a99926bdaecf5063556 100644
--- a/include/laser_scan_utils/corner_finder.h
+++ b/include/laser_scan_utils/corner_finder.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/corner_finder_inscribed_angle.h b/include/laser_scan_utils/corner_finder_inscribed_angle.h
index 65518f4bc181fde6984eb8a0e4494287ef52b7b3..6f16c9c01d6afdd5c43d73e1aae2406a5dd9c31f 100644
--- a/include/laser_scan_utils/corner_finder_inscribed_angle.h
+++ b/include/laser_scan_utils/corner_finder_inscribed_angle.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/corner_point.h b/include/laser_scan_utils/corner_point.h
index 08637510c5171dc29b2c6070ec9aacd5517f8bf9..a85ff032d7abb27f900704dfd3ca037262743cc8 100644
--- a/include/laser_scan_utils/corner_point.h
+++ b/include/laser_scan_utils/corner_point.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/grid_2d.h b/include/laser_scan_utils/grid_2d.h
index 5da0737de194a7ac42ac8db1a3f51396bd2b5863..00b615435b793521e31a98cecfa5ff213cea7421 100644
--- a/include/laser_scan_utils/grid_2d.h
+++ b/include/laser_scan_utils/grid_2d.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/grid_cluster.h b/include/laser_scan_utils/grid_cluster.h
index be510ba0fa868572fdc76b8799d26ad9eb383a41..026b62129957c2145f2cb75eb26e8345b2c80bf1 100644
--- a/include/laser_scan_utils/grid_cluster.h
+++ b/include/laser_scan_utils/grid_cluster.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/icp.h b/include/laser_scan_utils/icp.h
index 113b23e3959c45d43e8698da181fa3c17fa9d40d..71ba9678023671b5c2f3ceeb11887cfe39444a3a 100644
--- a/include/laser_scan_utils/icp.h
+++ b/include/laser_scan_utils/icp.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/laser_scan.h b/include/laser_scan_utils/laser_scan.h
index 8871fcc8643f4fb0fe4ec1609c4a48f5f9f88cbe..144f031d4e46aa87dd7af207581f6d9470e162ba 100644
--- a/include/laser_scan_utils/laser_scan.h
+++ b/include/laser_scan_utils/laser_scan.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/laser_scan_utils.h b/include/laser_scan_utils/laser_scan_utils.h
index 7eb29a6173b24293aa302f0bbbf6c1d8637fef78..b055283cb984abb36f27aa875fca148522fa59dc 100644
--- a/include/laser_scan_utils/laser_scan_utils.h
+++ b/include/laser_scan_utils/laser_scan_utils.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/laser_scan_with_params.h b/include/laser_scan_utils/laser_scan_with_params.h
index 45ab37dfd46d52c1de027ad52cc7c24b60765361..db93fc00cb13fa9794083f5607be3d989f460bff 100644
--- a/include/laser_scan_utils/laser_scan_with_params.h
+++ b/include/laser_scan_utils/laser_scan_with_params.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/line_finder.h b/include/laser_scan_utils/line_finder.h
index c2281ae19d8e8e519c1c7a69f9c40b8573ac039f..5696da18fa50106dd904a75648a8d6773d79af1a 100644
--- a/include/laser_scan_utils/line_finder.h
+++ b/include/laser_scan_utils/line_finder.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/line_finder_hough.h b/include/laser_scan_utils/line_finder_hough.h
index eaf538668b37f5583292608892403133f8792c51..1b4f8840bd5870458113263015b538cfb6354032 100644
--- a/include/laser_scan_utils/line_finder_hough.h
+++ b/include/laser_scan_utils/line_finder_hough.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/line_finder_iterative.h b/include/laser_scan_utils/line_finder_iterative.h
index 2e04dfeca12064928beeece50c4c3e1ceb60eea7..c68055cbb5f39f81aba83ac3833861df1a1f962e 100644
--- a/include/laser_scan_utils/line_finder_iterative.h
+++ b/include/laser_scan_utils/line_finder_iterative.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/line_finder_jump_fit.h b/include/laser_scan_utils/line_finder_jump_fit.h
index d967615c0fcc5bc313fe63e853dcf9ebb7f4b943..ed3acb1061a5e12fe82d760ab6516c63ad778656 100644
--- a/include/laser_scan_utils/line_finder_jump_fit.h
+++ b/include/laser_scan_utils/line_finder_jump_fit.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/line_segment.h b/include/laser_scan_utils/line_segment.h
index bd03fa8f14c460ac4335e23498776d69f3dad7fc..6b5b28a9eb73c939bf88ce5f4afdf45fe4c5383a 100644
--- a/include/laser_scan_utils/line_segment.h
+++ b/include/laser_scan_utils/line_segment.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/loop_closure_base.h b/include/laser_scan_utils/loop_closure_base.h
index 1bd342bc8b22320722fb5ad3c10483c7b0e7c349..f509f869f81602860a33740618b35f622266f883 100644
--- a/include/laser_scan_utils/loop_closure_base.h
+++ b/include/laser_scan_utils/loop_closure_base.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/loop_closure_falko.h b/include/laser_scan_utils/loop_closure_falko.h
index 52559daadb99d5a587434747a012fc510cfc6133..c5b3e1d2a104118f594ab4c59c929a9213574905 100644
--- a/include/laser_scan_utils/loop_closure_falko.h
+++ b/include/laser_scan_utils/loop_closure_falko.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/match_loop_closure_scene.h b/include/laser_scan_utils/match_loop_closure_scene.h
index 96e0cd634d8175fb0480d56b6818ec04c44fc70e..98053932e2f9f7b81b16c668a4d5b39c7e2bdb1b 100644
--- a/include/laser_scan_utils/match_loop_closure_scene.h
+++ b/include/laser_scan_utils/match_loop_closure_scene.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/point_set.h b/include/laser_scan_utils/point_set.h
index 516efd5f9757945252224ed9e69130be5557a087..1dc8b8e90ad819f588782693c8f451f479557ee8 100644
--- a/include/laser_scan_utils/point_set.h
+++ b/include/laser_scan_utils/point_set.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/polyline.h b/include/laser_scan_utils/polyline.h
index 7769a2ac33b1a414b1fc15e436658967db4e3507..03012f69ae513ad05b1333e4341b52f0d227a525 100644
--- a/include/laser_scan_utils/polyline.h
+++ b/include/laser_scan_utils/polyline.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/scan_segment.h b/include/laser_scan_utils/scan_segment.h
index 39bad354f0ff63e0296c8da41da0caf9e6bbadb5..21520f7bc801dbf295733ca0d62fe961569866bc 100644
--- a/include/laser_scan_utils/scan_segment.h
+++ b/include/laser_scan_utils/scan_segment.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/scene_base.h b/include/laser_scan_utils/scene_base.h
index b53bd00917370027986c25b778926e82bd4ad61a..cb3d2220ca1e1cb4105e3213371c09f0d7cf546f 100644
--- a/include/laser_scan_utils/scene_base.h
+++ b/include/laser_scan_utils/scene_base.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/include/laser_scan_utils/scene_falko.h b/include/laser_scan_utils/scene_falko.h
index b341c8014883db3146c77aa321c6b299489c22ab..9719b465b607ba404e4f1c8b87ea9d6293841f5a 100644
--- a/include/laser_scan_utils/scene_falko.h
+++ b/include/laser_scan_utils/scene_falko.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/scripts/license_header_2022.txt b/scripts/license_header_2024.txt
similarity index 88%
rename from scripts/license_header_2022.txt
rename to scripts/license_header_2024.txt
index 2e6616c0ff54d5e75cb9fa652a78e00b660da4a0..79c7f8e83b1e45193da1eef04ebc4df4a4c6a9ba 100644
--- a/scripts/license_header_2022.txt
+++ b/scripts/license_header_2024.txt
@@ -1,4 +1,4 @@
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/src/corner_falko_2d.cpp b/src/corner_falko_2d.cpp
index 3b98f1eb63e2c8347ef0ebbb89f59768c8a4d48e..1b49e3316eaaa757e39ddda3ab15e6e461e8deee 100644
--- a/src/corner_falko_2d.cpp
+++ b/src/corner_falko_2d.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/src/corner_finder.cpp b/src/corner_finder.cpp
index d4d5f6d8facdfe0927bfb0e7dbf4abb144adc67b..023b6cd817a1d140b5c5dbf481da9b26793a9459 100644
--- a/src/corner_finder.cpp
+++ b/src/corner_finder.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/src/corner_finder_inscribed_angle.cpp b/src/corner_finder_inscribed_angle.cpp
index 3783b8dd4181e0ef62b0a24c468e1585850e0991..5e8d7e4823504f8950ee685637a1b56ec49012c3 100644
--- a/src/corner_finder_inscribed_angle.cpp
+++ b/src/corner_finder_inscribed_angle.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/src/corner_point.cpp b/src/corner_point.cpp
index d966f381f4edd320080bbdca2418ca63d15dc6ab..ebc5338616e21e0373211b7ecf95941f809aaf0b 100644
--- a/src/corner_point.cpp
+++ b/src/corner_point.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/src/grid_2d.cpp b/src/grid_2d.cpp
index b4dff7e56b48bdae0bb59542b841086cd93bdb57..a9cafde5fd2d3ae32624c43bc62602949d133b31 100644
--- a/src/grid_2d.cpp
+++ b/src/grid_2d.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/src/grid_cluster.cpp b/src/grid_cluster.cpp
index 17168cf8136b71f88e0810592dc293989f0e4669..aa129436facb83c3b02a58c923cab7b7e0e7d6f2 100644
--- a/src/grid_cluster.cpp
+++ b/src/grid_cluster.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/src/icp.cpp b/src/icp.cpp
index 1e60fbc7f77fdaff24b9c542feb3b8eefc35d1be..354a905d86521fac7a72650b6cac461a32849e03 100644
--- a/src/icp.cpp
+++ b/src/icp.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
@@ -165,9 +165,9 @@ icpOutput ICP::align(const LaserScan &_current_ls,
 
     if (result.valid)
     {
-        result.res_transf(0) = csm_output.x[0];//*_current_scan_params.range_max_/100;
-        result.res_transf(1) = csm_output.x[1];//*_current_scan_params.range_max_/100;
-        result.res_transf(2) = csm_output.x[2];//*_current_scan_params.range_max_/100;
+        result.res_transf(0) = csm_output.x[0];
+        result.res_transf(1) = csm_output.x[1];
+        result.res_transf(2) = csm_output.x[2];
 
         if (csm_input.do_compute_covariance)
         {
@@ -176,21 +176,21 @@ icpOutput ICP::align(const LaserScan &_current_ls,
                     result.res_covar(i, j) = _icp_params.cov_factor *
                     csm_output.cov_x_m->data[i * csm_output.cov_x_m->tda + j];
             
-            // Multiply variance in bigger eigenvalue direction
-            if (abs(_icp_params.cov_factor - 1) > 1e3)
+            // Grow covariance in the biggest eigenvalue direction
+            if (_icp_params.cov_max_eigv_factor - 1 > 1e-6)
             {
-                Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(result.res_covar);
+                Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(result.res_covar.topLeftCorner<2,2>());
 
                 if (eigensolver.info() == Eigen::Success)
                 {
-                    Eigen::Vector3d eigvs = eigensolver.eigenvalues();
+                    Eigen::Vector2d eigvs = eigensolver.eigenvalues();
                     Eigen::Index maxRow, maxCol;
                     float max_eigv = eigvs.maxCoeff(&maxRow, &maxCol);
 
-                    eigvs(maxRow) = _icp_params.cov_factor * max_eigv;
-                    result.res_covar = eigensolver.eigenvectors() *
-                                       eigvs.asDiagonal() *
-                                       eigensolver.eigenvectors().transpose();
+                    eigvs(maxRow) = _icp_params.cov_max_eigv_factor * max_eigv;
+                    result.res_covar.topLeftCorner<2,2>() = eigensolver.eigenvectors() *
+                                                            eigvs.asDiagonal() *
+                                                            eigensolver.eigenvectors().transpose();
                 }
             }
         }
diff --git a/src/laser_scan.cpp b/src/laser_scan.cpp
index 2827e90d8ef8e8a89b3cde6eccf9dfd03a418055..18a0cb2e05583856b9228f35e56c44b22bd6262b 100644
--- a/src/laser_scan.cpp
+++ b/src/laser_scan.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/src/laser_scan_with_params.cpp b/src/laser_scan_with_params.cpp
index 8e5ce0d99f0bf8d0b15fe45071cb43a06c389acd..7bfe8b2c787a8d4a81ce32da6c65eba2656d1c43 100644
--- a/src/laser_scan_with_params.cpp
+++ b/src/laser_scan_with_params.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/src/line_finder.cpp b/src/line_finder.cpp
index 418a40499e7f8eadaba675624cc1c7d5b280e8c4..964482ab09deec7cd9f16191771a47b4ed0f3714 100644
--- a/src/line_finder.cpp
+++ b/src/line_finder.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/src/line_finder_hough.cpp b/src/line_finder_hough.cpp
index 930015cc99e1d3f29b891c07de728ebbb4894250..79a2e3c25f620b02e5a66a789804d8b3d8b94c4f 100644
--- a/src/line_finder_hough.cpp
+++ b/src/line_finder_hough.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/src/line_finder_iterative.cpp b/src/line_finder_iterative.cpp
index d83e5ee2cbd64cde3570ca6a4b1503f8c925e9ff..bd6150c17d16b50c217bae5a1e60ee5f89454354 100644
--- a/src/line_finder_iterative.cpp
+++ b/src/line_finder_iterative.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/src/line_finder_jump_fit.cpp b/src/line_finder_jump_fit.cpp
index 38a1ea71a8c42eabe1af47272af31f8a9138ea21..614049c06e0e3207bc01d3f54d6e3e1d9de903ad 100644
--- a/src/line_finder_jump_fit.cpp
+++ b/src/line_finder_jump_fit.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/src/line_segment.cpp b/src/line_segment.cpp
index a5c9c893fecf661d3c8b197664d614dee3d84012..66d3a557f8834c6adee4b854d3af245d4320ec2f 100644
--- a/src/line_segment.cpp
+++ b/src/line_segment.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/src/loop_closure_base.cpp b/src/loop_closure_base.cpp
index 177a7b897555dbd22cc65cd6c2c140bfa19e2701..68c76bf53d699b2c2e4bf7f5d13b4960d9fd332f 100644
--- a/src/loop_closure_base.cpp
+++ b/src/loop_closure_base.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/src/point_set.cpp b/src/point_set.cpp
index a20fb16ce043add97d8a21f94834bfd69f854987..80b1eb262fb1cd99b13d8ea10879920c98d12ff1 100644
--- a/src/point_set.cpp
+++ b/src/point_set.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/src/polyline.cpp b/src/polyline.cpp
index 28bc1d617e89cd7588fab1a34ec926ccedce5521..75e1ab852b3d8c3b74bcd8056ab54d72f8027d9a 100644
--- a/src/polyline.cpp
+++ b/src/polyline.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/src/scan_segment.cpp b/src/scan_segment.cpp
index 94b81a462e23bae12dddd2873a9611a40e6763d8..7abb6eabd282e940e010839b0c11d0a8350149e8 100644
--- a/src/scan_segment.cpp
+++ b/src/scan_segment.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 3234bd9b88144230baa4ac384d7bb97408b8c192..da699ec408bbb5680d1d07be82a6c37a692aa201 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -4,18 +4,18 @@ add_subdirectory(gtest)
 ############# USE THIS TEST AS AN EXAMPLE ####################
 #                                                            #
 # Create a specific test executable for gtest_example        #
-# laser_scan_utils_add_gtest(gtest_example gtest_example.cpp)#
+# add_gtest(gtest_example gtest_example.cpp)#
 #                                                            #
 ##############################################################
 
 # gtest example
-laser_scan_utils_add_gtest(gtest_example gtest_example.cpp)
+add_gtest(gtest_example gtest_example.cpp)
 
 # gtest icp
 IF(csm_FOUND)
-	laser_scan_utils_add_gtest(gtest_icp gtest_icp.cpp)
+	add_gtest(gtest_icp gtest_icp.cpp)
 ENDIF(csm_FOUND)
 
 IF(falkolib_FOUND)
-	laser_scan_utils_add_gtest(gtest_loop_closure_falko gtest_loop_closure_falko.cpp)
+	add_gtest(gtest_loop_closure_falko gtest_loop_closure_falko.cpp)
 ENDIF(falkolib_FOUND)
\ No newline at end of file
diff --git a/test/data/scan_data.h b/test/data/scan_data.h
index 46516f63bec63c83663d5d4f1bb05e34ce6cd5a9..4c48f052634d163bc24f6de10ca5c8a5119827ea 100644
--- a/test/data/scan_data.h
+++ b/test/data/scan_data.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/test/gtest/CMakeLists.txt b/test/gtest/CMakeLists.txt
index 00fa077402ab147ea05bb4c8346e321f30d66100..3b52f512ade635f2beb371d13805ab1faefabc82 100644
--- a/test/gtest/CMakeLists.txt
+++ b/test/gtest/CMakeLists.txt
@@ -1,73 +1,15 @@
-if(${CMAKE_VERSION} VERSION_LESS "3.11.0") 
-  message("CMake version less than 3.11.0")
+include(FetchContent)
 
-  # Enable ExternalProject CMake module
-  include(ExternalProject)
+FetchContent_Declare(
+  googletest
+  GIT_REPOSITORY https://github.com/google/googletest.git 
+  GIT_TAG main)
 
-  set(GTEST_FORCE_SHARED_CRT ON)
-  set(GTEST_DISABLE_PTHREADS ON) # without this in ubuntu 18.04 we get linking errors
+SET(INSTALL_GTEST OFF) # Disable installation of googletest
+FetchContent_MakeAvailable(googletest)
 
-  # Download GoogleTest
-  ExternalProject_Add(googletest
-      GIT_REPOSITORY https://github.com/google/googletest.git
-      GIT_TAG        v1.8.x
-      # TIMEOUT 1 # We'll try this
-      CMAKE_ARGS -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG:PATH=DebugLibs
-      -DCMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE:PATH=ReleaseLibs
-      -DCMAKE_CXX_FLAGS=${MSVC_COMPILER_DEFS}
-      -Dgtest_force_shared_crt=${GTEST_FORCE_SHARED_CRT}
-      -Dgtest_disable_pthreads=${GTEST_DISABLE_PTHREADS}
-      -DBUILD_GTEST=ON
-      PREFIX "${CMAKE_CURRENT_BINARY_DIR}"
-      # Disable install step
-      INSTALL_COMMAND ""
-      UPDATE_DISCONNECTED 1 # 1: do not update googletest; 0: update googletest via github
-  )
-
-  # Get GTest source and binary directories from CMake project
-
-  # Specify include dir
-  ExternalProject_Get_Property(googletest source_dir)
-  set(GTEST_INCLUDE_DIRS ${source_dir}/googletest/include PARENT_SCOPE)
-
-  # Specify MainTest's link libraries
-  ExternalProject_Get_Property(googletest binary_dir)
-  set(GTEST_LIBS_DIR ${binary_dir}/googlemock/gtest PARENT_SCOPE)
-
-  # Create a libgtest target to be used as a dependency by test programs
-  add_library(libgtest IMPORTED STATIC GLOBAL)
-  add_dependencies(libgtest googletest)
-
-  # Set libgtest properties
-  set_target_properties(libgtest PROPERTIES
-      "IMPORTED_LOCATION" "${binary_dir}/googlemock/gtest/libgtest.a"
-      "IMPORTED_LINK_INTERFACE_LIBRARIES" "${CMAKE_THREAD_LIBS_INIT}"
-  )
-
-else()
-
-  message("CMake version equal or greater than 3.11.0")
-
-  include(FetchContent)
-
-  FetchContent_Declare(
-    googletest
-    GIT_REPOSITORY https://github.com/google/googletest.git 
-    GIT_TAG v1.8.x)
-
-  SET(INSTALL_GTEST OFF) # Disable installation of googletest
-  FetchContent_MakeAvailable(googletest)
-
-endif()
-  
-function(laser_scan_utils_add_gtest target)
+function(add_gtest target)
   add_executable(${target} ${ARGN})
-  if(${CMAKE_VERSION} VERSION_LESS "3.11.0") 
-    add_dependencies(${target} libgtest)
-    target_link_libraries(${target} PUBLIC libgtest ${PROJECT_NAME})
-    target_include_directories(${target} PUBLIC ${GTEST_INCLUDE_DIRS})
-  else()
-    target_link_libraries(${target} PUBLIC gtest_main ${PROJECT_NAME})
-  endif()
+  target_link_libraries(${target} PUBLIC gtest_main ${PROJECT_NAME})
   add_test(NAME ${target} COMMAND ${target})
 endfunction()
diff --git a/test/gtest/utils_gtest.h b/test/gtest/utils_gtest.h
index cdc9cf0193ac3eaf44f694827b43c4419712e23d..15ab2385027cb84056c986e181ca68227b48acff 100644
--- a/test/gtest/utils_gtest.h
+++ b/test/gtest/utils_gtest.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/test/gtest_example.cpp b/test/gtest_example.cpp
index f2258b6a11b17e82e00f3f798164aa2e7e45c017..3fd271cc234acfa3854a7b7ab964a11d4b8700d9 100644
--- a/test/gtest_example.cpp
+++ b/test/gtest_example.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/test/gtest_icp.cpp b/test/gtest_icp.cpp
index 481f367b23eb6ed6fc91d90f0887e6ea7158b3c4..e3ec9b44c73ce89e62f9abd068651abaffb673d6 100644
--- a/test/gtest_icp.cpp
+++ b/test/gtest_icp.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp
index 56be534370a343e148965ca8d3eb3923645a94e2..6225f5fa6c909f4d7de69ca15bd0775c8c5eb0d4 100644
--- a/test/gtest_loop_closure_falko.cpp
+++ b/test/gtest_loop_closure_falko.cpp
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //
diff --git a/test/matplotlibcpp.h b/test/matplotlibcpp.h
index cd41047f0a289ba605215d4fe86b7a3fd2c4bed2..c1b2d9b41949609046d286843d9e99d5f872ae8a 100644
--- a/test/matplotlibcpp.h
+++ b/test/matplotlibcpp.h
@@ -1,6 +1,6 @@
 //--------LICENSE_START--------
 //
-// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
 // Authors: Joan Vallvé Navarro (jvallve@iri.upc.edu)
 // All rights reserved.
 //