ros-noetic-slam-toolbox.patch

diff --git a/CMakeLists.txt b/CMakeLists.txt
index b423d97..17e60c8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -40,6 +40,7 @@ find_package(Cholmod REQUIRED)
 find_package(LAPACK REQUIRED)
 find_package(Ceres REQUIRED COMPONENTS SuiteSparse)
 find_package(rostest REQUIRED)
+find_package(TBB REQUIRED CONFIG)
 
 set(CMAKE_INCLUDE_CURRENT_DIR ON)
 add_definitions(-DQT_NO_KEYWORDS)
@@ -90,7 +91,7 @@ add_library(ceres_solver_plugin solvers/ceres_solver.cpp)
 target_link_libraries(ceres_solver_plugin ${catkin_LIBRARIES} 
                                           ${CERES_LIBRARIES}
                                           ${Boost_LIBRARIES}
-                                          ${TBB_LIBRARIES}
+                                          TBB::tbb
 )
 
 #### Tool lib for mapping
diff --git a/lib/karto_sdk/CMakeLists.txt b/lib/karto_sdk/CMakeLists.txt
index ad6ef02..15eab81 100644
--- a/lib/karto_sdk/CMakeLists.txt
+++ b/lib/karto_sdk/CMakeLists.txt
@@ -8,7 +8,7 @@ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/")
 find_package(catkin REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(Boost REQUIRED system serialization filesystem thread)
-find_package(TBB REQUIRED)
+find_package(TBB REQUIRED CONFIG)
 add_compile_options(-std=c++17)
 
 catkin_package(
@@ -26,7 +26,7 @@ add_definitions(${EIGEN3_DEFINITIONS})
 
 include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${TBB_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
 add_library(kartoSlamToolbox SHARED src/Karto.cpp src/Mapper.cpp)
-target_link_libraries(kartoSlamToolbox ${Boost_LIBRARIES} ${TBB_LIBRARIES})
+target_link_libraries(kartoSlamToolbox ${Boost_LIBRARIES} TBB::tbb)
 
 install(DIRECTORY include/ DESTINATION include)
 install(TARGETS kartoSlamToolbox
diff --git a/lib/karto_sdk/include/karto_sdk/Karto.h b/lib/karto_sdk/include/karto_sdk/Karto.h
index c4c4f27..5373b46 100644
--- a/lib/karto_sdk/include/karto_sdk/Karto.h
+++ b/lib/karto_sdk/include/karto_sdk/Karto.h
@@ -28,6 +28,7 @@
 #include <iomanip>
 #include <sstream>
 #include <stdexcept>
+#include <mutex>
 #include <shared_mutex>
 
 #include <math.h>
diff --git a/lib/karto_sdk/include/karto_sdk/Mapper.h b/lib/karto_sdk/include/karto_sdk/Mapper.h
index 2a439c6..94164f3 100644
--- a/lib/karto_sdk/include/karto_sdk/Mapper.h
+++ b/lib/karto_sdk/include/karto_sdk/Mapper.h
@@ -26,7 +26,7 @@
 #include <Eigen/Core>
 
 #include "tbb/parallel_for.h"
-#include "tbb/parallel_do.h"
+#include "tbb/parallel_for_each.h"
 #include "tbb/blocked_range.h"
 #include <algorithm>
 #include <chrono>
diff --git a/lib/karto_sdk/src/Mapper.cpp b/lib/karto_sdk/src/Mapper.cpp
index 7466216..97384d7 100644
--- a/lib/karto_sdk/src/Mapper.cpp
+++ b/lib/karto_sdk/src/Mapper.cpp
@@ -776,7 +776,7 @@ namespace karto
     m_nAngles = nAngles;
     m_searchAngleResolution = searchAngleResolution;
     m_doPenalize = doPenalize;
-    tbb::parallel_do(m_yPoses, (*this) );
+    tbb::parallel_for_each(m_yPoses, (*this) );
 
     // find value of best response (in [0; 1])
     kt_double bestResponse = -1;
diff --git a/solvers/ceres_solver.cpp b/solvers/ceres_solver.cpp
index 99a03a7..88da9f6 100644
--- a/solvers/ceres_solver.cpp
+++ b/solvers/ceres_solver.cpp
@@ -341,9 +341,9 @@ void CeresSolver::AddConstraint(karto::Edge<karto::LocalizedRangeScan>* pEdge)
    cost_function, loss_function_, 
    &node1it->second(0), &node1it->second(1), &node1it->second(2),
    &node2it->second(0), &node2it->second(1), &node2it->second(2));
-  problem_->SetParameterization(&node1it->second(2),
+  problem_->SetManifold(&node1it->second(2),
     angle_local_parameterization_);
-  problem_->SetParameterization(&node2it->second(2),
+  problem_->SetManifold(&node2it->second(2),
     angle_local_parameterization_);
 
   blocks_->insert(std::pair<std::size_t, ceres::ResidualBlockId>(
diff --git a/solvers/ceres_solver.hpp b/solvers/ceres_solver.hpp
index 9c0f060..edcd2a2 100644
--- a/solvers/ceres_solver.hpp
+++ b/solvers/ceres_solver.hpp
@@ -15,7 +15,7 @@
 
 #include <karto_sdk/Mapper.h>
 #include <ceres/ceres.h>
-#include <ceres/local_parameterization.h>
+#include <ceres/manifold.h>
 #include <cmath>
 #include <math.h>
 
@@ -57,7 +57,7 @@ private:
   ceres::Problem::Options options_problem_;
   ceres::LossFunction* loss_function_;
   ceres::Problem* problem_;
-  ceres::LocalParameterization* angle_local_parameterization_;
+  ceres::Manifold* angle_local_parameterization_;
   bool was_constant_set_, debug_logging_;
 
   // graph
diff --git a/solvers/ceres_utils.h b/solvers/ceres_utils.h
index 08b5f62..021d1b1 100644
--- a/solvers/ceres_utils.h
+++ b/solvers/ceres_utils.h
@@ -4,7 +4,7 @@
  */
 
 #include <ceres/ceres.h>
-#include <ceres/local_parameterization.h>
+#include <ceres/manifold.h>
 #include <cmath>
 #include <utility>
 
@@ -34,6 +34,24 @@ template <typename T> inline T NormalizeAngle(const T& angle_radians)
 class AngleLocalParameterization
 {
  public:
+  template <typename T>
+  bool Plus(const T* x_radians,
+            const T* delta_radians,
+            T* x_plus_delta_radians) const {
+    *x_plus_delta_radians = NormalizeAngle(*x_radians + *delta_radians);
+    return true;
+  }
+
+  template <typename T>
+  bool Minus(const T* y_radians,
+             const T* x_radians,
+             T* y_minus_x_radians) const {
+    *y_minus_x_radians =
+        NormalizeAngle(*y_radians) - NormalizeAngle(*x_radians);
+
+    return true;
+  }
+
   template <typename T>
   bool operator()(const T* theta_radians, const T* delta_theta_radians, T* theta_radians_plus_delta) const
   {
@@ -41,9 +59,9 @@ class AngleLocalParameterization
     return true;
   }
 
-  static ceres::LocalParameterization* Create()
+  static ceres::Manifold* Create()
   {
-    return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization, 1, 1>);
+    return (new ceres::AutoDiffManifold<AngleLocalParameterization, 1, 1>);
   }
 };