humble support

.github/workflows/main.yml

@@ -1,15 +1,15 @@
-name: foxy
+name: build
 
-on: [push, pull_request]
+on: pull_request
 
 jobs:
-    job1:
-        name: Build
-        runs-on: ubuntu-20.04
+    job:
+        name: Humble-Build
+        runs-on: ubuntu-22.04
         steps:
             - name: ROS2 Install
               run: |
-                  # Ref: https://index.ros.org/doc/ros2/Installation/Foxy/Linux-Install-Debians/
+                  # Ref: https://index.ros.org/doc/ros2/Installation/Humble/Linux-Install-Debians/
                   sudo apt update && sudo apt install locales
                   sudo locale-gen en_US en_US.UTF-8
                   sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
@@ -18,8 +18,8 @@ jobs:
                   curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
                   sudo sh -c 'echo "deb [arch=amd64,arm64] http://packages.ros.org/ros2/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list'
                   sudo apt update
-                  sudo apt install ros-foxy-desktop
-                  source /opt/ros/foxy/setup.bash
+                  sudo apt install ros-humble-desktop
+                  source /opt/ros/humble/setup.bash
             - uses: actions/checkout@v1
               with:
                 submodules: true
@@ -29,19 +29,19 @@ jobs:
                   cp -rf . ~/ros2_ws/src/li_slam_ros2
             - name: Install gtsam
               run: |
-                  sudo add-apt-repository ppa:borglab/gtsam-release-4.0
+                  sudo add-apt-repository ppa:borglab/gtsam-release-4.1
                   sudo apt update
                   sudo apt install libgtsam-dev libgtsam-unstable-dev
             - name: Install dependencies
               run: |
-                  source /opt/ros/foxy/setup.bash
+                  source /opt/ros/humble/setup.bash
                   sudo apt install -y python3-rosdep2
                   rosdep update
                   cd ~/ros2_ws/src
-                  rosdep install -r -y --from-paths . --ignore-src       
+                  rosdep install -r -y --from-paths . --ignore-src
             - name: Build packages
               run: |
-                  source /opt/ros/foxy/setup.bash
+                  source /opt/ros/humble/setup.bash
                   # Install colcon
                   # Ref: https://index.ros.org/doc/ros2/Tutorials/Colcon-Tutorial/
                   sudo apt install python3-colcon-common-extensions

README.md

@@ -40,13 +40,15 @@ git clone --recursive https://github.com/rsasaki0109/li_slam_ros2
 ```
 gtsam install
 ```
-sudo add-apt-repository ppa:borglab/gtsam-release-4.0
+sudo add-apt-repository ppa:borglab/gtsam-release-4.1
 sudo apt update
 sudo apt install libgtsam-dev libgtsam-unstable-dev
 ```
 build
 ```
 cd ~/ros2_ws
+rosdep update
+rosdep install --from-paths src --ignore-src -yr
 colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
 ```
 

Thirdparty/ndt_omp_ros2/CMakeLists.txt

@@ -18,8 +18,9 @@ set(CMAKE_BUILD_TYPE "RELEASE")
 #)
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
 
-find_package(PCL REQUIRED)
+find_package(PCL 1.12 REQUIRED)
 include_directories(${PCL_INCLUDE_DIRS})
 link_directories(${PCL_LIBRARY_DIRS})
 add_definitions(${PCL_DEFINITIONS})
@@ -68,6 +69,7 @@ target_link_libraries(align
 
 ament_target_dependencies(align
   rclcpp
+  std_msgs
 )
 
 ############

Thirdparty/ndt_omp_ros2/apps/align.cpp

@@ -15,7 +15,7 @@
 #include <std_msgs/msg/string.hpp>
 
 // align point clouds and measure processing time
-pcl::PointCloud<pcl::PointXYZ>::Ptr align(pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>::Ptr registration, const pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud, const pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud ) {
+pcl::PointCloud<pcl::PointXYZ>::Ptr align(boost::shared_ptr<pcl::Registration<pcl::PointXYZ, pcl::PointXYZ>> registration, const pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud, const pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud ) {
   registration->setInputTarget(target_cloud);
   registration->setInputSource(source_cloud);
   pcl::PointCloud<pcl::PointXYZ>::Ptr aligned(new pcl::PointCloud<pcl::PointXYZ>());
@@ -78,17 +78,17 @@ int main(int argc, char** argv) {
 
   // benchmark
   std::cout << "--- pcl::GICP ---" << std::endl;
-  pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>::Ptr gicp(new pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>());
+  boost::shared_ptr<pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>> gicp(new pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>());
   pcl::PointCloud<pcl::PointXYZ>::Ptr aligned = align(gicp, target_cloud, source_cloud);
 
   //TODO:The problem of uninitialized member variables
   std::cout << "--- pclomp::GICP ---" << std::endl;
-  pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>::Ptr gicp_omp(new pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>());
+  boost::shared_ptr<pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>> gicp_omp(new pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>());
   aligned = align(gicp_omp, target_cloud, source_cloud);
 
 
   std::cout << "--- pcl::NDT ---" << std::endl;
-  pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+  boost::shared_ptr<pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>> ndt(new pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
   ndt->setResolution(1.0);
   aligned = align(ndt, target_cloud, source_cloud);
 

Thirdparty/ndt_omp_ros2/include/pclomp/gicp_omp.h

@@ -43,6 +43,7 @@
 
 #include <pcl/registration/icp.h>
 #include <pcl/registration/bfgs.h>
+#include <boost/shared_ptr.hpp>
 
 namespace pclomp
 {

Thirdparty/ndt_omp_ros2/include/pclomp/gicp_omp_impl.hpp

@@ -41,7 +41,7 @@
 #define PCL_REGISTRATION_IMPL_GICP_OMP_HPP_
 
 #include <atomic>
-#include <pcl/registration/boost.h>
+#include <boost/shared_ptr.hpp>
 #include <pcl/registration/exceptions.h>
 
 ////////////////////////////////////////////////////////////////////////////////////////
@@ -206,7 +206,6 @@ pclomp::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigi
   tmp_idx_tgt_ = &indices_tgt;
 
   // Optimize using forward-difference approximation LM
-  const double gradient_tol = 1e-2;
   OptimizationFunctorWithIndices functor(this);
   BFGS<OptimizationFunctorWithIndices> bfgs (functor);
   bfgs.parameters.sigma = 0.01;
@@ -227,7 +226,7 @@ pclomp::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigi
     {
       break;
     }
-    result = bfgs.testGradient(gradient_tol);
+    result = bfgs.testGradient();
   } while(result == BFGSSpace::Running && inner_iterations_ < max_inner_iterations_);
   if(result == BFGSSpace::NoProgress || result == BFGSSpace::Success || inner_iterations_ == max_inner_iterations_)
   {

Thirdparty/ndt_omp_ros2/include/pclomp/ndt_omp_impl.hpp

@@ -187,10 +187,10 @@ pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeDerivativ
 	hessian.setZero();
 	double score = 0;
 
-  std::vector<double> scores(input_->points.size());
-  std::vector<Eigen::Matrix<double, 6, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 1>>> score_gradients(input_->points.size());
-  std::vector<Eigen::Matrix<double, 6, 6>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 6>>> hessians(input_->points.size());
-  for (int i = 0; i < input_->points.size(); i++) {
+  std::vector<double> scores(num_threads_);
+  std::vector<Eigen::Matrix<double, 6, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 1>>> score_gradients(num_threads_);
+  std::vector<Eigen::Matrix<double, 6, 6>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 6>>> hessians(num_threads_);
+  for (int i = 0; i < num_threads_; i++) {
 		scores[i] = 0;
 		score_gradients[i].setZero();
 		hessians[i].setZero();
@@ -269,12 +269,12 @@ pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeDerivativ
 			score_pt += updateDerivatives(score_gradient_pt, hessian_pt, point_gradient_, point_hessian_, x_trans, c_inv, compute_hessian);
 		}
 
-		scores[idx] = score_pt;
-		score_gradients[idx].noalias() = score_gradient_pt;
-		hessians[idx].noalias() = hessian_pt;
+		scores[thread_n] += score_pt;
+		score_gradients[thread_n].noalias() += score_gradient_pt;
+		hessians[thread_n].noalias() += hessian_pt;
 	}
 
-  for (int i = 0; i < input_->points.size(); i++) {
+  for (int i = 0; i < num_threads_; i++) {
 		score += scores[i];
 		score_gradient += score_gradients[i];
 		hessian += hessians[i];
@@ -356,7 +356,7 @@ pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeAngleDeri
 		h_ang_c2_ << (-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0;
 		h_ang_c3_ << (cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0;
 
-		h_ang_d1_ << (-cy * cz), (cy * sz), (sy);
+		h_ang_d1_ << (-cy * cz), (cy * sz), (-sy);
 		h_ang_d2_ << (-sx * sy * cz), (sx * sy * sz), (sx * cy);
 		h_ang_d3_ << (cx * sy * cz), (-cx * sy * sz), (-cx * cy);
 
@@ -567,21 +567,7 @@ pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeHessian (
     // Find nieghbors (Radius search has been experimentally faster than direct neighbor checking.
     std::vector<TargetGridLeafConstPtr> neighborhood;
     std::vector<float> distances;
-		switch (search_method) {
-		case KDTREE:
-			target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances);
-			break;
-		case DIRECT26:
-			target_cells_.getNeighborhoodAtPoint(x_trans_pt, neighborhood);
-			break;
-		default:
-		case DIRECT7:
-			target_cells_.getNeighborhoodAtPoint7(x_trans_pt, neighborhood);
-			break;
-		case DIRECT1:
-			target_cells_.getNeighborhoodAtPoint1(x_trans_pt, neighborhood);
-			break;
-		}
+    target_cells_.radiusSearch (x_trans_pt, resolution_, neighborhood, distances);
 
     for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); neighborhood_it++)
     {
@@ -609,10 +595,10 @@ pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeHessian (
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointSource, typename PointTarget> void
-pclomp::NormalDistributionsTransform<PointSource, PointTarget>::updateHessian (Eigen::Matrix<double, 6, 6> &hessian,
-	const Eigen::Matrix<double, 3, 6> &point_gradient_,
-	const Eigen::Matrix<double, 18, 6> &point_hessian_,
-	const Eigen::Vector3d &x_trans,
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::updateHessian (Eigen::Matrix<double, 6, 6> &hessian, 
+	const Eigen::Matrix<double, 3, 6> &point_gradient_, 
+	const Eigen::Matrix<double, 18, 6> &point_hessian_, 
+	const Eigen::Vector3d &x_trans, 
 	const Eigen::Matrix3d &c_inv) const
 {
   Eigen::Vector3d cov_dxd_pi;
@@ -814,7 +800,7 @@ pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeStepLengt
   double g_u = auxilaryFunction_dPsiMT (d_phi_0, d_phi_0, mu);
 
   // Check used to allow More-Thuente step length calculation to be skipped by making step_min == step_max
-  bool interval_converged = (step_max - step_min) < 0, open_interval = true;
+  bool interval_converged = (step_max - step_min) > 0, open_interval = true;
 
   double a_t = step_init;
   a_t = std::min (a_t, step_max);
@@ -942,21 +928,7 @@ double pclomp::NormalDistributionsTransform<PointSource, PointTarget>::calculate
 		// Find nieghbors (Radius search has been experimentally faster than direct neighbor checking.
 		std::vector<TargetGridLeafConstPtr> neighborhood;
 		std::vector<float> distances;
-		switch (search_method) {
-		case KDTREE:
-			target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances);
-			break;
-		case DIRECT26:
-			target_cells_.getNeighborhoodAtPoint(x_trans_pt, neighborhood);
-			break;
-		default:
-		case DIRECT7:
-			target_cells_.getNeighborhoodAtPoint7(x_trans_pt, neighborhood);
-			break;
-		case DIRECT1:
-			target_cells_.getNeighborhoodAtPoint1(x_trans_pt, neighborhood);
-			break;
-		}
+		target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances);
 
 		for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin(); neighborhood_it != neighborhood.end(); neighborhood_it++)
 		{

Thirdparty/ndt_omp_ros2/include/pclomp/voxel_grid_covariance_omp.h

@@ -38,7 +38,7 @@
 #ifndef PCL_VOXEL_GRID_COVARIANCE_OMP_H_
 #define PCL_VOXEL_GRID_COVARIANCE_OMP_H_
 
-#include <pcl/filters/boost.h>
+#include <boost/shared_ptr.hpp>
 #include <pcl/filters/voxel_grid.h>
 #include <map>
 #include <unordered_map>

graph_based_slam/CMakeLists.txt

@@ -17,6 +17,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 SET(CMAKE_CXX_FLAGS "-O2 -g ${CMAKE_CXX_FLAGS}")
+add_compile_definitions(G2O_USE_VENDORED_CERES)
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
@@ -27,6 +28,10 @@ find_package(sensor_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(tf2_ros REQUIRED)
+find_package(tf2_eigen REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_sensor_msgs REQUIRED)
+find_package(pcl_conversions REQUIRED)
 find_package(lidarslam_msgs REQUIRED)
 find_package(ndt_omp_ros2 REQUIRED)
 find_package(PCL REQUIRED)
@@ -55,10 +60,14 @@ src/graph_based_slam_component.cpp
 target_compile_definitions(graph_based_slam_component PRIVATE "GS_GBS_BUILDING_DLL")
 
 ament_target_dependencies(graph_based_slam_component
-  rclcpp 
-  rclcpp_components 
-  tf2_ros 
-  geometry_msgs 
+  rclcpp
+  rclcpp_components
+  tf2_ros
+  tf2_eigen
+  tf2_geometry_msgs
+  tf2_sensor_msgs
+  pcl_conversions
+  geometry_msgs
   sensor_msgs
   nav_msgs
   std_srvs

graph_based_slam/include/graph_based_slam/graph_based_slam_component.h

@@ -44,9 +44,9 @@ extern "C" {
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_ros/transform_broadcaster.h>
-#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <tf2_eigen/tf2_eigen.h>
+#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2_eigen/tf2_eigen.hpp>
 
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <geometry_msgs/msg/point.hpp>
@@ -103,7 +103,7 @@ namespace graphslam
     tf2_ros::TransformListener listener_;
     tf2_ros::TransformBroadcaster broadcaster_;
 
-    pcl::Registration < pcl::PointXYZI, pcl::PointXYZI > ::Ptr registration_;
+    boost::shared_ptr<pcl::Registration < pcl::PointXYZI, pcl::PointXYZI >> registration_;
     pcl::VoxelGrid < pcl::PointXYZI > voxelgrid_;
 
     lidarslam_msgs::msg::MapArray map_array_msg_;

graph_based_slam/package.xml

@@ -20,7 +20,7 @@
   <depend>pcl_conversions</depend>
   <depend>lidarslam_msgs</depend>
   <depend>ndt_omp_ros2</depend>
-  <depend>g2o</depend>
+  <depend>libg2o</depend>
   <depend>libpcl-all-dev</depend>
 
   <test_depend>ament_lint_auto</test_depend>

graph_based_slam/src/graph_based_slam_component.cpp

@@ -58,7 +58,7 @@ GraphBasedSlamComponent::GraphBasedSlamComponent(const rclcpp::NodeOptions & opt
   voxelgrid_.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);
 
   if (registration_method == "NDT") {
-    pclomp::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI>::Ptr
+    boost::shared_ptr<pclomp::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI>>
       ndt(new pclomp::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI>());
     ndt->setMaximumIterations(100);
     ndt->setResolution(ndt_resolution);
@@ -68,7 +68,7 @@ GraphBasedSlamComponent::GraphBasedSlamComponent(const rclcpp::NodeOptions & opt
     if (ndt_num_threads > 0) {ndt->setNumThreads(ndt_num_threads);}
     registration_ = ndt;
   } else {
-    pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI>::Ptr
+    boost::shared_ptr<pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI>>
       gicp(new pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI>());
     gicp->setMaxCorrespondenceDistance(30);
     gicp->setMaximumIterations(100);