first commit

.clang-format

@@ -0,0 +1,117 @@
+---
+Language:        Cpp
+# BasedOnStyle:  Google
+AccessModifierOffset: -2
+AlignAfterOpenBracket: Align
+AlignConsecutiveAssignments: false
+AlignConsecutiveDeclarations: false
+AlignEscapedNewlines: Left
+AlignOperands:   true
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: true
+AllowShortBlocksOnASingleLine: false
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: Empty
+AllowShortIfStatementsOnASingleLine: true
+AllowShortLoopsOnASingleLine: true
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: true
+AlwaysBreakTemplateDeclarations: true
+BinPackArguments: true
+BinPackParameters: false
+BraceWrapping:   
+  AfterClass:      false
+  AfterControlStatement: false
+  AfterEnum:       false
+  AfterFunction:   false
+  AfterNamespace:  false
+  AfterObjCDeclaration: false
+  AfterStruct:     false
+  AfterUnion:      false
+  AfterExternBlock: false
+  BeforeCatch:     false
+  BeforeElse:      false
+  IndentBraces:    false
+  SplitEmptyFunction: true
+  SplitEmptyRecord: true
+  SplitEmptyNamespace: true
+BreakBeforeBinaryOperators: None
+BreakBeforeBraces: Attach
+BreakBeforeInheritanceComma: false
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializersBeforeComma: false
+BreakConstructorInitializers: BeforeColon
+BreakAfterJavaFieldAnnotations: false
+BreakStringLiterals: false
+ColumnLimit:     256
+CommentPragmas:  '^ IWYU pragma:'
+CompactNamespaces: false
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+ConstructorInitializerIndentWidth: 4
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+DerivePointerAlignment: true
+DisableFormat:   false
+ExperimentalAutoDetectBinPacking: false
+FixNamespaceComments: true
+ForEachMacros:   
+  - foreach
+  - Q_FOREACH
+  - BOOST_FOREACH
+IncludeBlocks:   Preserve
+IncludeCategories: 
+  - Regex:           '^<ext/.*\.h>'
+    Priority:        2
+  - Regex:           '^<.*\.h>'
+    Priority:        1
+  - Regex:           '^<.*'
+    Priority:        2
+  - Regex:           '.*'
+    Priority:        3
+IncludeIsMainRegex: '([-_](test|unittest))?$'
+IndentCaseLabels: true
+IndentPPDirectives: None
+IndentWidth:     2
+IndentWrappedFunctionNames: false
+JavaScriptQuotes: Leave
+JavaScriptWrapImports: true
+KeepEmptyLinesAtTheStartOfBlocks: false
+MacroBlockBegin: ''
+MacroBlockEnd:   ''
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCBlockIndentWidth: 2
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: false
+PenaltyBreakAssignment: 2
+PenaltyBreakBeforeFirstCallParameter: 1
+PenaltyBreakComment: 300
+PenaltyBreakFirstLessLess: 120
+PenaltyBreakString: 1000
+PenaltyExcessCharacter: 0
+PenaltyReturnTypeOnItsOwnLine: 200
+PointerAlignment: Left
+RawStringFormats: 
+  - Delimiter:       pb
+    Language:        TextProto
+    BasedOnStyle:    google
+ReflowComments:  true
+SortIncludes:    false
+SortUsingDeclarations: false
+SpaceAfterCStyleCast: false
+SpaceAfterTemplateKeyword: false
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeParens: Never
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 2
+SpacesInAngles:  false
+SpacesInContainerLiterals: true
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+Standard:        Auto
+TabWidth:        8
+UseTab:          Never
+...
+

.github/workflows/build.yml

@@ -0,0 +1,34 @@
+name: Build
+
+on:
+  push:
+    branches: [ master ]
+  pull_request:
+    branches: [ master ]
+
+  # Allows you to run this workflow manually from the Actions tab
+  workflow_dispatch:
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    strategy:
+      matrix:
+        ROS_DISTRO: [melodic, melodic_llvm, noetic, noetic_llvm]
+
+    steps:
+      - uses: actions/checkout@v2
+
+      - name: Docker login
+        continue-on-error: true
+        uses: docker/login-action@v1
+        with:
+          username: ${{ secrets.DOCKER_USERNAME }}
+          password: ${{ secrets.DOCKER_TOKEN }}
+
+      - name: Docker build
+        uses: docker/build-push-action@v2
+        with:
+          file: ${{github.workspace}}/docker/${{ matrix.ROS_DISTRO }}/Dockerfile
+          context: .
+          push: false

.gitignore

@@ -0,0 +1,3 @@
+.vscode/*
+imgui.ini
+rviz/*

CMakeLists.txt

@@ -0,0 +1,183 @@
+# SPDX-License-Identifier: BSD-2-Clause
+cmake_minimum_required(VERSION 2.8.3)
+project(hdl_graph_slam)
+
+# Can we use C++17 in indigo?
+if(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64")
+  add_definitions(-std=c++11)
+  set(CMAKE_CXX_FLAGS "-std=c++11")
+else()
+  add_definitions(-std=c++14 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2)
+  set(CMAKE_CXX_FLAGS "-std=c++14 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
+endif()
+
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake")
+
+# pcl 1.7 causes a segfault when it is built with debug mode
+set(CMAKE_BUILD_TYPE "RELEASE")
+
+find_package(catkin REQUIRED COMPONENTS
+  fast_gicp
+  geodesy
+  geometry_msgs
+  interactive_markers
+  message_generation
+  ndt_omp
+  nmea_msgs
+  pcl_ros
+  roscpp
+  rospy
+  sensor_msgs
+  std_msgs
+  tf_conversions
+  laser_geometry  # 加上这个
+)
+catkin_python_setup()
+
+find_package(PCL REQUIRED)
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+
+message(STATUS "PCL_INCLUDE_DIRS:" ${PCL_INCLUDE_DIRS})
+message(STATUS "PCL_LIBRARY_DIRS:" ${PCL_LIBRARY_DIRS})
+message(STATUS "PCL_DEFINITIONS:" ${PCL_DEFINITIONS})
+
+find_package(G2O REQUIRED)
+include_directories(SYSTEM ${G2O_INCLUDE_DIR} ${G2O_INCLUDE_DIRS})
+link_directories(${G2O_LIBRARY_DIRS})
+# link_libraries(${G2O_LIBRARIES})
+
+find_package(OpenMP)
+if (OPENMP_FOUND)
+    set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+    set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+endif()
+
+find_library(VGICP_CUDA_FOUND NAMES fast_vgicp_cuda)
+message(STATUS "VGICP_CUDA_FOUND:" ${VGICP_CUDA_FOUND})
+if(VGICP_CUDA_FOUND)
+  add_definitions(-DUSE_VGICP_CUDA)
+endif()
+
+########################
+## message generation ##
+########################
+add_message_files(FILES
+  FloorCoeffs.msg
+  ScanMatchingStatus.msg
+)
+
+add_service_files(FILES
+  SaveMap.srv
+  LoadGraph.srv
+  DumpGraph.srv
+)
+
+generate_messages(DEPENDENCIES std_msgs geometry_msgs)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES hdl_graph_slam_nodelet
+  CATKIN_DEPENDS
+    geometry_msgs
+    message_runtime
+    nmea_msgs
+    roscpp
+    sensor_msgs
+    std_msgs
+    tf_conversions
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+include_directories(include)
+include_directories(
+  ${PCL_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+)
+
+# nodelets
+add_executable(laser_odom src/laser_odom.cpp)
+# add_library(laser_odom src/laser_odom.cpp)
+target_link_libraries(laser_odom
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+)
+
+add_library(prefiltering_nodelet apps/prefiltering_nodelet.cpp)
+target_link_libraries(prefiltering_nodelet
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+)
+
+
+add_library(floor_detection_nodelet apps/floor_detection_nodelet.cpp)
+target_link_libraries(floor_detection_nodelet
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+)
+add_dependencies(floor_detection_nodelet ${PROJECT_NAME}_gencpp)
+
+
+add_library(scan_matching_odometry_nodelet
+  apps/scan_matching_odometry_nodelet.cpp
+  src/hdl_graph_slam/registrations.cpp
+)
+target_link_libraries(scan_matching_odometry_nodelet
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+)
+add_dependencies(scan_matching_odometry_nodelet ${PROJECT_NAME}_gencpp)
+
+
+add_library(hdl_graph_slam_nodelet
+  apps/hdl_graph_slam_nodelet.cpp
+  src/hdl_graph_slam/graph_slam.cpp
+  src/hdl_graph_slam/keyframe.cpp
+  src/hdl_graph_slam/map_cloud_generator.cpp
+  src/hdl_graph_slam/registrations.cpp
+  src/hdl_graph_slam/information_matrix_calculator.cpp
+  src/g2o/robust_kernel_io.cpp
+)
+target_link_libraries(hdl_graph_slam_nodelet
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+  ${G2O_TYPES_DATA}
+  ${G2O_CORE_LIBRARY}
+  ${G2O_STUFF_LIBRARY}
+  ${G2O_SOLVER_PCG}
+  ${G2O_SOLVER_CSPARSE}   # be aware of that CSPARSE is released under LGPL
+  ${G2O_SOLVER_CHOLMOD}   # be aware of that cholmod is released under GPL
+  ${G2O_TYPES_SLAM3D}
+  ${G2O_TYPES_SLAM3D_ADDONS}
+)
+add_dependencies(hdl_graph_slam_nodelet ${PROJECT_NAME}_gencpp)
+
+catkin_install_python(
+  PROGRAMS
+    src/${PROJECT_NAME}/bag_player.py
+    src/${PROJECT_NAME}/ford2bag.py
+    src/${PROJECT_NAME}/map2odom_publisher.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(FILES nodelet_plugins.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(TARGETS
+  prefiltering_nodelet
+  floor_detection_nodelet
+  scan_matching_odometry_nodelet
+  hdl_graph_slam_nodelet
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+
+install(DIRECTORY include/
+   DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
+)

LICENSE

@@ -0,0 +1,25 @@
+BSD 2-Clause License
+
+Copyright (c) 2019, k.koide
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

apps/floor_detection_nodelet.cpp

@@ -0,0 +1,269 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#include <memory>
+#include <iostream>
+
+#include <boost/optional.hpp>
+
+#include <ros/ros.h>
+#include <ros/time.h>
+#include <pcl_ros/point_cloud.h>
+
+#include <std_msgs/Time.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <hdl_graph_slam/FloorCoeffs.h>
+
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+
+#include <pcl/common/transforms.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/search/impl/search.hpp>
+#include <pcl/filters/impl/plane_clipper3D.hpp>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/sample_consensus/ransac.h>
+#include <pcl/sample_consensus/sac_model_plane.h>
+
+namespace hdl_graph_slam {
+
+class FloorDetectionNodelet : public nodelet::Nodelet {
+public:
+  typedef pcl::PointXYZI PointT;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  FloorDetectionNodelet() {}
+  virtual ~FloorDetectionNodelet() {}
+
+  virtual void onInit() {
+    NODELET_DEBUG("initializing floor_detection_nodelet...");
+    nh = getNodeHandle();
+    private_nh = getPrivateNodeHandle();
+
+    initialize_params();
+
+    points_sub = nh.subscribe("/filtered_points", 256, &FloorDetectionNodelet::cloud_callback, this);
+    floor_pub = nh.advertise<hdl_graph_slam::FloorCoeffs>("/floor_detection/floor_coeffs", 32);
+
+    read_until_pub = nh.advertise<std_msgs::Header>("/floor_detection/read_until", 32);
+    floor_filtered_pub = nh.advertise<sensor_msgs::PointCloud2>("/floor_detection/floor_filtered_points", 32);
+    floor_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/floor_detection/floor_points", 32);
+  }
+
+private:
+  /**
+   * @brief initialize parameters
+   */
+  void initialize_params() {
+    tilt_deg = private_nh.param<double>("tilt_deg", 0.0);                           // approximate sensor tilt angle [deg]
+    sensor_height = private_nh.param<double>("sensor_height", 2.0);                 // approximate sensor height [m]
+    height_clip_range = private_nh.param<double>("height_clip_range", 1.0);         // points with heights in [sensor_height - height_clip_range, sensor_height + height_clip_range] will be used for floor detection
+    floor_pts_thresh = private_nh.param<int>("floor_pts_thresh", 512);              // minimum number of support points of RANSAC to accept a detected floor plane
+    floor_normal_thresh = private_nh.param<double>("floor_normal_thresh", 10.0);    // verticality check thresold for the detected floor plane [deg]
+    use_normal_filtering = private_nh.param<bool>("use_normal_filtering", true);    // if true, points with "non-"vertical normals will be filtered before RANSAC
+    normal_filter_thresh = private_nh.param<double>("normal_filter_thresh", 20.0);  // "non-"verticality check threshold [deg]
+
+    points_topic = private_nh.param<std::string>("points_topic", "/velodyne_points");
+  }
+
+  /**
+   * @brief callback for point clouds
+   * @param cloud_msg  point cloud msg
+   */
+  void cloud_callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {  // 处理点云数据的回调函数
+    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
+    pcl::fromROSMsg(*cloud_msg, *cloud);
+
+    if(cloud->empty()) {
+      return;
+    }
+
+    // floor detection
+    boost::optional<Eigen::Vector4f> floor = detect(cloud);
+
+    // publish the detected floor coefficients
+    hdl_graph_slam::FloorCoeffs coeffs;
+    coeffs.header = cloud_msg->header;
+    if(floor) {
+      coeffs.coeffs.resize(4);
+      for(int i = 0; i < 4; i++) {
+        coeffs.coeffs[i] = (*floor)[i];
+      }
+    }
+
+    floor_pub.publish(coeffs);
+
+    // for offline estimation
+    std_msgs::HeaderPtr read_until(new std_msgs::Header());
+    read_until->frame_id = points_topic;
+    read_until->stamp = cloud_msg->header.stamp + ros::Duration(1, 0);
+    read_until_pub.publish(read_until);
+
+    read_until->frame_id = "/filtered_points";
+    read_until_pub.publish(read_until);
+  }
+
+  /**
+   * @brief detect the floor plane from a point cloud
+   * @param cloud  input cloud
+   * @return detected floor plane coefficients
+   */
+  boost::optional<Eigen::Vector4f> detect(const pcl::PointCloud<PointT>::Ptr& cloud) const {  // 主要的检测函数，用 RANSAC 算法你和一个平面，并进行垂直性检查
+    // compensate the tilt rotation
+    Eigen::Matrix4f tilt_matrix = Eigen::Matrix4f::Identity();
+    tilt_matrix.topLeftCorner(3, 3) = Eigen::AngleAxisf(tilt_deg * M_PI / 180.0f, Eigen::Vector3f::UnitY()).toRotationMatrix();
+
+    // filtering before RANSAC (height and normal filtering)
+    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>);
+    pcl::transformPointCloud(*cloud, *filtered, tilt_matrix);
+    filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height + height_clip_range), false);
+    filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height - height_clip_range), true);
+
+    if(use_normal_filtering) {
+      filtered = normal_filtering(filtered);
+    }
+
+    pcl::transformPointCloud(*filtered, *filtered, static_cast<Eigen::Matrix4f>(tilt_matrix.inverse()));
+
+    if(floor_filtered_pub.getNumSubscribers()) {
+      filtered->header = cloud->header;
+      floor_filtered_pub.publish(*filtered);
+    }
+
+    // too few points for RANSAC
+    if(filtered->size() < floor_pts_thresh) {
+      return boost::none;
+    }
+
+    // RANSAC
+    pcl::SampleConsensusModelPlane<PointT>::Ptr model_p(new pcl::SampleConsensusModelPlane<PointT>(filtered));
+    pcl::RandomSampleConsensus<PointT> ransac(model_p);
+    ransac.setDistanceThreshold(0.1);
+    ransac.computeModel();
+
+    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
+    ransac.getInliers(inliers->indices);
+
+    // too few inliers
+    if(inliers->indices.size() < floor_pts_thresh) {
+      return boost::none;
+    }
+
+    // verticality check of the detected floor's normal
+    Eigen::Vector4f reference = tilt_matrix.inverse() * Eigen::Vector4f::UnitZ();
+
+    Eigen::VectorXf coeffs;
+    ransac.getModelCoefficients(coeffs);
+
+    double dot = coeffs.head<3>().dot(reference.head<3>());
+    if(std::abs(dot) < std::cos(floor_normal_thresh * M_PI / 180.0)) {
+      // the normal is not vertical
+      return boost::none;
+    }
+
+    // make the normal upward
+    if(coeffs.head<3>().dot(Eigen::Vector3f::UnitZ()) < 0.0f) {
+      coeffs *= -1.0f;
+    }
+
+    if(floor_points_pub.getNumSubscribers()) {
+      pcl::PointCloud<PointT>::Ptr inlier_cloud(new pcl::PointCloud<PointT>);
+      pcl::ExtractIndices<PointT> extract;
+      extract.setInputCloud(filtered);
+      extract.setIndices(inliers);
+      extract.filter(*inlier_cloud);
+      inlier_cloud->header = cloud->header;
+
+      floor_points_pub.publish(*inlier_cloud);
+    }
+
+    return Eigen::Vector4f(coeffs);
+  }
+
+  /**
+   * @brief plane_clip
+   * @param src_cloud
+   * @param plane
+   * @param negative
+   * @return
+   */
+  pcl::PointCloud<PointT>::Ptr plane_clip(const pcl::PointCloud<PointT>::Ptr& src_cloud, const Eigen::Vector4f& plane, bool negative) const {
+    pcl::PlaneClipper3D<PointT> clipper(plane);
+    pcl::PointIndices::Ptr indices(new pcl::PointIndices);
+
+    clipper.clipPointCloud3D(*src_cloud, indices->indices);
+
+    pcl::PointCloud<PointT>::Ptr dst_cloud(new pcl::PointCloud<PointT>);
+
+    pcl::ExtractIndices<PointT> extract;
+    extract.setInputCloud(src_cloud);
+    extract.setIndices(indices);
+    extract.setNegative(negative);
+    extract.filter(*dst_cloud);
+
+    return dst_cloud;
+  }
+
+  /**
+   * @brief filter points with non-vertical normals
+   * @param cloud  input cloud
+   * @return filtered cloud
+   */
+  pcl::PointCloud<PointT>::Ptr normal_filtering(const pcl::PointCloud<PointT>::Ptr& cloud) const {
+    pcl::NormalEstimation<PointT, pcl::Normal> ne;
+    ne.setInputCloud(cloud);
+
+    pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
+    ne.setSearchMethod(tree);
+
+    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
+    ne.setKSearch(10);
+    ne.setViewPoint(0.0f, 0.0f, sensor_height);
+    ne.compute(*normals);
+
+    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>);
+    filtered->reserve(cloud->size());
+
+    for(int i = 0; i < cloud->size(); i++) {
+      float dot = normals->at(i).getNormalVector3fMap().normalized().dot(Eigen::Vector3f::UnitZ());
+      if(std::abs(dot) > std::cos(normal_filter_thresh * M_PI / 180.0)) {
+        filtered->push_back(cloud->at(i));
+      }
+    }
+
+    filtered->width = filtered->size();
+    filtered->height = 1;
+    filtered->is_dense = false;
+
+    return filtered;
+  }
+
+private:
+  ros::NodeHandle nh;
+  ros::NodeHandle private_nh;
+
+  // ROS topics
+  ros::Subscriber points_sub;
+
+  ros::Publisher floor_pub;
+  ros::Publisher floor_points_pub;
+  ros::Publisher floor_filtered_pub;
+
+  std::string points_topic;
+  ros::Publisher read_until_pub;
+
+  // floor detection parameters
+  // see initialize_params() for the details
+  double tilt_deg;
+  double sensor_height;
+  double height_clip_range;
+
+  int floor_pts_thresh;
+  double floor_normal_thresh;
+
+  bool use_normal_filtering;
+  double normal_filter_thresh;
+};
+
+}  // namespace hdl_graph_slam
+
+PLUGINLIB_EXPORT_CLASS(hdl_graph_slam::FloorDetectionNodelet, nodelet::Nodelet)

apps/prefiltering_nodelet.cpp

@@ -0,0 +1,276 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+  代码作用：
+  对激光雷达（LIDAR）生成的点云数据进行预处理，以提高后续处理步骤的效率和准确性。
+ */
+
+#include <string>
+
+#include <ros/ros.h>
+#include <ros/time.h>
+#include <pcl_ros/transforms.h>
+#include <pcl_ros/point_cloud.h>
+#include <tf/transform_listener.h>
+
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/PointCloud2.h>
+
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/approximate_voxel_grid.h>
+#include <pcl/filters/radius_outlier_removal.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+
+namespace hdl_graph_slam {
+
+class PrefilteringNodelet : public nodelet::Nodelet {
+public:
+  typedef pcl::PointXYZI PointT;
+
+  PrefilteringNodelet() {}           // 构造函数
+  virtual ~PrefilteringNodelet() {}  // 析构函数
+
+  virtual void onInit() {                 // 初始化函数
+    nh = getNodeHandle();                 // 初始化节点句柄
+    private_nh = getPrivateNodeHandle();  // 初始化私有节点句柄
+
+    initialize_params();  // 参数初始化函数
+
+    if(private_nh.param<bool>("deskewing", false)) {  // 判断是否启用去畸变
+      imu_sub = nh.subscribe("/imu/data", 1, &PrefilteringNodelet::imu_callback, this);
+    }
+
+    points_sub = nh.subscribe("/velodyne_points", 64, &PrefilteringNodelet::cloud_callback, this);  // 订阅点云数据，队列大小是64，接收到消息时调用 cloud_callback 函数
+    points_pub = nh.advertise<sensor_msgs::PointCloud2>("/filtered_points", 32);                    // 发布处理后的点云数据
+    colored_pub = nh.advertise<sensor_msgs::PointCloud2>("/colored_points", 32);                    // 发布彩色点云
+  }
+
+private:
+  void initialize_params() {  // 初始化节点的参数，包括下采样方法、离群点移除方法、距离滤波器的阈值等
+    std::string downsample_method = private_nh.param<std::string>("downsample_method", "VOXELGRID");
+    double downsample_resolution = private_nh.param<double>("downsample_resolution", 0.1);
+
+    if(downsample_method == "VOXELGRID") {
+      std::cout << "downsample: VOXELGRID " << downsample_resolution << std::endl;
+      auto voxelgrid = new pcl::VoxelGrid<PointT>();
+      voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
+      downsample_filter.reset(voxelgrid);
+    } else if(downsample_method == "APPROX_VOXELGRID") {
+      std::cout << "downsample: APPROX_VOXELGRID " << downsample_resolution << std::endl;
+      pcl::ApproximateVoxelGrid<PointT>::Ptr approx_voxelgrid(new pcl::ApproximateVoxelGrid<PointT>());
+      approx_voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
+      downsample_filter = approx_voxelgrid;
+    } else {
+      if(downsample_method != "NONE") {
+        std::cerr << "warning: unknown downsampling type (" << downsample_method << ")" << std::endl;
+        std::cerr << "       : use passthrough filter" << std::endl;
+      }
+      std::cout << "downsample: NONE" << std::endl;
+    }
+
+    std::string outlier_removal_method = private_nh.param<std::string>("outlier_removal_method", "STATISTICAL");
+    if(outlier_removal_method == "STATISTICAL") {
+      int mean_k = private_nh.param<int>("statistical_mean_k", 20);
+      double stddev_mul_thresh = private_nh.param<double>("statistical_stddev", 1.0);
+      std::cout << "outlier_removal: STATISTICAL " << mean_k << " - " << stddev_mul_thresh << std::endl;
+
+      pcl::StatisticalOutlierRemoval<PointT>::Ptr sor(new pcl::StatisticalOutlierRemoval<PointT>());
+      sor->setMeanK(mean_k);
+      sor->setStddevMulThresh(stddev_mul_thresh);
+      outlier_removal_filter = sor;
+    } else if(outlier_removal_method == "RADIUS") {
+      double radius = private_nh.param<double>("radius_radius", 0.8);
+      int min_neighbors = private_nh.param<int>("radius_min_neighbors", 2);
+      std::cout << "outlier_removal: RADIUS " << radius << " - " << min_neighbors << std::endl;
+
+      pcl::RadiusOutlierRemoval<PointT>::Ptr rad(new pcl::RadiusOutlierRemoval<PointT>());
+      rad->setRadiusSearch(radius);
+      rad->setMinNeighborsInRadius(min_neighbors);
+      outlier_removal_filter = rad;
+    } else {
+      std::cout << "outlier_removal: NONE" << std::endl;
+    }
+
+    use_distance_filter = private_nh.param<bool>("use_distance_filter", true);
+    distance_near_thresh = private_nh.param<double>("distance_near_thresh", 1.0);
+    distance_far_thresh = private_nh.param<double>("distance_far_thresh", 100.0);
+
+    base_link_frame = private_nh.param<std::string>("base_link_frame", "");
+  }
+
+  void imu_callback(const sensor_msgs::ImuConstPtr& imu_msg) {  // 处理IMU数据的回调函数，将IMU数据存储在一个队列中，以便后续用于点云数据的去畸变处理
+    imu_queue.push_back(imu_msg);
+  }
+
+  void cloud_callback(const pcl::PointCloud<PointT>& src_cloud_r) {  // 处理点云数据的回调函数，处理接收到的点云数据，包括去畸变、距离滤波、下采样和离群点移除，然后将处理后的点云发布出去
+    pcl::PointCloud<PointT>::ConstPtr src_cloud = src_cloud_r.makeShared();
+    if(src_cloud->empty()) {
+      return;
+    }
+
+    src_cloud = deskewing(src_cloud);
+
+    // if base_link_frame is defined, transform the input cloud to the frame
+    if(!base_link_frame.empty()) {
+      if(!tf_listener.canTransform(base_link_frame, src_cloud->header.frame_id, ros::Time(0))) {
+        std::cerr << "failed to find transform between " << base_link_frame << " and " << src_cloud->header.frame_id << std::endl;
+      }
+
+      tf::StampedTransform transform;
+      tf_listener.waitForTransform(base_link_frame, src_cloud->header.frame_id, ros::Time(0), ros::Duration(2.0));
+      tf_listener.lookupTransform(base_link_frame, src_cloud->header.frame_id, ros::Time(0), transform);
+
+      pcl::PointCloud<PointT>::Ptr transformed(new pcl::PointCloud<PointT>());
+      pcl_ros::transformPointCloud(*src_cloud, *transformed, transform);
+      transformed->header.frame_id = base_link_frame;
+      transformed->header.stamp = src_cloud->header.stamp;
+      src_cloud = transformed;
+    }
+
+    pcl::PointCloud<PointT>::ConstPtr filtered = distance_filter(src_cloud);
+    filtered = downsample(filtered);
+    filtered = outlier_removal(filtered);
+
+    points_pub.publish(*filtered);
+  }
+
+  pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud) const {  // 对点云进行下采样，以减少点的数量，提高处理速度
+    if(!downsample_filter) {
+      return cloud;
+    }
+
+    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
+    downsample_filter->setInputCloud(cloud);
+    downsample_filter->filter(*filtered);
+    filtered->header = cloud->header;
+
+    return filtered;
+  }
+
+  pcl::PointCloud<PointT>::ConstPtr outlier_removal(const pcl::PointCloud<PointT>::ConstPtr& cloud) const {   // 移除点云中的离群点，这些点可能是噪声或异常值
+    if(!outlier_removal_filter) {
+      return cloud;
+    }
+
+    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
+    outlier_removal_filter->setInputCloud(cloud);
+    outlier_removal_filter->filter(*filtered);
+    filtered->header = cloud->header;
+
+    return filtered;
+  }
+
+  pcl::PointCloud<PointT>::ConstPtr distance_filter(const pcl::PointCloud<PointT>::ConstPtr& cloud) const {   // 基于距离的滤波，移除距离过近或过远的点
+    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
+    filtered->reserve(cloud->size());
+
+    std::copy_if(cloud->begin(), cloud->end(), std::back_inserter(filtered->points), [&](const PointT& p) {
+      double d = p.getVector3fMap().norm();
+      return d > distance_near_thresh && d < distance_far_thresh;
+    });
+
+    filtered->width = filtered->size();
+    filtered->height = 1;
+    filtered->is_dense = false;
+
+    filtered->header = cloud->header;
+
+    return filtered;
+  }
+
+  pcl::PointCloud<PointT>::ConstPtr deskewing(const pcl::PointCloud<PointT>::ConstPtr& cloud) {   // 根据IMU数据对点云进行去畸变处理，以纠正由于传感器运动引起的点云变形
+    ros::Time stamp = pcl_conversions::fromPCL(cloud->header.stamp);
+    if(imu_queue.empty()) {
+      return cloud;
+    }
+
+    // the color encodes the point number in the point sequence
+    if(colored_pub.getNumSubscribers()) {
+      pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored(new pcl::PointCloud<pcl::PointXYZRGB>());
+      colored->header = cloud->header;
+      colored->is_dense = cloud->is_dense;
+      colored->width = cloud->width;
+      colored->height = cloud->height;
+      colored->resize(cloud->size());
+
+      for(int i = 0; i < cloud->size(); i++) {
+        double t = static_cast<double>(i) / cloud->size();
+        colored->at(i).getVector4fMap() = cloud->at(i).getVector4fMap();
+        colored->at(i).r = 255 * t;
+        colored->at(i).g = 128;
+        colored->at(i).b = 255 * (1 - t);
+      }
+      colored_pub.publish(*colored);
+    }
+
+    sensor_msgs::ImuConstPtr imu_msg = imu_queue.front();
+
+    auto loc = imu_queue.begin();
+    for(; loc != imu_queue.end(); loc++) {
+      imu_msg = (*loc);
+      if((*loc)->header.stamp > stamp) {
+        break;
+      }
+    }
+
+    imu_queue.erase(imu_queue.begin(), loc);
+
+    Eigen::Vector3f ang_v(imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z);
+    ang_v *= -1;
+
+    pcl::PointCloud<PointT>::Ptr deskewed(new pcl::PointCloud<PointT>());
+    deskewed->header = cloud->header;
+    deskewed->is_dense = cloud->is_dense;
+    deskewed->width = cloud->width;
+    deskewed->height = cloud->height;
+    deskewed->resize(cloud->size());
+
+    double scan_period = private_nh.param<double>("scan_period", 0.1);
+    for(int i = 0; i < cloud->size(); i++) {
+      const auto& pt = cloud->at(i);
+
+      // TODO: transform IMU data into the LIDAR frame
+      double delta_t = scan_period * static_cast<double>(i) / cloud->size();
+      Eigen::Quaternionf delta_q(1, delta_t / 2.0 * ang_v[0], delta_t / 2.0 * ang_v[1], delta_t / 2.0 * ang_v[2]);
+      Eigen::Vector3f pt_ = delta_q.inverse() * pt.getVector3fMap();
+
+      deskewed->at(i) = cloud->at(i);
+      deskewed->at(i).getVector3fMap() = pt_;
+    }
+
+    return deskewed;
+  }
+
+private:
+  ros::NodeHandle nh;
+  ros::NodeHandle private_nh;
+
+  ros::Subscriber imu_sub;
+  std::vector<sensor_msgs::ImuConstPtr> imu_queue;
+
+  ros::Subscriber points_sub;
+  ros::Publisher points_pub;
+
+  ros::Publisher colored_pub;
+
+  tf::TransformListener tf_listener;
+
+  std::string base_link_frame;
+
+  bool use_distance_filter;
+  double distance_near_thresh;
+  double distance_far_thresh;
+
+  pcl::Filter<PointT>::Ptr downsample_filter;
+  pcl::Filter<PointT>::Ptr outlier_removal_filter;
+};
+
+}  // namespace hdl_graph_slam
+
+PLUGINLIB_EXPORT_CLASS(hdl_graph_slam::PrefilteringNodelet, nodelet::Nodelet)   // 将PrefilteringNodelet类注册为一个插件，使其可以在ROS节点管理器中被动态加载
+

apps/scan_matching_odometry_nodelet.cpp

@@ -0,0 +1,443 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#include <memory>
+#include <iostream>
+
+#include <ros/ros.h>
+#include <ros/time.h>
+#include <ros/duration.h>
+#include <pcl_ros/point_cloud.h>
+#include <tf_conversions/tf_eigen.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+
+#include <std_msgs/Time.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/filters/approximate_voxel_grid.h>
+
+#include <hdl_graph_slam/ros_utils.hpp>
+#include <hdl_graph_slam/registrations.hpp>
+#include <hdl_graph_slam/ScanMatchingStatus.h>
+
+namespace hdl_graph_slam {
+
+class ScanMatchingOdometryNodelet : public nodelet::Nodelet {
+public:
+  typedef pcl::PointXYZI PointT;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  ScanMatchingOdometryNodelet() {}
+  virtual ~ScanMatchingOdometryNodelet() {}
+
+  virtual void onInit() {  // 初始化函数
+    NODELET_DEBUG("initializing scan_matching_odometry_nodelet...");
+    nh = getNodeHandle();                 // 节点句柄
+    private_nh = getPrivateNodeHandle();  // 私有节点句柄
+
+    initialize_params();  // 参数初始化函数i
+
+    if(private_nh.param<bool>("enable_imu_frontend", false)) {  // .launch中是true，考虑imu的信息，默认是false
+      msf_pose_sub = nh.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/msf_core/pose", 1, boost::bind(&ScanMatchingOdometryNodelet::msf_pose_callback, this, _1, false));
+      // 这个是实时发布的？
+      msf_pose_after_update_sub = nh.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/msf_core/pose_after_update", 1, boost::bind(&ScanMatchingOdometryNodelet::msf_pose_callback, this, _1, true));
+      // 这个是某个关键帧更新后发布的？
+    }
+
+    points_sub = nh.subscribe("/filtered_points", 256, &ScanMatchingOdometryNodelet::cloud_callback, this);  // 接收过滤后的点云数据
+    read_until_pub = nh.advertise<std_msgs::Header>("/scan_matching_odometry/read_until", 32);               //
+    odom_pub = nh.advertise<nav_msgs::Odometry>(published_odom_topic, 32);                                   // 发布机器人的里程计信息
+    trans_pub = nh.advertise<geometry_msgs::TransformStamped>("/scan_matching_odometry/transform", 32);      //
+    status_pub = private_nh.advertise<ScanMatchingStatus>("/scan_matching_odometry/status", 8);              //
+    aligned_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/aligned_points", 32);                      //
+  }
+
+
+// 只发布三个话题:处理后的激光数据   前后帧合并的激光数据   odom
+
+// 定义一个debug 宏(定义宏),如果是ture,则发布消息,否则不发布
+
+// #define DEBUG 0
+// if(DEBUG){
+
+// 雷达厂家会给一个程序,会用一个topic发数据出来
+
+// }
+
+
+
+private:
+  /**
+   * @brief initialize parameters
+   */
+  void initialize_params() {
+    auto& pnh = private_nh;
+    published_odom_topic = private_nh.param<std::string>("published_odom_topic", "/odom");
+    points_topic = pnh.param<std::string>("points_topic", "/velodyne_points");
+    odom_frame_id = pnh.param<std::string>("odom_frame_id", "odom");
+    robot_odom_frame_id = pnh.param<std::string>("robot_odom_frame_id", "robot_odom");
+
+    // The minimum tranlational distance and rotation angle between keyframes.
+    // If this value is zero, frames are always compared with the previous frame
+    keyframe_delta_trans = pnh.param<double>("keyframe_delta_trans", 0.25);  // 三个参数有一个满足，则更新关键帧 keyframe
+    keyframe_delta_angle = pnh.param<double>("keyframe_delta_angle", 0.15);
+    keyframe_delta_time = pnh.param<double>("keyframe_delta_time", 1.0);
+
+    // Registration validation by thresholding
+    transform_thresholding = pnh.param<bool>("transform_thresholding", false);  // .launch 中也是给false
+    max_acceptable_trans = pnh.param<double>("max_acceptable_trans", 1.0);
+    max_acceptable_angle = pnh.param<double>("max_acceptable_angle", 1.0);
+
+    // select a downsample method (VOXELGRID, APPROX_VOXELGRID, NONE)
+    std::string downsample_method = pnh.param<std::string>("downsample_method", "VOXELGRID");  // launch 给的参数是 VOXELGRID
+
+    double downsample_resolution = pnh.param<double>("downsample_resolution", 0.1);  // 体素的大小，决定每个提速立方体的边长    .launch 中给的参数是0.1
+
+    if(downsample_method == "VOXELGRID") {
+      // 选用的是这个方法
+      std::cout << "downsample: VOXELGRID " << downsample_resolution << std::endl;
+      auto voxelgrid = new pcl::VoxelGrid<PointT>();  // 创建体素网格下采样对象
+
+      // 设置体素的长宽高。  体素的大小决定了网络的精细程度，体素越小，下采样后的点云越接近原始点云
+      voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
+      downsample_filter.reset(voxelgrid);
+
+    } else if(downsample_method == "APPROX_VOXELGRID") {
+      std::cout << "downsample: APPROX_VOXELGRID " << downsample_resolution << std::endl;
+      pcl::ApproximateVoxelGrid<PointT>::Ptr approx_voxelgrid(new pcl::ApproximateVoxelGrid<PointT>());
+      approx_voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
+      downsample_filter = approx_voxelgrid;
+    } else {
+      if(downsample_method != "NONE") {
+        std::cerr << "warning: unknown downsampling type (" << downsample_method << ")" << std::endl;
+        std::cerr << "       : use passthrough filter" << std::endl;
+      }
+      std::cout << "downsample: NONE" << std::endl;
+      pcl::PassThrough<PointT>::Ptr passthrough(new pcl::PassThrough<PointT>());
+      downsample_filter = passthrough;
+    }
+    registration = select_registration_method(pnh);
+
+    /*
+     三种下采样方法的基本原理：
+    VOXELGRID:
+        基本原理: 体素网格（VoxelGrid）下采样是一种常见的点云下采样方法。它将三维空间划分为一个由体素（三维像素）组成的网格，并在每个体素内部选择一个代表性点（通常是中心点或最接近中心的点），从而减少点的数量。所有在同一个体素内的点都被这个代表性点替代。
+        特点: 这种方法简单有效，可以均匀地减少点数，但可能会丢失一些细节。
+
+    APPROX_VOXELGRID:
+        基本原理: 近似体素网格（ApproximateVoxelGrid）下采样与体素网格类似，但它在处理每个体素内的点时采用更简单的方法来选择代表性点，例如选择包含在体素内的最大点或平均位置点。这种方法的计算效率更高，但可能不如体素网格下采样精确。
+        特点: 计算速度比VOXELGRID快，但精度较低，适用于对处理速度要求较高的场景。
+
+    PASSTHROUGH:
+        基本原理: 通道（PassThrough）下采样不是通过空间划分来减少点数，而是根据点的某些属性（如位置或强度）来筛选点。例如，可以设置一个过滤条件，只保留在特定高度范围内的点，或者只保留某个方向上的点。
+        特点: 允许用户定义更灵活的筛选条件，但不会像体素网格方法那样均匀地减少点数。如果没有指定下采样方法，或者指定的方法不被识别，就会使用通道过滤器作为默认方法。
+     */
+  }
+
+  /**
+   * @brief callback for point clouds
+   * @param cloud_msg  point cloud msg
+   */
+  void cloud_callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {  // cloud_msg：滤波后的点云数据
+    if(!ros::ok()) {
+      return;
+    }
+
+    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());  // 创建一个 pcl::PointCloud<PointT> 类型的指针
+    pcl::fromROSMsg(*cloud_msg, *cloud);                                // 将ROS消息格式的点云（*cloud_msg）转换为PCL（Point Cloud Library）格式的点云（*cloud）
+
+    Eigen::Matrix4f pose = matching(cloud_msg->header.stamp, cloud);              // 点云匹配函数，返回
+    publish_odometry(cloud_msg->header.stamp, cloud_msg->header.frame_id, pose);  // 发布里程计数据
+
+    // In offline estimation, point clouds until the published time will be supplied
+    std_msgs::HeaderPtr read_until(new std_msgs::Header());
+    read_until->frame_id = points_topic;
+    read_until->stamp = cloud_msg->header.stamp + ros::Duration(1, 0);
+    read_until_pub.publish(read_until);
+
+    read_until->frame_id = "/filtered_points";
+    read_until_pub.publish(read_until);
+  }
+
+  void msf_pose_callback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose_msg, bool after_update) {  // 多状态 MSF 的回调函数，接受MSF里程计的位置信息，并根据是否为更新后的位置，存储在不同变量中
+    if(after_update) {
+      msf_pose_after_update = pose_msg;
+    } else {
+      msf_pose = pose_msg;
+    }
+  }
+
+  /**
+   * @brief downsample a point cloud
+   * @param cloud  input cloud
+   * @return downsampled point cloud
+   */
+  pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud) const {  // 对点云数据进行向下采样，减少点的数量以提高处理速度
+    if(!downsample_filter) {
+      return cloud;
+    }
+
+    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());  // 创建一个新的点云对象，用来存储下采样后的点云数据
+    downsample_filter->setInputCloud(cloud);
+    downsample_filter->filter(*filtered);
+
+    return filtered;
+  }
+
+  /**
+   * @brief estimate the relative pose between an input cloud and a keyframe cloud
+   * @param stamp  the timestamp of the input cloud
+   * @param cloud  the input cloud
+   * @return the relative pose between the input cloud and the keyframe cloud
+   */
+  Eigen::Matrix4f matching(const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud) {  // 执行扫描匹配算法，估计输入点云和关键帧之间的相对位置
+
+    if(!keyframe) {                            // 判断 keyframe 是否为空指针，是的话说明还没有初始化关键真
+      prev_time = ros::Time();                 //
+      prev_trans.setIdentity();                // 设置为单位矩阵，表示：与上一次的位资没有发生变化
+      keyframe_pose.setIdentity();             // 关键帧的初始位资
+      keyframe_stamp = stamp;                  // 关键帧的时间戳
+      keyframe = downsample(cloud);            // 对点云数据进行下采样，减少点的数量，提高处理效率和精度
+      registration->setInputTarget(keyframe);  // 将 keyframe 设置成关键帧
+      return Eigen::Matrix4f::Identity();      // 没有之前的位资信息，假设与上次位资没有发生变化，返回单位矩阵
+
+      // registration 对象负责计算输入点云（cloud）与已有关键帧（keyframe）之间的相对变换。这种变换估算允许系统理解传感器（如激光雷达）在两次扫描之间的移动
+    }
+
+    auto filtered = downsample(cloud);       // 下采样
+    registration->setInputSource(filtered);  // 把点云数据给到 registration
+
+    std::string msf_source;                                       // 记录初始位资估计的来源（imu 或者 odometry)
+    Eigen::Isometry3f msf_delta = Eigen::Isometry3f::Identity();  // 三维仿射变换的单位矩阵，用于存储上一帧到当前帧的位资变化，3表示三维
+    // 缩放变换和旋转变换称为线性变换（linear transform）   线性变换和平移变换统称为仿射变换（affine transfrom）
+
+    if(private_nh.param<bool>("enable_imu_frontend", false)) {  // .launch 中是 true
+      if(msf_pose && msf_pose->header.stamp > keyframe_stamp && msf_pose_after_update && msf_pose_after_update->header.stamp > keyframe_stamp) {
+        // 如果 msf_pose 不是空指针，   msf_pose：imu 数据
+        // 如果 msf_pose 的数据比当前关键帧的数据更新
+        // 如果 msf_pose_after_update 不是空指针
+        Eigen::Isometry3d pose0 = pose2isometry(msf_pose_after_update->pose.pose);  // 函数pose2isometry()  将机器人在世界坐标系中的位置和方向 转换成对应的 仿射变换矩阵
+        Eigen::Isometry3d pose1 = pose2isometry(msf_pose->pose.pose);
+        Eigen::Isometry3d delta = pose0.inverse() * pose1;  // 相乘得到一个描述从pose0到pose1的变换的3D仿射变换
+
+        msf_source = "imu";
+        msf_delta = delta.cast<float>();  // 将double转换成float
+      } else {
+        std::cerr << "msf data is too old" << std::endl;
+      }
+    }                                                                                                  //
+    else if(private_nh.param<bool>("enable_robot_odometry_init_guess", false) && !prev_time.isZero())  // .launch 给的参数是 false     !prev_time.isZero() 判断是不是有效的时间戳
+    {
+      tf::StampedTransform transform;  // 声明一个变量，用来存储两个坐标帧之间的变换信息
+
+      if(tf_listener.waitForTransform(cloud->header.frame_id, stamp, cloud->header.frame_id, prev_time, robot_odom_frame_id, ros::Duration(0)))
+      // 将cloud->header.frame_id 坐标帧变换到 robot_odom_frame_id 坐标帧
+      // waitForTransform 方法等待直到可以计算出这个变换，或者直到超时（这里设置的超时时间为0，意味着无限等待）   stamp 是当前的时间戳
+      {
+        tf_listener.lookupTransform(cloud->header.frame_id, stamp, cloud->header.frame_id, prev_time, robot_odom_frame_id, transform);
+        // 如果waitForTransform成功，那么调用lookupTransform方法来获取实际的变换，并将其存储在transform变量中
+      }
+
+      else if(tf_listener.waitForTransform(cloud->header.frame_id, ros::Time(0), cloud->header.frame_id, prev_time, robot_odom_frame_id, ros::Duration(0)))
+      // 如果上面的变换失败了,再尝试一遍,但参考时间改成 ros::Time(0)
+      {
+        tf_listener.lookupTransform(cloud->header.frame_id, ros::Time(0), cloud->header.frame_id, prev_time, robot_odom_frame_id, transform);
+      }
+
+      if(transform.stamp_.isZero()) {
+        NODELET_WARN_STREAM("failed to look up transform between " << cloud->header.frame_id << " and " << robot_odom_frame_id);
+      } else {
+        msf_source = "odometry";
+        msf_delta = tf2isometry(transform).cast<float>();  // 将 transform 转换为 Eigen::Isometry3f 类型（3D仿射变换，使用float类型）
+      }
+    }
+
+    pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());  // 创建一个新的点云对象aligned，用于存储对齐后的点云数据
+    registration->align(*aligned, prev_trans * msf_delta.matrix());       // 用 registration 来对齐点云, 如果没有imu或者odom信息, msf_delta 是单位矩阵, 即将观测到的点云数据对齐到关键帧?
+
+    publish_scan_matching_status(stamp, cloud->header.frame_id, aligned, msf_source, msf_delta);  // 发布扫描匹配的状态信息，包括时间戳、坐标帧ID、对齐后的点云、位姿来源和位姿更新
+
+    if(!registration->hasConverged()) {  // 检查扫描匹配是否收敛(是否匹配成功?)。如果没有收敛，输出信息并返回上一次的位姿
+      NODELET_INFO_STREAM("scan matching has not converged!!");
+      NODELET_INFO_STREAM("ignore this frame(" << stamp << ")");
+      return keyframe_pose * prev_trans;
+    }
+
+    Eigen::Matrix4f trans = registration->getFinalTransformation();  // 获得当前点云和上一帧点云 关键帧 的仿射变换
+    Eigen::Matrix4f odom = keyframe_pose * trans;                    // 算出来 odom
+
+    if(transform_thresholding) {  // .launch 设置为false  默认为 false
+      // 如果启用了变换阈值判断，计算本次变换的平移和旋转，并与最大可接受值进行比较。如果超出阈值，输出信息并返回上一次的位姿
+      // 即如果某两帧的点云差别特别大,忽略后面这一帧的匹配,返回上一个姿态的 odom
+      Eigen::Matrix4f delta = prev_trans.inverse() * trans;
+      double dx = delta.block<3, 1>(0, 3).norm();
+      double da = std::acos(Eigen::Quaternionf(delta.block<3, 3>(0, 0)).w());
+
+      if(dx > max_acceptable_trans || da > max_acceptable_angle) {
+        NODELET_INFO_STREAM("too large transform!!  " << dx << "[m] " << da << "[rad]");
+        NODELET_INFO_STREAM("ignore this frame(" << stamp << ")");
+        return keyframe_pose * prev_trans;
+      }
+    }
+
+    prev_time = stamp;   // 当前帧的时间戳
+    prev_trans = trans;  // 当前帧的仿射变换
+
+    auto keyframe_trans = matrix2transform(stamp, keyframe_pose, odom_frame_id, "keyframe");  // 将变换矩阵转换为tf::Transform对象，用于发布关键帧的变换
+    keyframe_broadcaster.sendTransform(keyframe_trans);                                       // 发布关键帧的变换(这个是发送到哪里?  )
+
+    double delta_trans = trans.block<3, 1>(0, 3).norm();                              // 计算 当前帧 与 关键帧 变换的 平移距离
+    double delta_angle = std::acos(Eigen::Quaternionf(trans.block<3, 3>(0, 0)).w());  // 计算 当前帧 与 关键帧 变换的 旋转角度
+    double delta_time = (stamp - keyframe_stamp).toSec();                             // 计算 当前帧 与 关键帧 变换的 时间差
+    if(delta_trans > keyframe_delta_trans || delta_angle > keyframe_delta_angle || delta_time > keyframe_delta_time) {  // 如果有一个超过阈值,更新关键帧
+      keyframe = filtered;
+      registration->setInputTarget(keyframe);
+
+      keyframe_pose = odom;
+      keyframe_stamp = stamp;
+      prev_time = stamp;
+      prev_trans.setIdentity();
+    }
+
+    if(aligned_points_pub.getNumSubscribers() > 0) {  // 如果有节点订阅了对齐后的点云，进行变换并发布
+      pcl::transformPointCloud(*cloud, *aligned, odom);
+      aligned->header.frame_id = odom_frame_id;
+      aligned_points_pub.publish(*aligned);
+    }
+    std::cout << "The matrix odom is: \n" << odom << std::endl;
+    return odom;  // 返回里程计
+  }
+
+  /**
+   * @brief publish odometry
+   * @param stamp  timestamp
+   * @param pose   odometry pose to be published
+   */
+  void publish_odometry(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose) {  // 发布里程计数据
+    // publish transform stamped for IMU integration
+    geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, odom_frame_id, base_frame_id);
+    trans_pub.publish(odom_trans);
+
+    // broadcast the transform over tf
+    odom_broadcaster.sendTransform(odom_trans);
+
+    // publish the transform
+    nav_msgs::Odometry odom;
+    odom.header.stamp = stamp;
+    odom.header.frame_id = odom_frame_id;
+
+    odom.pose.pose.position.x = pose(0, 3);
+    odom.pose.pose.position.y = pose(1, 3);
+    odom.pose.pose.position.z = pose(2, 3);
+    odom.pose.pose.orientation = odom_trans.transform.rotation;
+
+    odom.child_frame_id = base_frame_id;
+    odom.twist.twist.linear.x = 0.0;
+    odom.twist.twist.linear.y = 0.0;
+    odom.twist.twist.angular.z = 0.0;
+
+    odom_pub.publish(odom);
+  }
+
+  /**
+   * @brief publish scan matching status
+   */
+  void publish_scan_matching_status(const ros::Time& stamp, const std::string& frame_id, pcl::PointCloud<pcl::PointXYZI>::ConstPtr aligned, const std::string& msf_source, const Eigen::Isometry3f& msf_delta) {
+    // 发布扫描的状态，包括匹配是否收敛、匹配误差、内点比例、相对位置等信息
+    if(!status_pub.getNumSubscribers()) {
+      return;
+    }
+
+    ScanMatchingStatus status;
+    status.header.frame_id = frame_id;
+    status.header.stamp = stamp;
+    status.has_converged = registration->hasConverged();
+    status.matching_error = registration->getFitnessScore();
+
+    const double max_correspondence_dist = 0.5;
+
+    int num_inliers = 0;
+    std::vector<int> k_indices;
+    std::vector<float> k_sq_dists;
+    for(int i = 0; i < aligned->size(); i++) {
+      const auto& pt = aligned->at(i);
+      registration->getSearchMethodTarget()->nearestKSearch(pt, 1, k_indices, k_sq_dists);
+      if(k_sq_dists[0] < max_correspondence_dist * max_correspondence_dist) {
+        num_inliers++;
+      }
+    }
+    status.inlier_fraction = static_cast<float>(num_inliers) / aligned->size();
+
+    status.relative_pose = isometry2pose(Eigen::Isometry3f(registration->getFinalTransformation()).cast<double>());
+
+    if(!msf_source.empty()) {
+      status.prediction_labels.resize(1);
+      status.prediction_labels[0].data = msf_source;
+
+      status.prediction_errors.resize(1);
+      Eigen::Isometry3f error = Eigen::Isometry3f(registration->getFinalTransformation()).inverse() * msf_delta;
+      status.prediction_errors[0] = isometry2pose(error.cast<double>());
+    }
+
+    status_pub.publish(status);
+  }
+
+private:
+  // ROS topics
+  ros::NodeHandle nh;
+  ros::NodeHandle private_nh;
+
+  ros::Subscriber points_sub;
+  ros::Subscriber msf_pose_sub;
+  ros::Subscriber msf_pose_after_update_sub;
+
+  ros::Publisher odom_pub;
+  ros::Publisher trans_pub;
+  ros::Publisher aligned_points_pub;
+  ros::Publisher status_pub;
+  tf::TransformListener tf_listener;
+  tf::TransformBroadcaster odom_broadcaster;
+  tf::TransformBroadcaster keyframe_broadcaster;
+
+  std::string published_odom_topic;
+  std::string points_topic;
+  std::string odom_frame_id;
+  std::string robot_odom_frame_id;
+  ros::Publisher read_until_pub;
+
+  // keyframe parameters
+  double keyframe_delta_trans;  // minimum distance between keyframes
+  double keyframe_delta_angle;  //
+  double keyframe_delta_time;   //
+
+  // registration validation by thresholding
+  bool transform_thresholding;  //
+  double max_acceptable_trans;  //
+  double max_acceptable_angle;
+
+  // odometry calculation
+  geometry_msgs::PoseWithCovarianceStampedConstPtr msf_pose;
+  geometry_msgs::PoseWithCovarianceStampedConstPtr msf_pose_after_update;
+
+  ros::Time prev_time;                         // 当前关键帧的时间戳?
+  Eigen::Matrix4f prev_trans;                  // 地图起点到当前   帧  的放射变换
+  Eigen::Matrix4f keyframe_pose;               // 地图起点到当前 关键帧 的仿射变换
+  ros::Time keyframe_stamp;                    // 关键帧的时间戳
+  pcl::PointCloud<PointT>::ConstPtr keyframe;  // 关键帧
+
+  //
+  pcl::Filter<PointT>::Ptr downsample_filter;
+  pcl::Registration<PointT, PointT>::Ptr registration;
+};
+
+}  // namespace hdl_graph_slam
+
+PLUGINLIB_EXPORT_CLASS(hdl_graph_slam::ScanMatchingOdometryNodelet, nodelet::Nodelet)

cmake/FindG2O.cmake

@@ -0,0 +1,117 @@
+# Find the header files
+
+FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h
+  ${G2O_ROOT}/include
+  $ENV{G2O_ROOT}/include
+  $ENV{G2O_ROOT}
+  /usr/local/include
+  /usr/include
+  /opt/local/include
+  /sw/local/include
+  /sw/include
+  /opt/ros/$ENV{ROS_DISTRO}/include
+  NO_DEFAULT_PATH
+  )
+
+# Macro to unify finding both the debug and release versions of the
+# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY
+# macro.
+
+MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME)
+
+  FIND_LIBRARY("${MYLIBRARY}_DEBUG"
+    NAMES "g2o_${MYLIBRARYNAME}_d"
+    PATHS
+    ${G2O_ROOT}/lib/Debug
+    ${G2O_ROOT}/lib
+    $ENV{G2O_ROOT}/lib/Debug
+    $ENV{G2O_ROOT}/lib
+    /opt/ros/$ENV{ROS_DISTRO}/lib
+    NO_DEFAULT_PATH
+    )
+
+  FIND_LIBRARY("${MYLIBRARY}_DEBUG"
+    NAMES "g2o_${MYLIBRARYNAME}_d"
+    PATHS
+    ~/Library/Frameworks
+    /Library/Frameworks
+    /usr/local/lib
+    /usr/local/lib64
+    /usr/lib
+    /usr/lib64
+    /opt/local/lib
+    /sw/local/lib
+    /sw/lib
+    )
+  
+  FIND_LIBRARY(${MYLIBRARY}
+    NAMES "g2o_${MYLIBRARYNAME}"
+    PATHS
+    ${G2O_ROOT}/lib/Release
+    ${G2O_ROOT}/lib
+    $ENV{G2O_ROOT}/lib/Release
+    $ENV{G2O_ROOT}/lib
+    /opt/ros/$ENV{ROS_DISTRO}/lib
+    NO_DEFAULT_PATH
+    )
+
+  FIND_LIBRARY(${MYLIBRARY}
+    NAMES "g2o_${MYLIBRARYNAME}"
+    PATHS
+    ~/Library/Frameworks
+    /Library/Frameworks
+    /usr/local/lib
+    /usr/local/lib64
+    /usr/lib
+    /usr/lib64
+    /opt/local/lib
+    /sw/local/lib
+    /sw/lib
+    )
+  
+  IF(NOT ${MYLIBRARY}_DEBUG)
+    IF(MYLIBRARY)
+      SET(${MYLIBRARY}_DEBUG ${MYLIBRARY})
+    ENDIF(MYLIBRARY)
+  ENDIF( NOT ${MYLIBRARY}_DEBUG)
+  
+ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME)
+
+# Find the core elements
+FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff)
+FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core)
+
+# Find the CLI library
+FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli)
+
+# Find the pluggable solvers
+FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod)
+FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse)
+FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension)
+FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense)
+FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg)
+FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear)
+FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only)
+FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen)
+
+# Find the predefined types
+FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data)
+FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp)
+FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba)
+FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d)
+FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3)
+FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d)
+FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d)
+FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D_ADDONS types_slam3d_addons)
+
+# G2O solvers declared found if we found at least one solver
+SET(G2O_SOLVERS_FOUND "NO")
+IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
+  SET(G2O_SOLVERS_FOUND "YES")
+ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
+
+# G2O itself declared found if we found the core libraries and at least one solver
+SET(G2O_FOUND "NO")
+IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
+  SET(G2O_FOUND "YES")
+ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)

docker/build.sh

@@ -0,0 +1,2 @@
+#!/bin/bash
+docker build --tag hdl_graph_slam -f noetic/Dockerfile ..

docker/howtouse.md

@@ -0,0 +1,40 @@
+# hdl_graph_slam
+
+Original repository: https://github.com/koide3/hdl_graph_slam
+
+
+## Build
+```bash
+cd hdl_graph_slam/docker
+./build.sh
+```
+
+## Run
+
+### On host:
+```bash
+roscore
+```
+
+```bash
+rosparam set use_sim_time true
+
+cd hdl_graph_slam/rviz
+rviz -d hdl_graph_slam.rviz
+```
+
+```bash
+rosbag play --clock hdl_400.bag
+```
+http://www.aisl.cs.tut.ac.jp/databases/hdl_graph_slam/hdl_400.bag.tar.gz
+
+### On docker image:
+```bash
+cd hdl_graph_slam/docker
+./run.sh
+
+roslaunch hdl_graph_slam hdl_graph_slam_400.launch
+```
+
+
+![hdl_graph_slam](https://user-images.githubusercontent.com/31344317/98347836-4fed5a00-205b-11eb-931c-158f6cd056bf.gif)

docker/kinetic/Dockerfile

@@ -0,0 +1,25 @@
+FROM ros:kinetic
+
+RUN apt-get update && apt-get install --no-install-recommends -y && apt-get install --no-install-recommends -y wget nano build-essential ros-kinetic-geodesy ros-kinetic-pcl-ros ros-kinetic-nmea-msgs ros-kinetic-rviz ros-kinetic-tf-conversions ros-kinetic-libg2o \
+ && apt-get clean \
+ && rm -rf /var/lib/apt/lists/*
+
+
+RUN mkdir -p /root/catkin_ws/src
+WORKDIR /root/catkin_ws/src
+RUN /bin/bash -c '. /opt/ros/kinetic/setup.bash; catkin_init_workspace'
+RUN git clone https://github.com/koide3/ndt_omp.git
+RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive
+
+COPY . /root/catkin_ws/src/hdl_graph_slam/
+
+WORKDIR /root/catkin_ws
+RUN /bin/bash -c '. /opt/ros/kinetic/setup.bash; catkin_make'
+RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh
+
+RUN apt-get clean && rm -rf /var/lib/apt/lists/*
+
+WORKDIR /
+
+ENTRYPOINT ["/ros_entrypoint.sh"]
+CMD ["bash"]

docker/kinetic_llvm/Dockerfile

@@ -0,0 +1,30 @@
+FROM ros:kinetic
+
+RUN apt-get update && apt-get install --no-install-recommends -y \
+ && apt-get install --no-install-recommends -y \
+ wget nano build-essential libomp-dev clang lld-6.0 \
+ ros-kinetic-geodesy ros-kinetic-pcl-ros ros-kinetic-nmea-msgs \
+ ros-kinetic-rviz ros-kinetic-tf-conversions ros-kinetic-libg2o \
+ && apt-get clean \
+ && rm -rf /var/lib/apt/lists/*
+
+RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld-6.0 50
+
+RUN mkdir -p /root/catkin_ws/src
+WORKDIR /root/catkin_ws/src
+RUN /bin/bash -c '. /opt/ros/kinetic/setup.bash; catkin_init_workspace'
+RUN git clone https://github.com/koide3/ndt_omp.git
+RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive
+
+COPY . /root/catkin_ws/src/hdl_graph_slam/
+
+WORKDIR /root/catkin_ws
+RUN /bin/bash -c '. /opt/ros/kinetic/setup.bash; CC=clang CXX=clang++ catkin_make'
+RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh
+
+RUN apt-get clean && rm -rf /var/lib/apt/lists/*
+
+WORKDIR /
+
+ENTRYPOINT ["/ros_entrypoint.sh"]
+CMD ["bash"]

docker/melodic/Dockerfile

@@ -0,0 +1,26 @@
+FROM ros:melodic
+
+RUN apt-get update && apt-get install -y --no-install-recommends \
+  && apt-get install -y --no-install-recommends \
+  wget nano build-essential libomp-dev clang lld\
+  ros-melodic-geodesy ros-melodic-pcl-ros ros-melodic-nmea-msgs \
+  ros-melodic-rviz ros-melodic-tf-conversions ros-melodic-libg2o \
+  && apt-get clean \
+  && rm -rf /var/lib/apt/lists/*
+
+RUN mkdir -p /root/catkin_ws/src
+WORKDIR /root/catkin_ws/src
+RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_init_workspace'
+RUN git clone https://github.com/koide3/ndt_omp.git
+RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive
+
+COPY . /root/catkin_ws/src/hdl_graph_slam/
+
+WORKDIR /root/catkin_ws
+RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_make'
+RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh
+
+WORKDIR /
+
+ENTRYPOINT ["/ros_entrypoint.sh"]
+CMD ["bash"]

docker/melodic_llvm/Dockerfile

@@ -0,0 +1,28 @@
+FROM ros:melodic
+
+RUN apt-get update && apt-get install -y --no-install-recommends \
+  && apt-get install -y --no-install-recommends \
+  wget nano build-essential libomp-dev clang lld\
+  ros-melodic-geodesy ros-melodic-pcl-ros ros-melodic-nmea-msgs \
+  ros-melodic-rviz ros-melodic-tf-conversions ros-melodic-libg2o \
+  && apt-get clean \
+  && rm -rf /var/lib/apt/lists/*
+
+RUN mkdir -p /root/catkin_ws/src
+WORKDIR /root/catkin_ws/src
+RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_init_workspace'
+RUN git clone -b melodic https://github.com/koide3/ndt_omp.git
+RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive
+
+COPY . /root/catkin_ws/src/hdl_graph_slam/
+
+RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50
+
+WORKDIR /root/catkin_ws
+RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; CC=clang CXX=clang++ catkin_make'
+RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh
+
+WORKDIR /
+
+ENTRYPOINT ["/ros_entrypoint.sh"]
+CMD ["bash"]

docker/noetic/Dockerfile

@@ -0,0 +1,26 @@
+FROM ros:noetic
+
+RUN apt-get update && apt-get install -y --no-install-recommends \
+  && apt-get install -y --no-install-recommends \
+  wget nano build-essential libomp-dev clang lld git\
+  ros-noetic-geodesy ros-noetic-pcl-ros ros-noetic-nmea-msgs \
+  ros-noetic-rviz ros-noetic-tf-conversions ros-noetic-libg2o \
+  && apt-get clean \
+  && rm -rf /var/lib/apt/lists/*
+
+RUN mkdir -p /root/catkin_ws/src
+WORKDIR /root/catkin_ws/src
+RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_init_workspace'
+RUN git clone https://github.com/koide3/ndt_omp.git
+RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive
+
+COPY . /root/catkin_ws/src/hdl_graph_slam/
+
+WORKDIR /root/catkin_ws
+RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_make'
+RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh
+
+WORKDIR /
+
+ENTRYPOINT ["/ros_entrypoint.sh"]
+CMD ["bash"]

docker/noetic_llvm/Dockerfile

@@ -0,0 +1,28 @@
+FROM ros:noetic
+
+RUN apt-get update && apt-get install -y --no-install-recommends \
+  && apt-get install -y --no-install-recommends \
+  wget nano build-essential libomp-dev clang lld git\
+  ros-noetic-geodesy ros-noetic-pcl-ros ros-noetic-nmea-msgs \
+  ros-noetic-rviz ros-noetic-tf-conversions ros-noetic-libg2o \
+  && apt-get clean \
+  && rm -rf /var/lib/apt/lists/*
+
+RUN mkdir -p /root/catkin_ws/src
+WORKDIR /root/catkin_ws/src
+RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_init_workspace'
+RUN git clone https://github.com/koide3/ndt_omp.git
+RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive
+
+COPY . /root/catkin_ws/src/hdl_graph_slam/
+
+RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50
+
+WORKDIR /root/catkin_ws
+RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; CC=clang CXX=clang++ catkin_make'
+RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh
+
+WORKDIR /
+
+ENTRYPOINT ["/ros_entrypoint.sh"]
+CMD ["bash"]

docker/run.sh

@@ -0,0 +1,6 @@
+#!/bin/bash
+docker run --net=host -it --rm \
+           -v $(realpath ..):/root/catkin_ws/src/hdl_graph_slam \
+           -w /root/catkin_ws/src/hdl_graph_slam \
+           $@ \
+           hdl_graph_slam

include/g2o/edge_plane_identity.hpp

@@ -0,0 +1,103 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef EDGE_PLANE_IDENTITY_HPP
+#define EDGE_PLANE_IDENTITY_HPP
+
+#include <Eigen/Dense>
+#include <g2o/core/base_binary_edge.h>
+#include <g2o/types/slam3d_addons/vertex_plane.h>
+
+namespace g2o {
+
+/**
+ * @brief A modified version of g2o::EdgePlane. This class takes care of flipped plane normals.
+ *
+ */
+class EdgePlaneIdentity : public BaseBinaryEdge<4, Eigen::Vector4d, VertexPlane, VertexPlane> {
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  EdgePlaneIdentity() : BaseBinaryEdge<4, Eigen::Vector4d, VertexPlane, VertexPlane>() {
+    _information.setIdentity();
+    _error.setZero();
+  }
+  void computeError() {
+    const VertexPlane* v1 = static_cast<const VertexPlane*>(_vertices[0]);
+    const VertexPlane* v2 = static_cast<const VertexPlane*>(_vertices[1]);
+
+    Eigen::Vector4d p1 = v1->estimate().toVector();
+    Eigen::Vector4d p2 = v2->estimate().toVector();
+
+    if(p1.dot(p2) < 0.0) {
+      p2 = -p2;
+    }
+
+    _error = (p2 - p1) - _measurement;
+  }
+  virtual bool read(std::istream& is) override {
+    Eigen::Vector4d v;
+    for(int i = 0; i < 4; ++i) {
+      is >> v[i];
+    }
+
+    setMeasurement(v);
+    for(int i = 0; i < information().rows(); ++i) {
+      for(int j = i; j < information().cols(); ++j) {
+        is >> information()(i, j);
+        if(i != j) {
+          information()(j, i) = information()(i, j);
+        }
+      }
+    }
+
+    return true;
+  }
+
+  virtual bool write(std::ostream& os) const override {
+    for(int i = 0; i < 4; ++i) {
+      os << _measurement[i] << " ";
+    }
+
+    for(int i = 0; i < information().rows(); ++i) {
+      for(int j = i; j < information().cols(); ++j) {
+        os << " " << information()(i, j);
+      };
+    }
+    return os.good();
+  }
+
+  virtual void setMeasurement(const Eigen::Vector4d& m) override {
+    _measurement = m;
+  }
+
+  virtual int measurementDimension() const override {
+    return 4;
+  }
+};
+
+}  // namespace g2o
+
+#endif  // EDGE_PLANE_PARALLEL_HPP

include/g2o/edge_plane_parallel.hpp

@@ -0,0 +1,159 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef EDGE_PLANE_PARALLEL_HPP
+#define EDGE_PLANE_PARALLEL_HPP
+
+#include <Eigen/Dense>
+#include <g2o/core/base_binary_edge.h>
+#include <g2o/types/slam3d_addons/vertex_plane.h>
+
+namespace g2o {
+
+class EdgePlaneParallel : public BaseBinaryEdge<3, Eigen::Vector3d, VertexPlane, VertexPlane> {
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  EdgePlaneParallel() : BaseBinaryEdge<3, Eigen::Vector3d, VertexPlane, VertexPlane>() {
+    _information.setIdentity();
+    _error.setZero();
+  }
+
+  void computeError() override {
+    const VertexPlane* v1 = static_cast<const VertexPlane*>(_vertices[0]);
+    const VertexPlane* v2 = static_cast<const VertexPlane*>(_vertices[1]);
+
+    Eigen::Vector3d normal1 = v1->estimate().normal();
+    Eigen::Vector3d normal2 = v2->estimate().normal();
+
+    if(normal1.dot(normal2) < 0.0) {
+      normal2 = -normal2;
+    }
+
+    _error = (normal2 - normal1) - _measurement;
+  }
+  virtual bool read(std::istream& is) override {
+    Eigen::Vector3d v;
+    for(int i = 0; i < 3; ++i) {
+      is >> v[i];
+    }
+
+    setMeasurement(v);
+    for(int i = 0; i < information().rows(); ++i) {
+      for(int j = i; j < information().cols(); ++j) {
+        is >> information()(i, j);
+        if(i != j) {
+          information()(j, i) = information()(i, j);
+        }
+      }
+    }
+
+    return true;
+  }
+
+  virtual bool write(std::ostream& os) const override {
+    for(int i = 0; i < 3; ++i) {
+      os << _measurement[i] << " ";
+    }
+
+    for(int i = 0; i < information().rows(); ++i) {
+      for(int j = i; j < information().cols(); ++j) {
+        os << " " << information()(i, j);
+      };
+    }
+    return os.good();
+  }
+
+  virtual void setMeasurement(const Eigen::Vector3d& m) override {
+    _measurement = m;
+  }
+
+  virtual int measurementDimension() const override {
+    return 3;
+  }
+};
+
+class EdgePlanePerpendicular : public BaseBinaryEdge<1, Eigen::Vector3d, VertexPlane, VertexPlane> {
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  EdgePlanePerpendicular() : BaseBinaryEdge<1, Eigen::Vector3d, VertexPlane, VertexPlane>() {
+    _information.setIdentity();
+    _error.setZero();
+  }
+
+  void computeError() override {
+    const VertexPlane* v1 = static_cast<const VertexPlane*>(_vertices[0]);
+    const VertexPlane* v2 = static_cast<const VertexPlane*>(_vertices[1]);
+
+    Eigen::Vector3d normal1 = v1->estimate().normal().normalized();
+    Eigen::Vector3d normal2 = v2->estimate().normal().normalized();
+
+    _error[0] = normal1.dot(normal2);
+  }
+
+  virtual bool read(std::istream& is) override {
+    Eigen::Vector3d v;
+    for(int i = 0; i < 3; ++i) {
+      is >> v[i];
+    }
+
+    setMeasurement(v);
+    for(int i = 0; i < information().rows(); ++i) {
+      for(int j = i; j < information().cols(); ++j) {
+        is >> information()(i, j);
+        if(i != j) {
+          information()(j, i) = information()(i, j);
+        }
+      }
+    }
+
+    return true;
+  }
+
+  virtual bool write(std::ostream& os) const override {
+    for(int i = 0; i < 3; ++i) {
+      os << _measurement[i] << " ";
+    }
+
+    for(int i = 0; i < information().rows(); ++i) {
+      for(int j = i; j < information().cols(); ++j) {
+        os << " " << information()(i, j);
+      };
+    }
+    return os.good();
+  }
+
+  virtual void setMeasurement(const Eigen::Vector3d& m) override {
+    _measurement = m;
+  }
+
+  virtual int measurementDimension() const override {
+    return 3;
+  }
+};
+
+}  // namespace g2o
+
+#endif  // EDGE_PLANE_PARALLEL_HPP

include/g2o/edge_plane_prior.hpp

@@ -0,0 +1,107 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef EDGE_PLANE_PRIOR_HPP
+#define EDGE_PLANE_PRIOR_HPP
+
+#include <Eigen/Dense>
+#include <g2o/core/base_unary_edge.h>
+#include <g2o/types/slam3d_addons/vertex_plane.h>
+
+namespace g2o {
+class EdgePlanePriorNormal : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexPlane> {
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  EdgePlanePriorNormal() : g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexPlane>() {}
+
+  void computeError() override {
+    const g2o::VertexPlane* v1 = static_cast<const g2o::VertexPlane*>(_vertices[0]);
+    Eigen::Vector3d normal = v1->estimate().normal();
+
+    if(normal.dot(_measurement) < 0.0) {
+      normal = -normal;
+    }
+
+    _error = normal - _measurement;
+  }
+
+  void setMeasurement(const Eigen::Vector3d& m) override {
+    _measurement = m;
+  }
+
+  virtual bool read(std::istream& is) override {
+    Eigen::Vector3d v;
+    is >> v(0) >> v(1) >> v(2);
+    setMeasurement(v);
+    for(int i = 0; i < information().rows(); ++i)
+      for(int j = i; j < information().cols(); ++j) {
+        is >> information()(i, j);
+        if(i != j) information()(j, i) = information()(i, j);
+      }
+    return true;
+  }
+  virtual bool write(std::ostream& os) const override {
+    Eigen::Vector3d v = _measurement;
+    os << v(0) << " " << v(1) << " " << v(2) << " ";
+    for(int i = 0; i < information().rows(); ++i)
+      for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j);
+    return os.good();
+  }
+};
+
+class EdgePlanePriorDistance : public g2o::BaseUnaryEdge<1, double, g2o::VertexPlane> {
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  EdgePlanePriorDistance() : g2o::BaseUnaryEdge<1, double, g2o::VertexPlane>() {}
+
+  void computeError() override {
+    const g2o::VertexPlane* v1 = static_cast<const g2o::VertexPlane*>(_vertices[0]);
+    _error[0] = _measurement - v1->estimate().distance();
+  }
+
+  void setMeasurement(const double& m) override {
+    _measurement = m;
+  }
+
+  virtual bool read(std::istream& is) override {
+    is >> _measurement;
+    for(int i = 0; i < information().rows(); ++i)
+      for(int j = i; j < information().cols(); ++j) {
+        is >> information()(i, j);
+        if(i != j) information()(j, i) = information()(i, j);
+      }
+    return true;
+  }
+  virtual bool write(std::ostream& os) const override {
+    os << _measurement;
+    for(int i = 0; i < information().rows(); ++i)
+      for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j);
+    return os.good();
+  }
+};
+}  // namespace g2o
+
+#endif  // EDGE_SE3_PRIORXY_HPP

include/g2o/edge_se3_plane.hpp

@@ -0,0 +1,74 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef KKL_G2O_EDGE_SE3_PLANE_HPP
+#define KKL_G2O_EDGE_SE3_PLANE_HPP
+
+#include <g2o/types/slam3d/edge_se3.h>
+#include <g2o/types/slam3d/vertex_se3.h>
+#include <g2o/types/slam3d_addons/vertex_plane.h>
+
+namespace g2o {
+class EdgeSE3Plane : public g2o::BaseBinaryEdge<3, g2o::Plane3D, g2o::VertexSE3, g2o::VertexPlane> {
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  EdgeSE3Plane() : BaseBinaryEdge<3, g2o::Plane3D, g2o::VertexSE3, g2o::VertexPlane>() {}
+
+  void computeError() override {
+    const g2o::VertexSE3* v1 = static_cast<const g2o::VertexSE3*>(_vertices[0]);
+    const g2o::VertexPlane* v2 = static_cast<const g2o::VertexPlane*>(_vertices[1]);
+
+    Eigen::Isometry3d w2n = v1->estimate().inverse();
+    Plane3D local_plane = w2n * v2->estimate();
+    _error = local_plane.ominus(_measurement);
+  }
+
+  void setMeasurement(const g2o::Plane3D& m) override {
+    _measurement = m;
+  }
+
+  virtual bool read(std::istream& is) override {
+    Eigen::Vector4d v;
+    is >> v(0) >> v(1) >> v(2) >> v(3);
+    setMeasurement(Plane3D(v));
+    for(int i = 0; i < information().rows(); ++i)
+      for(int j = i; j < information().cols(); ++j) {
+        is >> information()(i, j);
+        if(i != j) information()(j, i) = information()(i, j);
+      }
+    return true;
+  }
+  virtual bool write(std::ostream& os) const override {
+    Eigen::Vector4d v = _measurement.toVector();
+    os << v(0) << " " << v(1) << " " << v(2) << " " << v(3) << " ";
+    for(int i = 0; i < information().rows(); ++i)
+      for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j);
+    return os.good();
+  }
+};
+}  // namespace g2o
+
+#endif

include/g2o/edge_se3_priorquat.hpp

@@ -0,0 +1,78 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef KKL_G2O_EDGE_SE3_PRIORQUAT_HPP
+#define KKL_G2O_EDGE_SE3_PRIORQUAT_HPP
+
+#include <g2o/types/slam3d/types_slam3d.h>
+#include <g2o/types/slam3d_addons/types_slam3d_addons.h>
+
+namespace g2o {
+class EdgeSE3PriorQuat : public g2o::BaseUnaryEdge<3, Eigen::Quaterniond, g2o::VertexSE3> {
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  EdgeSE3PriorQuat() : g2o::BaseUnaryEdge<3, Eigen::Quaterniond, g2o::VertexSE3>() {}
+
+  void computeError() override {
+    const g2o::VertexSE3* v1 = static_cast<const g2o::VertexSE3*>(_vertices[0]);
+
+    Eigen::Quaterniond estimate = Eigen::Quaterniond(v1->estimate().linear());
+
+    if(_measurement.coeffs().dot(estimate.coeffs()) < 0.0) {
+      estimate.coeffs() = -estimate.coeffs();
+    }
+    _error = estimate.vec() - _measurement.vec();
+  }
+
+  void setMeasurement(const Eigen::Quaterniond& m) override {
+    _measurement = m;
+    if(m.w() < 0.0) {
+      _measurement.coeffs() = -m.coeffs();
+    }
+  }
+
+  virtual bool read(std::istream& is) override {
+    Eigen::Quaterniond q;
+    is >> q.w() >> q.x() >> q.y() >> q.z();
+    setMeasurement(q);
+    for(int i = 0; i < information().rows(); ++i)
+      for(int j = i; j < information().cols(); ++j) {
+        is >> information()(i, j);
+        if(i != j) information()(j, i) = information()(i, j);
+      }
+    return true;
+  }
+  virtual bool write(std::ostream& os) const override {
+    Eigen::Quaterniond q = _measurement;
+    os << q.w() << " " << q.x() << " " << q.y() << " " << q.z();
+    for(int i = 0; i < information().rows(); ++i)
+      for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j);
+    return os.good();
+  }
+};
+}  // namespace g2o
+
+#endif

include/g2o/edge_se3_priorvec.hpp

@@ -0,0 +1,76 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef KKL_G2O_EDGE_SE3_PRIORVEC_HPP
+#define KKL_G2O_EDGE_SE3_PRIORVEC_HPP
+
+#include <g2o/types/slam3d/types_slam3d.h>
+#include <g2o/types/slam3d_addons/types_slam3d_addons.h>
+
+namespace g2o {
+class EdgeSE3PriorVec : public g2o::BaseUnaryEdge<3, Eigen::Matrix<double, 6, 1>, g2o::VertexSE3> {
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  EdgeSE3PriorVec() : g2o::BaseUnaryEdge<3, Eigen::Matrix<double, 6, 1>, g2o::VertexSE3>() {}
+
+  void computeError() override {
+    const g2o::VertexSE3* v1 = static_cast<const g2o::VertexSE3*>(_vertices[0]);
+
+    Eigen::Vector3d direction = _measurement.head<3>();
+    Eigen::Vector3d measurement = _measurement.tail<3>();
+
+    Eigen::Vector3d estimate = (v1->estimate().linear().inverse() * direction);
+
+    _error = estimate - measurement;
+  }
+
+  void setMeasurement(const Eigen::Matrix<double, 6, 1>& m) override {
+    _measurement.head<3>() = m.head<3>().normalized();
+    _measurement.tail<3>() = m.tail<3>().normalized();
+  }
+
+  virtual bool read(std::istream& is) override {
+    Eigen::Matrix<double, 6, 1> v;
+    is >> v[0] >> v[1] >> v[2] >> v[3] >> v[4] >> v[5];
+    setMeasurement(v);
+    for(int i = 0; i < information().rows(); ++i)
+      for(int j = i; j < information().cols(); ++j) {
+        is >> information()(i, j);
+        if(i != j) information()(j, i) = information()(i, j);
+      }
+    return true;
+  }
+  virtual bool write(std::ostream& os) const override {
+    Eigen::Matrix<double, 6, 1> v = _measurement;
+    os << v[0] << " " << v[1] << " " << v[2] << " " << v[3] << " " << v[4] << " " << v[5];
+    for(int i = 0; i < information().rows(); ++i)
+      for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j);
+    return os.good();
+  }
+};
+}  // namespace g2o
+
+#endif

include/g2o/edge_se3_priorxy.hpp

@@ -0,0 +1,71 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef EDGE_SE3_PRIORXY_HPP
+#define EDGE_SE3_PRIORXY_HPP
+
+#include <g2o/types/slam3d/types_slam3d.h>
+#include <g2o/types/slam3d_addons/types_slam3d_addons.h>
+
+namespace g2o {
+class EdgeSE3PriorXY : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3> {
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  EdgeSE3PriorXY() : g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3>() {}
+
+  void computeError() override {
+    const g2o::VertexSE3* v1 = static_cast<const g2o::VertexSE3*>(_vertices[0]);
+
+    Eigen::Vector2d estimate = v1->estimate().translation().head<2>();
+    _error = estimate - _measurement;
+  }
+
+  void setMeasurement(const Eigen::Vector2d& m) override {
+    _measurement = m;
+  }
+
+  virtual bool read(std::istream& is) override {
+    Eigen::Vector2d v;
+    is >> v(0) >> v(1);
+    setMeasurement(v);
+    for(int i = 0; i < information().rows(); ++i)
+      for(int j = i; j < information().cols(); ++j) {
+        is >> information()(i, j);
+        if(i != j) information()(j, i) = information()(i, j);
+      }
+    return true;
+  }
+  virtual bool write(std::ostream& os) const override {
+    Eigen::Vector2d v = _measurement;
+    os << v(0) << " " << v(1) << " ";
+    for(int i = 0; i < information().rows(); ++i)
+      for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j);
+    return os.good();
+  }
+};
+}  // namespace g2o
+
+#endif  // EDGE_SE3_PRIORXY_HPP

include/g2o/edge_se3_priorxyz.hpp

@@ -0,0 +1,71 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef KKL_G2O_EDGE_SE3_PRIORXYZ_HPP
+#define KKL_G2O_EDGE_SE3_PRIORXYZ_HPP
+
+#include <g2o/types/slam3d/types_slam3d.h>
+#include <g2o/types/slam3d_addons/types_slam3d_addons.h>
+
+namespace g2o {
+class EdgeSE3PriorXYZ : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3> {
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  EdgeSE3PriorXYZ() : g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3>() {}
+
+  void computeError() override {
+    const g2o::VertexSE3* v1 = static_cast<const g2o::VertexSE3*>(_vertices[0]);
+
+    Eigen::Vector3d estimate = v1->estimate().translation();
+    _error = estimate - _measurement;
+  }
+
+  void setMeasurement(const Eigen::Vector3d& m) override {
+    _measurement = m;
+  }
+
+  virtual bool read(std::istream& is) override {
+    Eigen::Vector3d v;
+    is >> v(0) >> v(1) >> v(2);
+    setMeasurement(v);
+    for(int i = 0; i < information().rows(); ++i)
+      for(int j = i; j < information().cols(); ++j) {
+        is >> information()(i, j);
+        if(i != j) information()(j, i) = information()(i, j);
+      }
+    return true;
+  }
+  virtual bool write(std::ostream& os) const override {
+    Eigen::Vector3d v = _measurement;
+    os << v(0) << " " << v(1) << " " << v(2) << " ";
+    for(int i = 0; i < information().rows(); ++i)
+      for(int j = i; j < information().cols(); ++j) os << " " << information()(i, j);
+    return os.good();
+  }
+};
+}  // namespace g2o
+
+#endif

include/g2o/robust_kernel_io.hpp

@@ -0,0 +1,19 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#ifndef ROBUST_KERNEL_IO_HPP
+#define ROBUST_KERNEL_IO_HPP
+
+#include <string>
+#include <g2o/core/sparse_optimizer.h>
+
+namespace g2o {
+
+std::string kernel_type(g2o::RobustKernel* kernel);
+
+bool save_robust_kernels(const std::string& filename, g2o::SparseOptimizer* graph);
+
+bool load_robust_kernels(const std::string& filename, g2o::SparseOptimizer* graph);
+
+}  // namespace g2o
+
+#endif  // ROBUST_KERNEL_IO_HPP

include/hdl_graph_slam/graph_slam.hpp

@@ -0,0 +1,148 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#ifndef GRAPH_SLAM_HPP
+#define GRAPH_SLAM_HPP
+
+#include <memory>
+#include <ros/time.h>
+
+#include <g2o/core/hyper_graph.h>
+#include <Eigen/Dense>
+
+namespace g2o {
+class VertexSE3;
+class VertexPlane;
+class VertexPointXYZ;
+class EdgeSE3;
+class EdgeSE3Plane;
+class EdgeSE3PointXYZ;
+class EdgeSE3PriorXY;
+class EdgeSE3PriorXYZ;
+class EdgeSE3PriorVec;
+class EdgeSE3PriorQuat;
+class EdgePlane;
+class EdgePlaneIdentity;
+class EdgePlaneParallel;
+class EdgePlanePerpendicular;
+class EdgePlanePriorNormal;
+class EdgePlanePriorDistance;
+class RobustKernelFactory;
+}  // namespace g2o
+
+namespace hdl_graph_slam {
+
+class GraphSLAM {
+public:
+  GraphSLAM(const std::string& solver_type = "lm_var");
+  virtual ~GraphSLAM();
+
+  int num_vertices() const;
+  int num_edges() const;
+
+  void set_solver(const std::string& solver_type);
+
+  /**
+   * @brief add a SE3 node to the graph
+   * @param pose
+   * @return registered node
+   */
+  g2o::VertexSE3* add_se3_node(const Eigen::Isometry3d& pose);
+
+  /**
+   * @brief add a plane node to the graph
+   * @param plane_coeffs
+   * @return registered node
+   */
+  g2o::VertexPlane* add_plane_node(const Eigen::Vector4d& plane_coeffs);
+
+  /**
+   * @brief add a point_xyz node to the graph
+   * @param xyz
+   * @return registered node
+   */
+  g2o::VertexPointXYZ* add_point_xyz_node(const Eigen::Vector3d& xyz);
+
+  /**
+   * @brief add an edge between SE3 nodes
+   * @param v1  node1
+   * @param v2  node2
+   * @param relative_pose  relative pose between node1 and node2
+   * @param information_matrix  information matrix (it must be 6x6)
+   * @return registered edge
+   */
+  g2o::EdgeSE3* add_se3_edge(g2o::VertexSE3* v1, g2o::VertexSE3* v2, const Eigen::Isometry3d& relative_pose, const Eigen::MatrixXd& information_matrix);
+
+  /**
+   * @brief add an edge between an SE3 node and a plane node
+   * @param v_se3    SE3 node
+   * @param v_plane  plane node
+   * @param plane_coeffs  plane coefficients w.r.t. v_se3
+   * @param information_matrix  information matrix (it must be 3x3)
+   * @return registered edge
+   */
+  g2o::EdgeSE3Plane* add_se3_plane_edge(g2o::VertexSE3* v_se3, g2o::VertexPlane* v_plane, const Eigen::Vector4d& plane_coeffs, const Eigen::MatrixXd& information_matrix);
+
+  /**
+   * @brief add an edge between an SE3 node and a point_xyz node
+   * @param v_se3        SE3 node
+   * @param v_xyz        point_xyz node
+   * @param xyz          xyz coordinate
+   * @param information  information_matrix (it must be 3x3)
+   * @return registered edge
+   */
+  g2o::EdgeSE3PointXYZ* add_se3_point_xyz_edge(g2o::VertexSE3* v_se3, g2o::VertexPointXYZ* v_xyz, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix);
+
+  /**
+   * @brief add a prior edge to an SE3 node
+   * @param v_se3
+   * @param xy
+   * @param information_matrix
+   * @return
+   */
+  g2o::EdgePlanePriorNormal* add_plane_normal_prior_edge(g2o::VertexPlane* v, const Eigen::Vector3d& normal, const Eigen::MatrixXd& information_matrix);
+
+  g2o::EdgePlanePriorDistance* add_plane_distance_prior_edge(g2o::VertexPlane* v, double distance, const Eigen::MatrixXd& information_matrix);
+
+  g2o::EdgeSE3PriorXY* add_se3_prior_xy_edge(g2o::VertexSE3* v_se3, const Eigen::Vector2d& xy, const Eigen::MatrixXd& information_matrix);
+
+  g2o::EdgeSE3PriorXYZ* add_se3_prior_xyz_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix);
+
+  g2o::EdgeSE3PriorQuat* add_se3_prior_quat_edge(g2o::VertexSE3* v_se3, const Eigen::Quaterniond& quat, const Eigen::MatrixXd& information_matrix);
+
+  g2o::EdgeSE3PriorVec* add_se3_prior_vec_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& direction, const Eigen::Vector3d& measurement, const Eigen::MatrixXd& information_matrix);
+
+  g2o::EdgePlane* add_plane_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector4d& measurement, const Eigen::Matrix4d& information);
+
+  g2o::EdgePlaneIdentity* add_plane_identity_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector4d& measurement, const Eigen::Matrix4d& information);
+
+  g2o::EdgePlaneParallel* add_plane_parallel_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector3d& measurement, const Eigen::Matrix3d& information);
+
+  g2o::EdgePlanePerpendicular* add_plane_perpendicular_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector3d& measurement, const Eigen::MatrixXd& information);
+
+  void add_robust_kernel(g2o::HyperGraph::Edge* edge, const std::string& kernel_type, double kernel_size);
+
+  /**
+   * @brief perform graph optimization
+   */
+  int optimize(int num_iterations);
+
+  /**
+   * @brief save the pose graph to a file
+   * @param filename  output filename
+   */
+  void save(const std::string& filename);
+
+  /**
+   * @brief load the pose graph from file
+   * @param filename  output filename
+   */
+  bool load(const std::string& filename);
+
+public:
+  g2o::RobustKernelFactory* robust_kernel_factory;
+  std::unique_ptr<g2o::HyperGraph> graph;  // g2o graph
+};
+
+}  // namespace hdl_graph_slam
+
+#endif  // GRAPH_SLAM_HPP

include/hdl_graph_slam/information_matrix_calculator.hpp

@@ -0,0 +1,59 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#ifndef INFORMATION_MATRIX_CALCULATOR_HPP
+#define INFORMATION_MATRIX_CALCULATOR_HPP
+
+#include <ros/ros.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+namespace hdl_graph_slam {
+
+class InformationMatrixCalculator {
+public:
+  using PointT = pcl::PointXYZI;
+
+  InformationMatrixCalculator() {}
+  InformationMatrixCalculator(ros::NodeHandle& nh);
+  ~InformationMatrixCalculator();
+
+  template<typename ParamServer>
+  void load(ParamServer& params) {
+    use_const_inf_matrix = params.template param<bool>("use_const_inf_matrix", false);
+    const_stddev_x = params.template param<double>("const_stddev_x", 0.5);
+    const_stddev_q = params.template param<double>("const_stddev_q", 0.1);
+
+    var_gain_a = params.template param<double>("var_gain_a", 20.0);
+    min_stddev_x = params.template param<double>("min_stddev_x", 0.1);
+    max_stddev_x = params.template param<double>("max_stddev_x", 5.0);
+    min_stddev_q = params.template param<double>("min_stddev_q", 0.05);
+    max_stddev_q = params.template param<double>("max_stddev_q", 0.2);
+    fitness_score_thresh = params.template param<double>("fitness_score_thresh", 2.5);
+  }
+
+  static double calc_fitness_score(const pcl::PointCloud<PointT>::ConstPtr& cloud1, const pcl::PointCloud<PointT>::ConstPtr& cloud2, const Eigen::Isometry3d& relpose, double max_range = std::numeric_limits<double>::max());
+
+  Eigen::MatrixXd calc_information_matrix(const pcl::PointCloud<PointT>::ConstPtr& cloud1, const pcl::PointCloud<PointT>::ConstPtr& cloud2, const Eigen::Isometry3d& relpose) const;
+
+private:
+  double weight(double a, double max_x, double min_y, double max_y, double x) const {
+    double y = (1.0 - std::exp(-a * x)) / (1.0 - std::exp(-a * max_x));
+    return min_y + (max_y - min_y) * y;
+  }
+
+private:
+  bool use_const_inf_matrix;
+  double const_stddev_x;
+  double const_stddev_q;
+
+  double var_gain_a;
+  double min_stddev_x;
+  double max_stddev_x;
+  double min_stddev_q;
+  double max_stddev_q;
+  double fitness_score_thresh;
+};
+
+}  // namespace hdl_graph_slam
+
+#endif  // INFORMATION_MATRIX_CALCULATOR_HPP

include/hdl_graph_slam/keyframe.hpp

@@ -0,0 +1,74 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#ifndef KEYFRAME_HPP
+#define KEYFRAME_HPP
+
+#include <ros/ros.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <boost/optional.hpp>
+
+namespace g2o {
+class VertexSE3;
+class HyperGraph;
+class SparseOptimizer;
+}  // namespace g2o
+
+namespace hdl_graph_slam {
+
+/**
+ * @brief KeyFrame (pose node)
+ */
+struct KeyFrame {
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  using PointT = pcl::PointXYZI;
+  using Ptr = std::shared_ptr<KeyFrame>;
+
+  KeyFrame(const ros::Time& stamp, const Eigen::Isometry3d& odom, double accum_distance, const pcl::PointCloud<PointT>::ConstPtr& cloud);
+  KeyFrame(const std::string& directory, g2o::HyperGraph* graph);
+  virtual ~KeyFrame();
+
+  void save(const std::string& directory);
+  bool load(const std::string& directory, g2o::HyperGraph* graph);
+
+  long id() const;
+  Eigen::Isometry3d estimate() const;
+
+public:
+  ros::Time stamp;                                // timestamp
+  Eigen::Isometry3d odom;                         // odometry (estimated by scan_matching_odometry)
+  double accum_distance;                          // accumulated distance from the first node (by scan_matching_odometry)
+  pcl::PointCloud<PointT>::ConstPtr cloud;        // point cloud
+  boost::optional<Eigen::Vector4d> floor_coeffs;  // detected floor's coefficients
+  boost::optional<Eigen::Vector3d> utm_coord;     // UTM coord obtained by GPS
+
+  boost::optional<Eigen::Vector3d> acceleration;    //
+  boost::optional<Eigen::Quaterniond> orientation;  //
+
+  g2o::VertexSE3* node;  // node instance
+};
+
+/**
+ * @brief KeyFramesnapshot for map cloud generation
+ */
+struct KeyFrameSnapshot {
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  using PointT = KeyFrame::PointT;
+  using Ptr = std::shared_ptr<KeyFrameSnapshot>;
+
+  KeyFrameSnapshot(const KeyFrame::Ptr& key);
+  KeyFrameSnapshot(const Eigen::Isometry3d& pose, const pcl::PointCloud<PointT>::ConstPtr& cloud);
+
+  ~KeyFrameSnapshot();
+
+public:
+  Eigen::Isometry3d pose;                   // pose estimated by graph optimization
+  pcl::PointCloud<PointT>::ConstPtr cloud;  // point cloud
+};
+
+}  // namespace hdl_graph_slam
+
+#endif  // KEYFRAME_HPP

include/hdl_graph_slam/keyframe_updater.hpp

@@ -0,0 +1,77 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#ifndef KEYFRAME_UPDATER_HPP
+#define KEYFRAME_UPDATER_HPP
+
+#include <ros/ros.h>
+#include <Eigen/Dense>
+
+namespace hdl_graph_slam {
+
+/**
+ * @brief this class decides if a new frame should be registered to the pose graph as a keyframe
+ */
+class KeyframeUpdater {
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /**
+   * @brief constructor
+   * @param pnh
+   */
+  KeyframeUpdater(ros::NodeHandle& pnh) : is_first(true), prev_keypose(Eigen::Isometry3d::Identity()) {
+    keyframe_delta_trans = pnh.param<double>("keyframe_delta_trans", 2.0);
+    keyframe_delta_angle = pnh.param<double>("keyframe_delta_angle", 2.0);
+
+    accum_distance = 0.0;
+  }
+
+  /**
+   * @brief decide if a new frame should be registered to the graph
+   * @param pose  pose of the frame
+   * @return  if true, the frame should be registered
+   */
+  bool update(const Eigen::Isometry3d& pose) {
+    // first frame is always registered to the graph
+    if(is_first) {
+      is_first = false;
+      prev_keypose = pose;
+      return true;
+    }
+
+    // calculate the delta transformation from the previous keyframe
+    Eigen::Isometry3d delta = prev_keypose.inverse() * pose;
+    double dx = delta.translation().norm();
+    double da = Eigen::AngleAxisd(delta.linear()).angle();
+
+    // too close to the previous frame
+    if(dx < keyframe_delta_trans && da < keyframe_delta_angle) {
+      return false;
+    }
+
+    accum_distance += dx;
+    prev_keypose = pose;
+    return true;
+  }
+
+  /**
+   * @brief the last keyframe's accumulated distance from the first keyframe
+   * @return accumulated distance
+   */
+  double get_accum_distance() const {
+    return accum_distance;
+  }
+
+private:
+  // parameters
+  double keyframe_delta_trans;  //
+  double keyframe_delta_angle;  //
+
+  bool is_first;
+  double accum_distance;
+  Eigen::Isometry3d prev_keypose;
+};
+
+}  // namespace hdl_graph_slam
+
+#endif  // KEYFRAME_UPDATOR_HPP

include/hdl_graph_slam/loop_detector.hpp

@@ -0,0 +1,188 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#ifndef LOOP_DETECTOR_HPP
+#define LOOP_DETECTOR_HPP
+
+#include <boost/format.hpp>
+#include <hdl_graph_slam/keyframe.hpp>
+#include <hdl_graph_slam/registrations.hpp>
+#include <hdl_graph_slam/graph_slam.hpp>
+
+#include <g2o/types/slam3d/vertex_se3.h>
+
+namespace hdl_graph_slam {
+
+struct Loop {
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  using Ptr = std::shared_ptr<Loop>;
+
+  Loop(const KeyFrame::Ptr& key1, const KeyFrame::Ptr& key2, const Eigen::Matrix4f& relpose) : key1(key1), key2(key2), relative_pose(relpose) {}
+
+public:
+  KeyFrame::Ptr key1;
+  KeyFrame::Ptr key2;
+  Eigen::Matrix4f relative_pose;
+};
+
+/**
+ * @brief this class finds loops by scam matching and adds them to the pose graph
+ */
+class LoopDetector {
+public:
+  typedef pcl::PointXYZI PointT;
+
+  /**
+   * @brief constructor
+   * @param pnh
+   */
+  LoopDetector(ros::NodeHandle& pnh) {
+    distance_thresh = pnh.param<double>("distance_thresh", 5.0);
+    accum_distance_thresh = pnh.param<double>("accum_distance_thresh", 8.0);
+    distance_from_last_edge_thresh = pnh.param<double>("min_edge_interval", 5.0);
+
+    fitness_score_max_range = pnh.param<double>("fitness_score_max_range", std::numeric_limits<double>::max());
+    fitness_score_thresh = pnh.param<double>("fitness_score_thresh", 0.5);
+
+    registration = select_registration_method(pnh);
+    last_edge_accum_distance = 0.0;
+  }
+
+  /**
+   * @brief detect loops and add them to the pose graph
+   * @param keyframes       keyframes
+   * @param new_keyframes   newly registered keyframes
+   * @param graph_slam      pose graph
+   */
+  std::vector<Loop::Ptr> detect(const std::vector<KeyFrame::Ptr>& keyframes, const std::deque<KeyFrame::Ptr>& new_keyframes, hdl_graph_slam::GraphSLAM& graph_slam) {
+    std::vector<Loop::Ptr> detected_loops;
+    for(const auto& new_keyframe : new_keyframes) {
+      auto candidates = find_candidates(keyframes, new_keyframe);
+      auto loop = matching(candidates, new_keyframe, graph_slam);
+      if(loop) {
+        detected_loops.push_back(loop);
+      }
+    }
+
+    return detected_loops;
+  }
+
+  double get_distance_thresh() const {
+    return distance_thresh;
+  }
+
+private:
+  /**
+   * @brief find loop candidates. A detected loop begins at one of #keyframes and ends at #new_keyframe
+   * @param keyframes      candidate keyframes of loop start
+   * @param new_keyframe   loop end keyframe
+   * @return loop candidates
+   */
+  std::vector<KeyFrame::Ptr> find_candidates(const std::vector<KeyFrame::Ptr>& keyframes, const KeyFrame::Ptr& new_keyframe) const {
+    // too close to the last registered loop edge
+    if(new_keyframe->accum_distance - last_edge_accum_distance < distance_from_last_edge_thresh) {
+      return std::vector<KeyFrame::Ptr>();
+    }
+
+    std::vector<KeyFrame::Ptr> candidates;
+    candidates.reserve(32);
+
+    for(const auto& k : keyframes) {
+      // traveled distance between keyframes is too small
+      if(new_keyframe->accum_distance - k->accum_distance < accum_distance_thresh) {
+        continue;
+      }
+
+      const auto& pos1 = k->node->estimate().translation();
+      const auto& pos2 = new_keyframe->node->estimate().translation();
+
+      // estimated distance between keyframes is too small
+      double dist = (pos1.head<2>() - pos2.head<2>()).norm();
+      if(dist > distance_thresh) {
+        continue;
+      }
+
+      candidates.push_back(k);
+    }
+
+    return candidates;
+  }
+
+  /**
+   * @brief To validate a loop candidate this function applies a scan matching between keyframes consisting the loop. If they are matched well, the loop is added to the pose graph
+   * @param candidate_keyframes  candidate keyframes of loop start
+   * @param new_keyframe         loop end keyframe
+   * @param graph_slam           graph slam
+   */
+  Loop::Ptr matching(const std::vector<KeyFrame::Ptr>& candidate_keyframes, const KeyFrame::Ptr& new_keyframe, hdl_graph_slam::GraphSLAM& graph_slam) {
+    if(candidate_keyframes.empty()) {
+      return nullptr;
+    }
+
+    registration->setInputTarget(new_keyframe->cloud);
+
+    double best_score = std::numeric_limits<double>::max();
+    KeyFrame::Ptr best_matched;
+    Eigen::Matrix4f relative_pose;
+
+    std::cout << std::endl;
+    std::cout << "--- loop detection ---" << std::endl;
+    std::cout << "num_candidates: " << candidate_keyframes.size() << std::endl;
+    std::cout << "matching" << std::flush;
+    auto t1 = ros::Time::now();
+
+    pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
+    for(const auto& candidate : candidate_keyframes) {
+      registration->setInputSource(candidate->cloud);
+      Eigen::Isometry3d new_keyframe_estimate = new_keyframe->node->estimate();
+      new_keyframe_estimate.linear() = Eigen::Quaterniond(new_keyframe_estimate.linear()).normalized().toRotationMatrix();
+      Eigen::Isometry3d candidate_estimate = candidate->node->estimate();
+      candidate_estimate.linear() = Eigen::Quaterniond(candidate_estimate.linear()).normalized().toRotationMatrix();
+      Eigen::Matrix4f guess = (new_keyframe_estimate.inverse() * candidate_estimate).matrix().cast<float>();
+      guess(2, 3) = 0.0;
+      registration->align(*aligned, guess);
+      std::cout << "." << std::flush;
+
+      double score = registration->getFitnessScore(fitness_score_max_range);
+      if(!registration->hasConverged() || score > best_score) {
+        continue;
+      }
+
+      best_score = score;
+      best_matched = candidate;
+      relative_pose = registration->getFinalTransformation();
+    }
+
+    auto t2 = ros::Time::now();
+    std::cout << " done" << std::endl;
+    std::cout << "best_score: " << boost::format("%.3f") % best_score << "    time: " << boost::format("%.3f") % (t2 - t1).toSec() << "[sec]" << std::endl;
+
+    if(best_score > fitness_score_thresh) {
+      std::cout << "loop not found..." << std::endl;
+      return nullptr;
+    }
+
+    std::cout << "loop found!!" << std::endl;
+    std::cout << "relpose: " << relative_pose.block<3, 1>(0, 3) << " - " << Eigen::Quaternionf(relative_pose.block<3, 3>(0, 0)).coeffs().transpose() << std::endl;
+
+    last_edge_accum_distance = new_keyframe->accum_distance;
+
+    return std::make_shared<Loop>(new_keyframe, best_matched, relative_pose);
+  }
+
+private:
+  double distance_thresh;                 // estimated distance between keyframes consisting a loop must be less than this distance
+  double accum_distance_thresh;           // traveled distance between ...
+  double distance_from_last_edge_thresh;  // a new loop edge must far from the last one at least this distance
+
+  double fitness_score_max_range;  // maximum allowable distance between corresponding points
+  double fitness_score_thresh;     // threshold for scan matching
+
+  double last_edge_accum_distance;
+
+  pcl::Registration<PointT, PointT>::Ptr registration;
+};
+
+}  // namespace hdl_graph_slam
+
+#endif  // LOOP_DETECTOR_HPP

include/hdl_graph_slam/map_cloud_generator.hpp

@@ -0,0 +1,34 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#ifndef MAP_CLOUD_GENERATOR_HPP
+#define MAP_CLOUD_GENERATOR_HPP
+
+#include <vector>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <hdl_graph_slam/keyframe.hpp>
+
+namespace hdl_graph_slam {
+
+/**
+ * @brief this class generates a map point cloud from registered keyframes
+ */
+class MapCloudGenerator {
+public:
+  using PointT = pcl::PointXYZI;
+
+  MapCloudGenerator();
+  ~MapCloudGenerator();
+
+  /**
+   * @brief generates a map point cloud
+   * @param keyframes   snapshots of keyframes
+   * @param resolution  resolution of generated map
+   * @return generated map point cloud
+   */
+  pcl::PointCloud<PointT>::Ptr generate(const std::vector<KeyFrameSnapshot::Ptr>& keyframes, double resolution) const;
+};
+
+}  // namespace hdl_graph_slam
+
+#endif  // MAP_POINTCLOUD_GENERATOR_HPP

include/hdl_graph_slam/nmea_sentence_parser.hpp

@@ -0,0 +1,108 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#ifndef NMEA_SENTENCE_PARSER_HPP
+#define NMEA_SENTENCE_PARSER_HPP
+
+#include <cmath>
+#include <string>
+#include <vector>
+#include <numeric>
+#include <boost/algorithm/string.hpp>
+
+namespace hdl_graph_slam {
+
+struct GPRMC {
+public:
+  GPRMC() {
+    status = 'V';
+  }
+
+  GPRMC(const std::vector<std::string>& tokens) {
+    if(tokens[0] != "$GPRMC" || tokens.size() < 12) {
+      status = 'V';
+      return;
+    }
+
+    long time = std::stol(tokens[1]);
+    hour = time / 10000;
+    minute = (time % 10000) / 100;
+    second = time % 100;
+
+    status = tokens[2][0];
+
+    latitude = degmin2deg(std::stod(tokens[3]));
+    latitude = tokens[4] == "N" ? latitude : -latitude;
+
+    longitude = degmin2deg(std::stod(tokens[5]));
+    longitude = tokens[6] == "E" ? longitude : -longitude;
+
+    speed_knots = std::stod(tokens[7]);
+    track_angle_degree = std::stod(tokens[8]);
+
+    long date = std::stol(tokens[9]);
+    year = date % 100;
+    month = (date / 100) % 100;
+    day = (date / 10000) % 100;
+
+    magnetic_variation = std::stod(tokens[10]);
+    magnetic_variation = tokens[11][0] == 'E' ? magnetic_variation : -magnetic_variation;
+  }
+
+  double degmin2deg(double degmin) {
+    double d = std::floor(degmin / 100.0);
+    double m = (degmin - d * 100.0) / 60.0;
+    return d + m;
+  }
+
+public:
+  char status;  // Status A=active or V=Void.
+
+  int hour;  // Fix taken at 12:35:19 UTC
+  int minute;
+  int second;
+
+  double latitude;  //
+  double longitude;
+
+  double speed_knots;         // Speed over the ground in knots
+  double track_angle_degree;  // Track angle in degrees True
+
+  int year;
+  int month;
+  int day;
+
+  double magnetic_variation;
+};
+
+class NmeaSentenceParser {
+public:
+  NmeaSentenceParser() {}
+  ~NmeaSentenceParser() {}
+
+  GPRMC parse(const std::string& sentence) const {
+    int checksum_loc = sentence.find('*');
+    if(checksum_loc == std::string::npos) {
+      return GPRMC();
+    }
+
+    int checksum = std::stoul(sentence.substr(checksum_loc + 1), nullptr, 16);
+
+    std::string substr = sentence.substr(1, checksum_loc - 1);
+    int sum = std::accumulate(substr.begin(), substr.end(), static_cast<unsigned char>(0), [=](unsigned char n, unsigned char c) { return n ^ c; });
+
+    if(checksum != (sum & 0xf)) {
+      std::cerr << "checksum doesn't match!!" << std::endl;
+      std::cerr << sentence << " " << sum << std::endl;
+      return GPRMC();
+    }
+
+    std::vector<std::string> tokens;
+    boost::split(tokens, sentence, boost::is_any_of(","));
+
+    return GPRMC(tokens);
+  }
+};
+
+}  // namespace hdl_graph_slam
+
+#endif  // NMEA_SENTENCE_PARSER_HPP

include/hdl_graph_slam/registrations.hpp

@@ -0,0 +1,21 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#ifndef HDL_GRAPH_SLAM_REGISTRATIONS_HPP
+#define HDL_GRAPH_SLAM_REGISTRATIONS_HPP
+
+#include <ros/ros.h>
+
+#include <pcl/registration/registration.h>
+
+namespace hdl_graph_slam {
+
+/**
+ * @brief select a scan matching algorithm according to rosparams
+ * @param pnh
+ * @return selected scan matching
+ */
+pcl::Registration<pcl::PointXYZI, pcl::PointXYZI>::Ptr select_registration_method(ros::NodeHandle& pnh);
+
+}  // namespace hdl_graph_slam
+
+#endif  //

include/hdl_graph_slam/ros_time_hash.hpp

@@ -0,0 +1,24 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#ifndef ROS_TIME_HASH_HPP
+#define ROS_TIME_HASH_HPP
+
+#include <unordered_map>
+#include <boost/functional/hash.hpp>
+
+#include <ros/time.h>
+
+/**
+ * @brief Hash calculation for ros::Time
+ */
+class RosTimeHash {
+public:
+  size_t operator()(const ros::Time& val) const {
+    size_t seed = 0;
+    boost::hash_combine(seed, val.sec);
+    boost::hash_combine(seed, val.nsec);
+    return seed;
+  }
+};
+
+#endif  // ROS_TIME_HASHER_HPP

include/hdl_graph_slam/ros_utils.hpp

@@ -0,0 +1,93 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#ifndef ROS_UTILS_HPP
+#define ROS_UTILS_HPP
+
+#include <Eigen/Dense>
+
+#include <ros/ros.h>
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/TransformStamped.h>
+
+namespace hdl_graph_slam {
+
+/**
+ * @brief convert Eigen::Matrix to geometry_msgs::TransformStamped
+ * @param stamp            timestamp
+ * @param pose             Eigen::Matrix to be converted
+ * @param frame_id         tf frame_id
+ * @param child_frame_id   tf child frame_id
+ * @return converted TransformStamped
+ */
+static geometry_msgs::TransformStamped matrix2transform(const ros::Time& stamp, const Eigen::Matrix4f& pose, const std::string& frame_id, const std::string& child_frame_id) {
+  Eigen::Quaternionf quat(pose.block<3, 3>(0, 0));
+  quat.normalize();
+  geometry_msgs::Quaternion odom_quat;
+  odom_quat.w = quat.w();
+  odom_quat.x = quat.x();
+  odom_quat.y = quat.y();
+  odom_quat.z = quat.z();
+
+  geometry_msgs::TransformStamped odom_trans;
+  odom_trans.header.stamp = stamp;
+  odom_trans.header.frame_id = frame_id;
+  odom_trans.child_frame_id = child_frame_id;
+
+  odom_trans.transform.translation.x = pose(0, 3);
+  odom_trans.transform.translation.y = pose(1, 3);
+  odom_trans.transform.translation.z = pose(2, 3);
+  odom_trans.transform.rotation = odom_quat;
+
+  return odom_trans;
+}
+
+static Eigen::Isometry3d pose2isometry(const geometry_msgs::Pose& pose) {
+  Eigen::Isometry3d mat = Eigen::Isometry3d::Identity();
+  mat.translation() = Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z);
+  mat.linear() = Eigen::Quaterniond(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z).toRotationMatrix();
+  return mat;
+}
+
+static Eigen::Isometry3d tf2isometry(const tf::StampedTransform& trans) {
+  Eigen::Isometry3d mat = Eigen::Isometry3d::Identity();
+  mat.translation() = Eigen::Vector3d(trans.getOrigin().x(), trans.getOrigin().y(), trans.getOrigin().z());
+  mat.linear() = Eigen::Quaterniond(trans.getRotation().w(), trans.getRotation().x(), trans.getRotation().y(), trans.getRotation().z()).toRotationMatrix();
+  return mat;
+}
+
+static geometry_msgs::Pose isometry2pose(const Eigen::Isometry3d& mat) {
+  Eigen::Quaterniond quat(mat.linear());
+  Eigen::Vector3d trans = mat.translation();
+
+  geometry_msgs::Pose pose;
+  pose.position.x = trans.x();
+  pose.position.y = trans.y();
+  pose.position.z = trans.z();
+  pose.orientation.w = quat.w();
+  pose.orientation.x = quat.x();
+  pose.orientation.y = quat.y();
+  pose.orientation.z = quat.z();
+
+  return pose;
+}
+
+static Eigen::Isometry3d odom2isometry(const nav_msgs::OdometryConstPtr& odom_msg) {
+  const auto& orientation = odom_msg->pose.pose.orientation;
+  const auto& position = odom_msg->pose.pose.position;
+
+  Eigen::Quaterniond quat;
+  quat.w() = orientation.w;
+  quat.x() = orientation.x;
+  quat.y() = orientation.y;
+  quat.z() = orientation.z;
+
+  Eigen::Isometry3d isometry = Eigen::Isometry3d::Identity();
+  isometry.linear() = quat.toRotationMatrix();
+  isometry.translation() = Eigen::Vector3d(position.x, position.y, position.z);
+  return isometry;
+}
+
+}  // namespace hdl_graph_slam
+
+#endif  // ROS_UTILS_HPP

include/laser_odom.hpp

@@ -0,0 +1,83 @@
+#include <memory>
+#include <iostream>
+
+#include <ros/ros.h>
+#include <ros/time.h>
+#include <ros/duration.h>
+#include <pcl_ros/point_cloud.h>
+#include <tf_conversions/tf_eigen.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+
+#include <std_msgs/Time.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/filters/approximate_voxel_grid.h>
+
+#include <hdl_graph_slam/ros_utils.hpp>
+#include <hdl_graph_slam/registrations.hpp>
+#include <hdl_graph_slam/ScanMatchingStatus.h>
+
+#include <laser_geometry/laser_geometry.h>
+#include <fast_gicp/gicp/fast_gicp.hpp>
+
+#include <new>
+#undef new
+
+// namespace laser_odom_ns {
+typedef pcl::PointXYZI PointT;
+
+class Laser_Odom {
+private:
+  laser_geometry::LaserProjection projector;
+  sensor_msgs::PointCloud2 cloud2;             // 点云数据类型转换的中间变量
+  pcl::PointCloud<PointT>::Ptr cloud;          // 点云数据
+  pcl::PointCloud<PointT>::ConstPtr keyframe;  // 关键帧
+  ros::Time prev_time;
+  Eigen::Matrix4f prev_trans;     // 地图起点到当前   帧  的放射变换
+  Eigen::Matrix4f keyframe_pose;  // 地图起点到当前 关键帧 的仿射变换
+  ros::Time keyframe_stamp;       // 关键帧的时间戳
+  // pcl::Registration<PointT, PointT>::Ptr registration;
+  fast_gicp::FastGICP<PointT, PointT>::Ptr registration;
+  pcl::Filter<PointT>::Ptr downsample_filter;
+
+  double keyframe_delta_trans = 0.25;
+  double keyframe_delta_angle = 0.15;
+  double keyframe_delta_time = 1.0;
+  // std::string odom_frame_id = "odom";
+  tf::TransformBroadcaster keyframe_broadcaster;
+  tf::TransformBroadcaster odom_broadcaster;
+
+  ros::NodeHandle nh;
+  ros::Publisher odom_pub;      // 里程计发布
+  // ros::Publisher read_until_pub;  //
+  ros::Publisher aligned_points_pub;
+  ros::Publisher map_points_pub;
+
+  static Laser_Odom* laser_odom;
+  Eigen::Matrix4f odom;
+
+  double downsample_resolution = 0.1;  // 下采样分辨率
+
+  ros::NodeHandle private_nh;
+
+public:
+  Laser_Odom();
+  ~Laser_Odom();
+  void init();
+  static void cloud_callback(const sensor_msgs::LaserScan::ConstPtr& laser_scan_msg);
+  Eigen::Matrix4f matching(const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud);
+  pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud);
+  geometry_msgs::TransformStamped matrix2transform(const ros::Time& stamp, const Eigen::Matrix4f& pose, const std::string& frame_id, const std::string& child_frame_id);
+  void publish_odometry(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose);
+  void publish_keyfrrame(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose);  // 发布里程计数据
+};
+// };  // namespace laser_odom_ns

launch/hdl_graph_slam.launch

@@ -0,0 +1,176 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- arguments -->
+  <arg name="nodelet_manager" default="velodyne_nodelet_manager" />
+  <arg name="enable_floor_detection" default="false" />
+  <arg name="enable_gps" default="false" />
+  <arg name="enable_imu_acc" default="false" />
+  <arg name="enable_imu_ori" default="false" />
+
+  <!-- hdl_graph_slam will publish its estimated pose from odom to base_link within the following structure -->
+  <!-- ***** map -> odom -> base_link -> velodyne ***** -->
+  <!-- these can be named in the following -->
+  <!-- topic for hdl to find velodyne points -->
+  <arg name="points_topic" default="/velodyne_points" />
+  <!-- what should the map frame be called -->
+  <arg name="map_frame_id" default="map" />
+  <!-- what should the base link frame be called -->
+  <arg name="base_link_frame_id" default="base_link" />
+  <!-- what should the odom frame be called -->
+  <arg name="lidar_odom_frame_id" default="odom" />
+
+  <!-- hdl_graph_slam will also publish an odom message -->
+  <!-- what should the topic be for odom messages published by hdl-graph-slam -->
+  <arg name="published_odom_topic" default="/odom" />
+
+  <!-- optional arguments -->
+  <arg name="enable_robot_odometry_init_guess" default="false" />
+  <arg name="robot_odom_frame_id" default="robot_odom" />
+
+  <!-- transformation between lidar and base_link -->
+  <node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="0 0 0 0 0 0 $(arg base_link_frame_id) velodyne 10" />
+
+  <!-- in case you use velodyne_driver, comment out the following line -->
+   <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen"/> 
+
+  <!-- prefiltering_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="prefiltering_nodelet" args="load hdl_graph_slam/PrefilteringNodelet $(arg nodelet_manager)">
+    <remap from="/velodyne_points" to="$(arg points_topic)" />
+    <!-- in case base_link_frame is blank, mapping will be performed in the lidar frame -->
+    <param name="base_link_frame" value="$(arg base_link_frame_id)" />
+    <!-- distance filter -->
+    <param name="use_distance_filter" value="false" />
+    <param name="distance_near_thresh" value="0.1" />
+    <param name="distance_far_thresh" value="1000.0" />
+    <!-- NONE, VOXELGRID, or APPROX_VOXELGRID -->
+    <param name="downsample_method" value="VOXELGRID" />
+    <param name="downsample_resolution" value="0.1" />
+    <!-- NONE, RADIUS, or STATISTICAL -->
+    <param name="outlier_removal_method" value="NONE" />
+    <param name="statistical_mean_k" value="30" />
+    <param name="statistical_stddev" value="1.2" />
+    <param name="radius_radius" value="0.5" />
+    <param name="radius_min_neighbors" value="2" />
+  </node>
+
+  <!-- scan_matching_odometry_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="scan_matching_odometry_nodelet" args="load hdl_graph_slam/ScanMatchingOdometryNodelet $(arg nodelet_manager)">
+    <param name="points_topic" value="$(arg points_topic)" />
+    <!-- published_odom_topic -->
+    <param name="published_odom_topic" value="$(arg published_odom_topic)" />
+    <param name="odom_frame_id" value="$(arg lidar_odom_frame_id)" />
+    <param name="robot_odom_frame_id" value="$(arg robot_odom_frame_id)" />
+    <param name="keyframe_delta_trans" value="1.0" />
+    <param name="keyframe_delta_angle" value="1.0" />
+    <param name="keyframe_delta_time" value="10000.0" />
+    <param name="transform_thresholding" value="false" />
+    <param name="enable_robot_odometry_init_guess" value="$(arg enable_robot_odometry_init_guess)" />
+    <param name="max_acceptable_trans" value="1.0" />
+    <param name="max_acceptable_angle" value="1.0" />
+    <param name="downsample_method" value="NONE" />
+    <param name="downsample_resolution" value="0.1" />
+    <!-- ICP, GICP, NDT, GICP_OMP, NDT_OMP, FAST_GICP(recommended), or FAST_VGICP -->
+    <param name="registration_method" value="FAST_GICP" />
+    <param name="reg_num_threads" value="0" />
+    <param name="reg_transformation_epsilon" value="0.01"/>
+    <param name="reg_maximum_iterations" value="64"/>
+    <param name="reg_max_correspondence_distance" value="2.5"/>
+    <param name="reg_max_optimizer_iterations" value="20"/>
+    <param name="reg_use_reciprocal_correspondences" value="false"/>
+    <param name="reg_correspondence_randomness" value="20"/>
+    <param name="reg_resolution" value="1.0" />
+    <param name="reg_nn_search_method" value="DIRECT7" />
+  </node>
+
+  <!-- floor_detection_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="floor_detection_nodelet" args="load hdl_graph_slam/FloorDetectionNodelet $(arg nodelet_manager)" if="$(arg enable_floor_detection)">
+    <param name="points_topic" value="$(arg points_topic)" />
+    <param name="tilt_deg" value="0.0" />
+    <param name="sensor_height" value="2.0" />
+    <param name="height_clip_range" value="1.0" />
+    <param name="floor_pts_thresh" value="512" />
+    <param name="use_normal_filtering" value="true" />
+    <param name="normal_filter_thresh" value="20.0" />
+  </node>
+
+  <!-- hdl_graph_slam_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="hdl_graph_slam_nodelet" args="load hdl_graph_slam/HdlGraphSlamNodelet $(arg nodelet_manager)">
+    <param name="points_topic" value="$(arg points_topic)" />
+    <!-- published_odom_topic -->
+    <param name="published_odom_topic" value="$(arg published_odom_topic)" />
+    <!-- frame settings -->
+    <param name="map_frame_id" value="$(arg map_frame_id)" />
+    <param name="odom_frame_id" value="$(arg lidar_odom_frame_id)" />
+    <!-- optimization params -->
+    <!-- typical solvers: gn_var, gn_fix6_3, gn_var_cholmod, lm_var, lm_fix6_3, lm_var_cholmod, ... -->
+    <param name="g2o_solver_type" value="lm_var_cholmod" />
+    <param name="g2o_solver_num_iterations" value="512" />
+    <!-- constraint switches -->
+    <param name="enable_gps" value="$(arg enable_gps)" />
+    <param name="enable_imu_acceleration" value="$(arg enable_imu_acc)" />
+    <param name="enable_imu_orientation" value="$(arg enable_imu_ori)" />
+    <!-- keyframe registration params -->
+    <param name="max_keyframes_per_update" value="10" />
+    <param name="keyframe_delta_trans" value="2.0" />
+    <param name="keyframe_delta_angle" value="2.0" />
+    <!-- fix first node for optimization stability -->
+    <param name="fix_first_node" value="true"/>
+    <param name="fix_first_node_stddev" value="10 10 10 1 1 1"/>
+    <param name="fix_first_node_adaptive" value="true"/>
+    <!-- loop closure params -->
+    <param name="distance_thresh" value="20.0" />
+    <param name="accum_distance_thresh" value="35.0" />
+    <param name="min_edge_interval" value="5.0" />
+    <param name="fitness_score_thresh" value="0.5" />
+    
+    <!-- scan matching params -->
+    <param name="registration_method" value="FAST_GICP" />
+    <param name="reg_num_threads" value="4" />
+    <param name="reg_transformation_epsilon" value="0.01"/>
+    <param name="reg_maximum_iterations" value="64"/>
+    <param name="reg_max_correspondence_distance" value="2.5"/>
+    <param name="reg_max_optimizer_iterations" value="20"/>
+    <param name="reg_use_reciprocal_correspondences" value="false"/>
+    <param name="reg_correspondence_randomness" value="20"/>
+    <param name="reg_resolution" value="1.0" />
+    <param name="reg_nn_search_method" value="DIRECT7" />
+    <!-- edge params -->
+    <!-- GPS -->
+    <param name="gps_edge_robust_kernel" value="NONE" />
+    <param name="gps_edge_robust_kernel_size" value="1.0" />
+    <param name="gps_edge_stddev_xy" value="20.0" />
+    <param name="gps_edge_stddev_z" value="5.0" />
+    <!-- IMU orientation -->
+    <param name="imu_orientation_edge_robust_kernel" value="NONE" />
+    <param name="imu_orientation_edge_stddev" value="1.0" />
+    <!-- IMU acceleration (gravity vector) -->
+    <param name="imu_acceleration_edge_robust_kernel" value="NONE" />
+    <param name="imu_acceleration_edge_stddev" value="1.0" />
+    <!-- ground plane -->
+    <param name="floor_edge_robust_kernel" value="NONE" />
+    <param name="floor_edge_stddev" value="10.0" />
+    <!-- scan matching -->
+    <!-- robust kernels: NONE, Cauchy, DCS, Fair, GemanMcClure, Huber, PseudoHuber, Saturated, Tukey, Welsch -->
+    <param name="odometry_edge_robust_kernel" value="NONE" />
+    <param name="odometry_edge_robust_kernel_size" value="1.0" />
+    <param name="loop_closure_edge_robust_kernel" value="Huber" />
+    <param name="loop_closure_edge_robust_kernel_size" value="1.0" />
+    <param name="use_const_inf_matrix" value="false" />
+    <param name="const_stddev_x" value="0.5" />
+    <param name="const_stddev_q" value="0.1" />
+    <param name="var_gain_a" value="20.0" />
+    <param name="min_stddev_x" value="0.1" />
+    <param name="max_stddev_x" value="5.0" />
+    <param name="min_stddev_q" value="0.05" />
+    <param name="max_stddev_q" value="0.2" />
+    <!-- update params -->
+    <param name="graph_update_interval" value="3.0" />
+    <param name="map_cloud_update_interval" value="10.0" />
+    <param name="map_cloud_resolution" value="0.05" />
+  </node>
+
+  <node pkg="hdl_graph_slam" type="map2odom_publisher.py" name="map2odom_publisher" >
+     <param name="map_frame_id" value="$(arg map_frame_id)" />
+     <param name="odom_frame_id" value="$(arg lidar_odom_frame_id)" />
+  </node>
+</launch>

launch/hdl_graph_slam_400.launch

@@ -0,0 +1,211 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- 
+    
+    前面4个节点的代码文件在hdl_graph_slam/apps/...   是在ROS中注册的插件
+
+    prefiltering_nodelet:               ！！！！ 点云滤波  ！！！！
+        包（pkg）: nodelet
+        类型（type）: nodelet
+        名称（name）: prefiltering_nodelet
+        作用：加载并执行hdl_graph_slam包中的PrefilteringNodelet，用于对激光雷达点云数据进行预处理，包括距离滤波、下采样和离群点移除。
+
+    scan_matching_odometry_nodelet:     ！！！！帧间匹配，也就是激光里程计（前端里程计） ！！！！
+        包（pkg）: nodelet
+        类型（type）: nodelet
+        名称（name）: scan_matching_odometry_nodelet
+        作用：加载并执行hdl_graph_slam包中的ScanMatchingOdometryNodelet，用于通过扫描匹配进行里程计估计。
+
+    floor_detection_nodelet (条件启动，取决于 enable_floor_detection 参数):           ！！！！地平面检测 ！！！！
+        包（pkg）: nodelet
+        类型（type）: nodelet
+        名称（name）: floor_detection_nodelet
+        作用：加载并执行hdl_graph_slam包中的FloorDetectionNodelet，用于检测地面平面。
+
+    hdl_graph_slam_nodelet:             ！！！！后端概率图构建  ！！！！
+        包（pkg）: nodelet
+        类型（type）: nodelet
+        名称（name）: hdl_graph_slam_nodelet
+        作用：加载并执行hdl_graph_slam包中的HdlGraphSlamNodelet，用于执行基于图的SLAM（Simultaneous Localization and Mapping）。
+
+    lidar2base_publisher:
+        包（pkg）: tf
+        类型（type）: static_transform_publisher
+        作用：发布从base_link到velodyne（激光雷达）的静态坐标变换。这里的参数0 0 0 0 0 0表示没有平移和旋转，即激光雷达与base_link坐标系重合。
+
+    velodyne_nodelet_manager:
+        包（pkg）: nodelet
+        类型（type）: nodelet
+        名称（name）: velodyne_nodelet_manager
+        作用：作为一个节点管理器，用于管理其他节点（nodelet）。
+
+    map2odom_publisher:
+        包（pkg）: hdl_graph_slam
+        类型（type）: map2odom_publisher.py
+        名称（name）: map2odom_publisher
+        作用：这是一个Python脚本，用于发布从地图到里程计的变换。
+  -->
+
+  <!-- 使用仿真时间 -->
+  <param name="use_sim_time" value="true" />
+  <!-- 回放rosbag文件 -->
+  <node pkg="rosbag" type="play" name="player" output="screen" args="--clock /home/mj/hdl_slam/hdl_400.bag"/>
+
+<!-- 线程池   用另外一个线程实现后端 -->
+
+
+
+  <!-- arguments -->
+  <arg name="nodelet_manager" default="velodyne_nodelet_manager" />
+  <arg name="enable_floor_detection" default="true" />
+  <arg name="enable_gps" default="false" />
+  <arg name="enable_imu_acc" default="false" />
+  <arg name="enable_imu_ori" default="false" />
+
+  <arg name="points_topic" default="/velodyne_points" />
+  <arg name="map_frame_id" default="map" />
+  <arg name="lidar_odom_frame_id" default="odom" />
+
+  <!-- optional arguments -->
+  <arg name="enable_robot_odometry_init_guess" default="false" />
+  <arg name="robot_odom_frame_id" default="robot_odom" />
+
+  <!-- transformation between lidar and base_link -->
+  <node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="0 0 0 0 0 0 base_link velodyne 10" />
+
+  <!-- in case you use velodyne_driver, comment out the following line -->
+  <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen"/>
+
+  <!-- prefiltering_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="prefiltering_nodelet" args="load hdl_graph_slam/PrefilteringNodelet $(arg nodelet_manager)">
+    <remap from="/velodyne_points" to="$(arg points_topic)" />
+    <!-- in case base_link_frame is blank, mapping will be performed in the lidar frame -->
+    <param name="base_link_frame" value="base_link" />
+    <!-- distance filter -->
+    <param name="use_distance_filter" value="true" />
+    <param name="distance_near_thresh" value="0.1" />
+    <param name="distance_far_thresh" value="100.0" />
+    <!-- NONE, VOXELGRID, or APPROX_VOXELGRID -->
+    <param name="downsample_method" value="VOXELGRID" />
+    <param name="downsample_resolution" value="0.1" />
+    <!-- NONE, RADIUS, or STATISTICAL -->
+    <param name="outlier_removal_method" value="RADIUS" />
+    <param name="statistical_mean_k" value="30" />
+    <param name="statistical_stddev" value="1.2" />
+    <param name="radius_radius" value="0.5" />
+    <param name="radius_min_neighbors" value="2" />
+  </node>
+
+  <!-- scan_matching_odometry_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="scan_matching_odometry_nodelet" args="load hdl_graph_slam/ScanMatchingOdometryNodelet $(arg nodelet_manager)" output="screen">
+    <param name="points_topic" value="$(arg points_topic)" />
+    <param name="odom_frame_id" value="$(arg lidar_odom_frame_id)" />
+    <param name="robot_odom_frame_id" value="$(arg robot_odom_frame_id)" />
+    <param name="keyframe_delta_trans" value="1.0" />
+    <param name="keyframe_delta_angle" value="1.0" />
+    <param name="keyframe_delta_time" value="10000.0" />
+    <param name="transform_thresholding" value="false" />
+    <param name="enable_robot_odometry_init_guess" value="$(arg enable_robot_odometry_init_guess)" />
+    <param name="max_acceptable_trans" value="1.0" />
+    <param name="max_acceptable_angle" value="1.0" />
+    <param name="downsample_method" value="NONE" />
+    <param name="downsample_resolution" value="0.1" />
+    <!-- ICP, GICP, NDT, GICP_OMP, NDT_OMP, FAST_GICP(recommended), or FAST_VGICP -->
+    <param name="registration_method" value="FAST_GICP" />
+    <param name="reg_num_threads" value="0" />
+    <param name="reg_transformation_epsilon" value="0.1"/>
+    <param name="reg_maximum_iterations" value="64"/>
+    <param name="reg_max_correspondence_distance" value="2.0"/>
+    <param name="reg_max_optimizer_iterations" value="20"/>
+    <param name="reg_use_reciprocal_correspondences" value="false"/>
+    <param name="reg_correspondence_randomness" value="20"/>
+    <param name="reg_resolution" value="1.0" />
+    <param name="reg_nn_search_method" value="DIRECT7" />
+  </node>
+
+  <!-- floor_detection_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="floor_detection_nodelet" args="load hdl_graph_slam/FloorDetectionNodelet $(arg nodelet_manager)" if="$(arg enable_floor_detection)">
+    <param name="points_topic" value="$(arg points_topic)" />
+    <param name="tilt_deg" value="0.0" />
+    <param name="sensor_height" value="2.0" />
+    <param name="height_clip_range" value="1.0" />
+    <param name="floor_pts_thresh" value="512" />
+    <param name="use_normal_filtering" value="true" />
+    <param name="normal_filter_thresh" value="20.0" />
+  </node>
+
+  <!-- hdl_graph_slam_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="hdl_graph_slam_nodelet" args="load hdl_graph_slam/HdlGraphSlamNodelet $(arg nodelet_manager)" output="screen">
+    <param name="points_topic" value="$(arg points_topic)" />
+    <!-- frame settings -->
+    <param name="map_frame_id" value="$(arg map_frame_id)" />
+    <param name="odom_frame_id" value="$(arg lidar_odom_frame_id)" />
+    <!-- optimization params -->
+    <!-- typical solvers: gn_var, gn_fix6_3, gn_var_cholmod, lm_var, lm_fix6_3, lm_var_cholmod, ... -->
+    <param name="g2o_solver_type" value="lm_var_cholmod" />
+    <param name="g2o_solver_num_iterations" value="512" />
+    <!-- constraint switches -->
+    <param name="enable_gps" value="$(arg enable_gps)" />
+    <param name="enable_imu_acceleration" value="$(arg enable_imu_acc)" />
+    <param name="enable_imu_orientation" value="$(arg enable_imu_ori)" />
+    <!-- keyframe registration params -->
+    <param name="max_keyframes_per_update" value="10" />
+    <param name="keyframe_delta_trans" value="2.0" />
+    <param name="keyframe_delta_angle" value="2.0" />
+    <!-- fix first node for optimization stability -->
+    <param name="fix_first_node" value="true"/>
+    <param name="fix_first_node_stddev" value="10 10 10 1 1 1"/>
+    <param name="fix_first_node_adaptive" value="true"/>
+    <!-- loop closure params -->
+    <param name="distance_thresh" value="15.0" />
+    <param name="accum_distance_thresh" value="25.0" />
+    <param name="min_edge_interval" value="15.0" />
+    <param name="fitness_score_thresh" value="2.5" />
+    <!-- scan matching params -->
+    <param name="registration_method" value="FAST_GICP" />
+    <param name="reg_num_threads" value="0" />
+    <param name="reg_transformation_epsilon" value="0.1"/>
+    <param name="reg_maximum_iterations" value="64"/>
+    <param name="reg_max_correspondence_distance" value="2.0"/>
+    <param name="reg_max_optimizer_iterations" value="20"/>
+    <param name="reg_use_reciprocal_correspondences" value="false"/>
+    <param name="reg_correspondence_randomness" value="20"/>
+    <param name="reg_resolution" value="1.0" />
+    <param name="reg_nn_search_method" value="DIRECT7" />
+    <!-- edge params -->
+    <!-- GPS -->
+    <param name="gps_edge_robust_kernel" value="NONE" />
+    <param name="gps_edge_robust_kernel_size" value="1.0" />
+    <param name="gps_edge_stddev_xy" value="20.0" />
+    <param name="gps_edge_stddev_z" value="5.0" />
+    <!-- IMU orientation -->
+    <param name="imu_orientation_edge_robust_kernel" value="NONE" />
+    <param name="imu_orientation_edge_stddev" value="1.0" />
+    <!-- IMU acceleration (gravity vector) -->
+    <param name="imu_acceleration_edge_robust_kernel" value="NONE" />
+    <param name="imu_acceleration_edge_stddev" value="1.0" />
+    <!-- ground plane -->
+    <param name="floor_edge_robust_kernel" value="NONE" />
+    <param name="floor_edge_stddev" value="10.0" />
+    <!-- scan matching -->
+    <!-- robust kernels: NONE, Cauchy, DCS, Fair, GemanMcClure, Huber, PseudoHuber, Saturated, Tukey, Welsch -->
+    <param name="odometry_edge_robust_kernel" value="NONE" />
+    <param name="odometry_edge_robust_kernel_size" value="1.0" />
+    <param name="loop_closure_edge_robust_kernel" value="Huber" />
+    <param name="loop_closure_edge_robust_kernel_size" value="1.0" />
+    <param name="use_const_inf_matrix" value="false" />
+    <param name="const_stddev_x" value="0.5" />
+    <param name="const_stddev_q" value="0.1" />
+    <param name="var_gain_a" value="20.0" />
+    <param name="min_stddev_x" value="0.1" />
+    <param name="max_stddev_x" value="5.0" />
+    <param name="min_stddev_q" value="0.05" />
+    <param name="max_stddev_q" value="0.2" />
+    <!-- update params -->
+    <param name="graph_update_interval" value="3.0" />
+    <param name="map_cloud_update_interval" value="10.0" />
+    <param name="map_cloud_resolution" value="0.05" />
+  </node>
+
+  <node pkg="hdl_graph_slam" type="map2odom_publisher.py" name="map2odom_publisher"  output="screen"/>
+</launch>

launch/hdl_graph_slam_501.launch

@@ -0,0 +1,156 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- arguments -->
+  <arg name="nodelet_manager" default="velodyne_nodelet_manager" />
+  <arg name="enable_floor_detection" default="true" />
+  <arg name="enable_gps" default="false" />
+  <arg name="enable_imu_acc" default="false" />
+  <arg name="enable_imu_ori" default="false" />
+
+  <arg name="points_topic" default="/velodyne_points" />
+  <arg name="map_frame_id" default="map" />
+  <arg name="lidar_odom_frame_id" default="odom" />
+
+  <!-- optional arguments -->
+  <arg name="enable_robot_odometry_init_guess" default="false" />
+  <arg name="robot_odom_frame_id" default="robot_odom" />
+
+  <!-- transformation between lidar and base_link -->
+  <node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="0 0 0 0 0 0 base_link velodyne 10" />
+
+  <!-- in case you use velodyne_driver, comment out the following line -->
+  <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen"/>
+
+  <!-- prefiltering_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="prefiltering_nodelet" args="load hdl_graph_slam/PrefilteringNodelet $(arg nodelet_manager)">
+    <remap from="/velodyne_points" to="$(arg points_topic)" />
+    <!-- in case base_link_frame is blank, mapping will be performed in the lidar frame -->
+    <param name="base_link_frame" value="base_link" />
+    <!-- distance filter -->
+    <param name="use_distance_filter" value="true" />
+    <param name="distance_near_thresh" value="0.5" />
+    <param name="distance_far_thresh" value="100.0" />
+    <!-- NONE, VOXELGRID, or APPROX_VOXELGRID -->
+    <param name="downsample_method" value="VOXELGRID" />
+    <param name="downsample_resolution" value="0.1" />
+    <!-- NONE, RADIUS, or STATISTICAL -->
+    <param name="outlier_removal_method" value="RADIUS" />
+    <param name="statistical_mean_k" value="30" />
+    <param name="statistical_stddev" value="1.2" />
+    <param name="radius_radius" value="0.5" />
+    <param name="radius_min_neighbors" value="2" />
+  </node>
+
+  <!-- scan_matching_odometry_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="scan_matching_odometry_nodelet" args="load hdl_graph_slam/ScanMatchingOdometryNodelet $(arg nodelet_manager)">
+    <param name="points_topic" value="$(arg points_topic)" />
+    <param name="odom_frame_id" value="$(arg lidar_odom_frame_id)" />
+    <param name="robot_odom_frame_id" value="$(arg robot_odom_frame_id)" />
+    <param name="keyframe_delta_trans" value="0.25" />
+    <param name="keyframe_delta_angle" value="1.0" />
+    <param name="keyframe_delta_time" value="10000.0" />
+    <param name="transform_thresholding" value="false" />
+    <param name="enable_robot_odometry_init_guess" value="$(arg enable_robot_odometry_init_guess)" />
+    <param name="max_acceptable_trans" value="1.0" />
+    <param name="max_acceptable_angle" value="1.0" />
+    <param name="downsample_method" value="NONE" />
+    <param name="downsample_resolution" value="0.1" />
+    <!-- ICP, GICP, NDT, GICP_OMP, NDT_OMP, FAST_GICP(recommended), or FAST_VGICP -->
+    <param name="registration_method" value="FAST_GICP" />
+    <param name="reg_num_threads" value="0" />
+    <param name="reg_transformation_epsilon" value="0.1"/>
+    <param name="reg_maximum_iterations" value="64"/>
+    <param name="reg_max_correspondence_distance" value="2.0"/>
+    <param name="reg_max_optimizer_iterations" value="20"/>
+    <param name="reg_use_reciprocal_correspondences" value="false"/>
+    <param name="reg_correspondence_randomness" value="20"/>
+    <param name="reg_resolution" value="1.0" />
+    <param name="reg_nn_search_method" value="DIRECT7" />
+  </node>
+
+  <!-- floor_detection_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="floor_detection_nodelet" args="load hdl_graph_slam/FloorDetectionNodelet $(arg nodelet_manager)" if="$(arg enable_floor_detection)">
+    <param name="points_topic" value="$(arg points_topic)" />
+    <param name="tilt_deg" value="0.0" />
+    <param name="sensor_height" value="1.5" />
+    <param name="height_clip_range" value="0.5" />
+    <param name="floor_pts_thresh" value="256" />
+    <param name="use_normal_filtering" value="true" />
+    <param name="normal_filter_thresh" value="20.0" />
+  </node>
+
+  <!-- hdl_graph_slam_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="hdl_graph_slam_nodelet" args="load hdl_graph_slam/HdlGraphSlamNodelet $(arg nodelet_manager)">
+    <param name="points_topic" value="$(arg points_topic)" />
+    <!-- frame settings -->
+    <param name="map_frame_id" value="$(arg map_frame_id)" />
+    <param name="odom_frame_id" value="$(arg lidar_odom_frame_id)" />
+    <!-- optimization params -->
+    <!-- typical solvers: gn_var, gn_fix6_3, gn_var_cholmod, lm_var, lm_fix6_3, lm_var_cholmod, ... -->
+    <param name="g2o_solver_type" value="lm_var_cholmod" />
+    <param name="g2o_solver_num_iterations" value="512" />
+    <!-- constraint switches -->
+    <param name="enable_gps" value="$(arg enable_gps)" />
+    <param name="enable_imu_acceleration" value="$(arg enable_imu_acc)" />
+    <param name="enable_imu_orientation" value="$(arg enable_imu_ori)" />
+    <!-- keyframe registration params -->
+    <param name="max_keyframes_per_update" value="10" />
+    <param name="keyframe_delta_trans" value="1.0" />
+    <param name="keyframe_delta_angle" value="2.0" />
+    <!-- fix first node for optimization stability -->
+    <param name="fix_first_node" value="true"/>
+    <param name="fix_first_node_stddev" value="10 10 10 1 1 1"/>
+    <param name="fix_first_node_adaptive" value="true"/>
+    <!-- loop closure params -->
+    <param name="distance_thresh" value="1.0" />
+    <param name="accum_distance_thresh" value="3.0" />
+    <param name="min_edge_interval" value="1.0" />
+    <param name="fitness_score_thresh" value="0.5" />
+    <!-- scan matching params -->
+    <param name="registration_method" value="FAST_GICP" />
+    <param name="reg_num_threads" value="0" />
+    <param name="reg_transformation_epsilon" value="0.1"/>
+    <param name="reg_maximum_iterations" value="64"/>
+    <param name="reg_max_correspondence_distance" value="2.0"/>
+    <param name="reg_max_optimizer_iterations" value="20"/>
+    <param name="reg_use_reciprocal_correspondences" value="false"/>
+    <param name="reg_correspondence_randomness" value="20"/>
+    <param name="reg_resolution" value="1.0" />
+    <param name="reg_nn_search_method" value="DIRECT7" />
+    <!-- edge params -->
+    <!-- GPS -->
+    <param name="gps_edge_robust_kernel" value="NONE" />
+    <param name="gps_edge_robust_kernel_size" value="1.0" />
+    <param name="gps_edge_stddev_xy" value="20.0" />
+    <param name="gps_edge_stddev_z" value="5.0" />
+    <!-- IMU orientation -->
+    <param name="imu_orientation_edge_robust_kernel" value="NONE" />
+    <param name="imu_orientation_edge_stddev" value="1.0" />
+    <!-- IMU acceleration (gravity vector) -->
+    <param name="imu_acceleration_edge_robust_kernel" value="NONE" />
+    <param name="imu_acceleration_edge_stddev" value="1.0" />
+    <!-- ground plane -->
+    <param name="floor_edge_robust_kernel" value="NONE" />
+    <param name="floor_edge_stddev" value="10.0" />
+    <!-- scan matching -->
+    <!-- robust kernels: NONE, Cauchy, DCS, Fair, GemanMcClure, Huber, PseudoHuber, Saturated, Tukey, Welsch -->
+    <param name="odometry_edge_robust_kernel" value="NONE" />
+    <param name="odometry_edge_robust_kernel_size" value="1.0" />
+    <param name="loop_closure_edge_robust_kernel" value="Huber" />
+    <param name="loop_closure_edge_robust_kernel_size" value="1.0" />
+    <param name="use_const_inf_matrix" value="false" />
+    <param name="const_stddev_x" value="0.5" />
+    <param name="const_stddev_q" value="0.1" />
+    <param name="var_gain_a" value="20.0" />
+    <param name="min_stddev_x" value="0.1" />
+    <param name="max_stddev_x" value="5.0" />
+    <param name="min_stddev_q" value="0.05" />
+    <param name="max_stddev_q" value="0.2" />
+    <!-- update params -->
+    <param name="graph_update_interval" value="1.5" />
+    <param name="map_cloud_update_interval" value="3.0" />
+    <param name="map_cloud_resolution" value="0.01" />
+  </node>
+
+  <node pkg="hdl_graph_slam" type="map2odom_publisher.py" name="map2odom_publisher" />
+</launch>

launch/hdl_graph_slam_imu.launch

@@ -0,0 +1,160 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- arguments -->
+  <arg name="nodelet_manager" default="velodyne_nodelet_manager" />
+  <arg name="enable_imu_frontend" default="true" />
+  <arg name="enable_floor_detection" default="false" />
+  <arg name="enable_gps" default="false" />
+  <arg name="enable_imu_acc" default="false" />
+  <arg name="enable_imu_ori" default="false" />
+  <arg name="points_topic" default="/velodyne_points" />
+  <arg name="imu_topic" default="/imu/data" />
+
+  <!-- transformation between lidar and base_link -->
+  <node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="0 0 0 0 0 0 base_link velodyne 10" />
+  <node pkg="tf" type="static_transform_publisher" name="state2base_publisher" args="0 0 0 0 0 0 odom world 10" />
+
+  <!-- in case you use velodyne_driver, comment out the following line -->
+  <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen"/>
+
+  <!-- IMU fusion -->
+  <node name="msf_lidar_scan_matching" pkg="msf_updates" type="pose_sensor" clear_params="true" output="screen">
+    <remap from="msf_core/hl_state_input" to="/fcu/ekf_state_out" />
+    <remap from="msf_core/correction" to="/fcu/ekf_state_in" />
+    <remap from="msf_core/imu_state_input" to="$(arg imu_topic)" />
+    <remap from="msf_updates/transform_input" to="/scan_matching_odometry/transform" />
+
+    <rosparam file="$(find hdl_graph_slam)/launch/msf_config.yaml"/>
+  </node>
+
+  <node pkg="rosservice" type="rosservice" name="initialize" args="call --wait /msf_lidar_scan_matching/pose_sensor/initialize_msf_scale 1"/>
+
+  <!-- prefiltering_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="prefiltering_nodelet" args="load hdl_graph_slam/PrefilteringNodelet $(arg nodelet_manager)" output="screen">
+    <remap from="/velodyne_points" to="$(arg points_topic)" />
+    <remap from="/imu/data" to="$(arg imu_topic)" />
+    <param name="deskewing" value="$(arg enable_imu_frontend)" />
+    <param name="scan_period" value="0.1" />
+    <!-- in case base_link_frame is blank, mapping will be performed in the lidar frame -->
+    <param name="base_link_frame" value="base_link" />
+    <!-- distance filter -->
+    <param name="use_distance_filter" value="true" />
+    <param name="distance_near_thresh" value="0.2" />
+    <param name="distance_far_thresh" value="100.0" />
+    <!-- NONE, VOXELGRID, or APPROX_VOXELGRID -->
+    <param name="downsample_method" value="VOXELGRID" />
+    <param name="downsample_resolution" value="0.1" />
+    <!-- NONE, RADIUS, or STATISTICAL -->
+    <param name="outlier_removal_method" value="RADIUS" />
+    <param name="statistical_mean_k" value="30" />
+    <param name="statistical_stddev" value="1.2" />
+    <param name="radius_radius" value="0.5" />
+    <param name="radius_min_neighbors" value="2" />
+  </node>
+
+  <!-- scan_matching_odometry_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="scan_matching_odometry_nodelet" args="load hdl_graph_slam/ScanMatchingOdometryNodelet $(arg nodelet_manager)">
+    <param name="enable_imu_frontend" value="$(arg enable_imu_frontend)" />
+    <param name="points_topic" value="$(arg points_topic)" />
+    <param name="odom_frame_id" value="odom" />
+    <param name="keyframe_delta_trans" value="0.25" />
+    <param name="keyframe_delta_angle" value="2.0" />
+    <param name="keyframe_delta_time" value="10000.0" />
+    <param name="transform_thresholding" value="false" />
+    <param name="max_acceptable_trans" value="1.0" />
+    <param name="max_acceptable_angle" value="1.0" />
+    <param name="downsample_method" value="NONE" />
+    <param name="downsample_resolution" value="0.1" />
+    <!-- ICP, GICP, NDT, GICP_OMP, or NDT_OMP(recommended) -->
+    <param name="registration_method" value="NDT_OMP" />
+    <param name="transformation_epsilon" value="0.01"/>
+    <param name="maximum_iterations" value="64"/>
+    <param name="use_reciprocal_correspondences" value="false"/>
+    <param name="gicp_correspondence_randomness" value="20"/>
+    <param name="gicp_max_optimizer_iterations" value="20"/>
+    <param name="ndt_resolution" value="10.0" />
+    <param name="ndt_num_threads" value="0" />
+    <param name="ndt_nn_search_method" value="DIRECT7" />
+  </node>
+
+  <!-- floor_detection_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="floor_detection_nodelet" args="load hdl_graph_slam/FloorDetectionNodelet $(arg nodelet_manager)" if="$(arg enable_floor_detection)">
+    <param name="points_topic" value="$(arg points_topic)" />
+    <param name="tilt_deg" value="0.0" />
+    <param name="sensor_height" value="1.5" />
+    <param name="height_clip_range" value="0.5" />
+    <param name="floor_pts_thresh" value="256" />
+    <param name="use_normal_filtering" value="true" />
+    <param name="normal_filter_thresh" value="20.0" />
+  </node>
+
+  <!-- hdl_graph_slam_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="hdl_graph_slam_nodelet" args="load hdl_graph_slam/HdlGraphSlamNodelet $(arg nodelet_manager)">
+    <param name="points_topic" value="$(arg points_topic)" />
+    <remap from="/gpsimu_driver/imu_data" to="$(arg imu_topic)" />
+    <!-- frame settings -->
+    <param name="map_frame_id" value="map" />
+    <param name="odom_frame_id" value="odom" />
+    <!-- optimization params -->
+    <!-- typical solvers: gn_var, gn_fix6_3, gn_var_cholmod, lm_var, lm_fix6_3, lm_var_cholmod, ... -->
+    <param name="g2o_solver_type" value="lm_var_cholmod" />
+    <param name="g2o_solver_num_iterations" value="512" />
+    <!-- constraint switches -->
+    <param name="enable_gps" value="$(arg enable_gps)" />
+    <param name="enable_imu_acceleration" value="$(arg enable_imu_acc)" />
+    <param name="enable_imu_orientation" value="$(arg enable_imu_ori)" />
+    <!-- keyframe registration params -->
+    <param name="max_keyframes_per_update" value="10" />
+    <param name="keyframe_delta_trans" value="1.0" />
+    <param name="keyframe_delta_angle" value="2.0" />
+    <!-- fix first node for optimization stability -->
+    <param name="fix_first_node" value="true"/>
+    <param name="fix_first_node_stddev" value="10 10 10 1 1 1"/>
+    <param name="fix_first_node_adaptive" value="true"/>
+    <!-- loop closure params -->
+    <param name="distance_thresh" value="1.0" />
+    <param name="accum_distance_thresh" value="3.0" />
+    <param name="min_edge_interval" value="1.0" />
+    <param name="fitness_score_thresh" value="0.5" />
+    <!-- scan matching params -->
+    <param name="registration_method" value="GICP" />
+    <param name="ndt_resolution" value="1.0" />
+    <param name="ndt_num_threads" value="0" />
+    <param name="ndt_nn_search_method" value="DIRECT7" />
+    <!-- edge params -->
+    <!-- GPS -->
+    <param name="gps_edge_robust_kernel" value="NONE" />
+    <param name="gps_edge_robust_kernel_size" value="1.0" />
+    <param name="gps_edge_stddev_xy" value="20.0" />
+    <param name="gps_edge_stddev_z" value="5.0" />
+    <!-- IMU orientation -->
+    <param name="imu_orientation_edge_robust_kernel" value="NONE" />
+    <param name="imu_orientation_edge_stddev" value="1.0" />
+    <!-- IMU acceleration (gravity vector) -->
+    <param name="imu_acceleration_edge_robust_kernel" value="NONE" />
+    <param name="imu_acceleration_edge_stddev" value="1.0" />
+    <!-- ground plane -->
+    <param name="floor_edge_robust_kernel" value="NONE" />
+    <param name="floor_edge_stddev" value="10.0" />
+    <!-- scan matching -->
+    <!-- robust kernels: NONE, Cauchy, DCS, Fair, GemanMcClure, Huber, PseudoHuber, Saturated, Tukey, Welsch -->
+    <param name="odometry_edge_robust_kernel" value="NONE" />
+    <param name="odometry_edge_robust_kernel_size" value="1.0" />
+    <param name="loop_closure_edge_robust_kernel" value="Huber" />
+    <param name="loop_closure_edge_robust_kernel_size" value="1.0" />
+    <param name="use_const_inf_matrix" value="false" />
+    <param name="const_stddev_x" value="0.5" />
+    <param name="const_stddev_q" value="0.1" />
+    <param name="var_gain_a" value="20.0" />
+    <param name="min_stddev_x" value="0.1" />
+    <param name="max_stddev_x" value="5.0" />
+    <param name="min_stddev_q" value="0.05" />
+    <param name="max_stddev_q" value="0.2" />
+    <!-- update params -->
+    <param name="graph_update_interval" value="1.5" />
+    <param name="map_cloud_update_interval" value="3.0" />
+    <param name="map_cloud_resolution" value="0.01" />
+  </node>
+
+  <node pkg="hdl_graph_slam" type="map2odom_publisher.py" name="map2odom_publisher" />
+</launch>

launch/hdl_graph_slam_kitti.launch

@@ -0,0 +1,147 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- arguments -->
+  <arg name="nodelet_manager" default="velodyne_nodelet_manager" />
+  <arg name="enable_floor_detection" default="true" />
+  <arg name="enable_gps" default="true" />
+  <arg name="enable_imu_acc" default="false" />
+  <arg name="enable_imu_ori" default="false" />
+  <arg name="points_topic" default="/velodyne_points" />
+
+  <!-- transformation between lidar and base_link -->
+  <node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="0 0 0 0 0 0 base_link velodyne 10" />
+
+  <!-- in case you use velodyne_driver, comment out the following line -->
+  <node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager)" args="manager" output="screen"/>
+
+  <!-- prefiltering_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="prefiltering_nodelet" args="load hdl_graph_slam/PrefilteringNodelet $(arg nodelet_manager)">
+    <remap from="/velodyne_points" to="$(arg points_topic)" />
+    <!-- in case base_link_frame is blank, mapping will be performed in the lidar frame -->
+    <param name="base_link_frame" value="base_link" />
+    <!-- distance filter -->
+    <param name="use_distance_filter" value="true" />
+    <param name="distance_near_thresh" value="0.1" />
+    <param name="distance_far_thresh" value="100.0" />
+    <!-- NONE, VOXELGRID, or APPROX_VOXELGRID -->
+    <param name="downsample_method" value="VOXELGRID" />
+    <param name="downsample_resolution" value="0.25" />
+    <!-- NONE, RADIUS, or STATISTICAL -->
+    <param name="outlier_removal_method" value="RADIUS" />
+    <param name="statistical_mean_k" value="30" />
+    <param name="statistical_stddev" value="1.2" />
+    <param name="radius_radius" value="0.5" />
+    <param name="radius_min_neighbors" value="2" />
+  </node>
+
+  <!-- scan_matching_odometry_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="scan_matching_odometry_nodelet" args="load hdl_graph_slam/ScanMatchingOdometryNodelet $(arg nodelet_manager)">
+      <param name="points_topic" value="$(arg points_topic)" />
+      <param name="odom_frame_id" value="odom" />
+      <param name="keyframe_delta_trans" value="5.0" />
+      <param name="keyframe_delta_angle" value="2.0" />
+      <param name="keyframe_delta_time" value="10000.0" />
+      <param name="transform_thresholding" value="false" />
+      <param name="max_acceptable_trans" value="1.0" />
+      <param name="max_acceptable_angle" value="1.0" />
+      <param name="downsample_method" value="NONE" />
+      <param name="downsample_resolution" value="0.1" />
+      <!-- ICP, GICP, NDT, GICP_OMP, NDT_OMP, FAST_GICP(recommended), or FAST_VGICP -->
+      <param name="registration_method" value="FAST_GICP" />
+      <param name="reg_num_threads" value="0" />
+      <param name="reg_transformation_epsilon" value="0.1"/>
+      <param name="reg_maximum_iterations" value="64"/>
+      <param name="reg_max_correspondence_distance" value="2.0"/>
+      <param name="reg_max_optimizer_iterations" value="20"/>
+      <param name="reg_use_reciprocal_correspondences" value="false"/>
+      <param name="reg_correspondence_randomness" value="20"/>
+      <param name="reg_resolution" value="1.0" />
+      <param name="reg_nn_search_method" value="DIRECT7" />
+  </node>
+
+  <!-- floor_detection_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="floor_detection_nodelet" args="load hdl_graph_slam/FloorDetectionNodelet $(arg nodelet_manager)" if="$(arg enable_floor_detection)">
+    <param name="points_topic" value="$(arg points_topic)" />
+    <param name="tilt_deg" value="0.0" />
+    <param name="sensor_height" value="3.0" />
+    <param name="height_clip_range" value="1.0" />
+    <param name="floor_pts_thresh" value="512" />
+    <param name="use_normal_filtering" value="false" />
+    <param name="normal_filter_thresh" value="20.0" />
+  </node>
+
+  <!-- hdl_graph_slam_nodelet -->
+  <node pkg="nodelet" type="nodelet" name="hdl_graph_slam_nodelet" args="load hdl_graph_slam/HdlGraphSlamNodelet $(arg nodelet_manager)">
+    <param name="points_topic" value="$(arg points_topic)" />
+    <!-- frame settings -->
+    <param name="map_frame_id" value="map" />
+    <param name="odom_frame_id" value="odom" />
+    <!-- optimization params -->
+    <!-- typical solvers: gn_var, gn_fix6_3, gn_var_cholmod, lm_var, lm_fix6_3, lm_var_cholmod, ... -->
+    <param name="g2o_solver_type" value="lm_var_cholmod" />
+    <param name="g2o_solver_num_iterations" value="512" />
+    <!-- constraint switches -->
+    <param name="enable_gps" value="$(arg enable_gps)" />
+    <param name="enable_imu_acceleration" value="$(arg enable_imu_acc)" />
+    <param name="enable_imu_orientation" value="$(arg enable_imu_ori)" />
+    <!-- keyframe registration params -->
+    <param name="max_keyframes_per_update" value="10" />
+    <param name="keyframe_delta_trans" value="5.0" />
+    <param name="keyframe_delta_angle" value="2.0" />
+    <!-- fix first node for optimization stability -->
+    <param name="fix_first_node" value="true"/>
+    <param name="fix_first_node_stddev" value="10 10 10 1 1 1"/>
+    <param name="fix_first_node_adaptive" value="true"/>
+    <!-- loop closure params -->
+    <param name="distance_thresh" value="30.0" />
+    <param name="accum_distance_thresh" value="25.0" />
+    <param name="min_edge_interval" value="15.0" />
+    <param name="fitness_score_thresh" value="2.5" />
+    <!-- scan matching params -->
+    <param name="registration_method" value="FAST_GICP" />
+    <param name="reg_num_threads" value="0" />
+    <param name="reg_transformation_epsilon" value="0.1"/>
+    <param name="reg_maximum_iterations" value="64"/>
+    <param name="reg_max_correspondence_distance" value="2.0"/>
+    <param name="reg_max_optimizer_iterations" value="20"/>
+    <param name="reg_use_reciprocal_correspondences" value="false"/>
+    <param name="reg_correspondence_randomness" value="20"/>
+    <param name="reg_resolution" value="1.0" />
+    <param name="reg_nn_search_method" value="DIRECT7" />
+    <!-- edge params -->
+    <!-- GPS -->
+    <param name="gps_edge_robust_kernel" value="NONE" />
+    <param name="gps_edge_robust_kernel_size" value="1.0" />
+    <param name="gps_edge_stddev_xy" value="20.0" />
+    <param name="gps_edge_stddev_z" value="5.0" />
+    <!-- IMU orientation -->
+    <param name="imu_orientation_edge_robust_kernel" value="NONE" />
+    <param name="imu_orientation_edge_stddev" value="1.0" />
+    <!-- IMU acceleration (gravity vector) -->
+    <param name="imu_acceleration_edge_robust_kernel" value="NONE" />
+    <param name="imu_acceleration_edge_stddev" value="1.0" />
+    <!-- ground plane -->
+    <param name="floor_edge_robust_kernel" value="NONE" />
+    <param name="floor_edge_stddev" value="10.0" />
+    <!-- scan matching -->
+    <!-- robust kernels: NONE, Cauchy, DCS, Fair, GemanMcClure, Huber, PseudoHuber, Saturated, Tukey, Welsch -->
+    <param name="odometry_edge_robust_kernel" value="NONE" />
+    <param name="odometry_edge_robust_kernel_size" value="1.0" />
+    <param name="loop_closure_edge_robust_kernel" value="Huber" />
+    <param name="loop_closure_edge_robust_kernel_size" value="1.0" />
+    <param name="use_const_inf_matrix" value="false" />
+    <param name="const_stddev_x" value="0.5" />
+    <param name="const_stddev_q" value="0.1" />
+    <param name="var_gain_a" value="20.0" />
+    <param name="min_stddev_x" value="0.1" />
+    <param name="max_stddev_x" value="5.0" />
+    <param name="min_stddev_q" value="0.05" />
+    <param name="max_stddev_q" value="0.2" />
+    <!-- update params -->
+    <param name="graph_update_interval" value="1.5" />
+    <param name="map_cloud_update_interval" value="5.0" />
+    <param name="map_cloud_resolution" value="0.05" />
+  </node>
+
+  <node pkg="hdl_graph_slam" type="map2odom_publisher.py" name="map2odom_publisher" />
+</launch>

launch/msf_config.yaml

@@ -0,0 +1,39 @@
+/msf_lidar_scan_matching/core/data_playback: true
+/msf_lidar_scan_matching/core/fixed_bias: 0
+
+#########IMU PARAMETERS#######
+######## Asctec Firefly IMU
+/msf_lidar_scan_matching/core/core_noise_acc: 0.083
+/msf_lidar_scan_matching/core/core_noise_accbias: 0.0083
+/msf_lidar_scan_matching/core/core_noise_gyr: 0.0013
+/msf_lidar_scan_matching/core/core_noise_gyrbias: 0.00013
+
+/msf_lidar_scan_matching/pose_sensor/fixed_scale: 1
+/msf_lidar_scan_matching/pose_sensor/pose_noise_scale: 0.0
+/msf_lidar_scan_matching/pose_sensor/pose_noise_p_wv: 0.0
+/msf_lidar_scan_matching/pose_sensor/pose_noise_q_wv: 0.0
+/msf_lidar_scan_matching/pose_sensor/pose_noise_q_ic: 0.0
+/msf_lidar_scan_matching/pose_sensor/pose_noise_p_ic: 0.0
+/msf_lidar_scan_matching/pose_sensor/pose_delay: 0.02
+/msf_lidar_scan_matching/pose_sensor/pose_noise_meas_p: 0.05
+/msf_lidar_scan_matching/pose_sensor/pose_noise_meas_q: 0.05
+
+/msf_lidar_scan_matching/pose_sensor/init/q_ic/w: 1.0
+/msf_lidar_scan_matching/pose_sensor/init/q_ic/x: 0.0
+/msf_lidar_scan_matching/pose_sensor/init/q_ic/y: 0.0
+/msf_lidar_scan_matching/pose_sensor/init/q_ic/z: 0.0
+/msf_lidar_scan_matching/pose_sensor/init/p_ic/x: 0.0
+/msf_lidar_scan_matching/pose_sensor/init/p_ic/y: 0.0
+/msf_lidar_scan_matching/pose_sensor/init/p_ic/z: 0.0
+
+/msf_lidar_scan_matching/pose_sensor/pose_fixed_scale: true
+/msf_lidar_scan_matching/pose_sensor/pose_fixed_p_ic: false
+/msf_lidar_scan_matching/pose_sensor/pose_fixed_q_ic: false
+/msf_lidar_scan_matching/pose_sensor/pose_fixed_p_wv: false
+/msf_lidar_scan_matching/pose_sensor/pose_fixed_q_wv: false
+
+/msf_lidar_scan_matching/pose_sensor/pose_absolute_measurements: false
+/msf_lidar_scan_matching/pose_sensor/pose_use_fixed_covariance: true
+/msf_lidar_scan_matching/pose_sensor/pose_measurement_world_sensor: true  # selects if sensor measures its position w.r.t. world (true, e.g. Vicon) or the position of the world coordinate system w.r.t. the sensor (false, e.g. ethzasl_ptam
+
+/msf_lidar_scan_matching/pose_sensor/pose_measurement_minimum_dt: 0.05  # Sets the minimum time in seconds between two pose measurements.

launch/start.launch

@@ -0,0 +1,10 @@
+<launch>
+    <node pkg="hdl_graph_slam" type="laser_odom" name="laser_odom" output="log" />
+    
+    
+    <!-- 使用仿真时间 -->
+    <param name="use_sim_time" value="true" />
+    <!-- 回放rosbag文件 -->
+    <node pkg="rosbag" type="play" name="player" output="screen" args="--clock /home/mj/hdl_slam/ros_yfm.bag"/>
+
+</launch>

msg/FloorCoeffs.msg

@@ -0,0 +1,3 @@
+Header header
+
+float32[] coeffs

msg/ScanMatchingStatus.msg

@@ -0,0 +1,9 @@
+Header header
+
+bool has_converged
+float32 matching_error
+float32 inlier_fraction
+geometry_msgs/Pose relative_pose
+
+std_msgs/String[] prediction_labels
+geometry_msgs/Pose[] prediction_errors

nodelet_plugins.xml

@@ -0,0 +1,15 @@
+<library path="lib/libscan_matching_odometry_nodelet">
+  <class name="hdl_graph_slam/ScanMatchingOdometryNodelet" type="hdl_graph_slam::ScanMatchingOdometryNodelet" base_class_type="nodelet::Nodelet" />
+</library>
+
+<library path="lib/libfloor_detection_nodelet">
+  <class name="hdl_graph_slam/FloorDetectionNodelet" type="hdl_graph_slam::FloorDetectionNodelet" base_class_type="nodelet::Nodelet" />
+</library>
+
+<library path="lib/libprefiltering_nodelet">
+  <class name="hdl_graph_slam/PrefilteringNodelet" type="hdl_graph_slam::PrefilteringNodelet" base_class_type="nodelet::Nodelet" />
+</library>
+
+<library path="lib/libhdl_graph_slam_nodelet">
+  <class name="hdl_graph_slam/HdlGraphSlamNodelet" type="hdl_graph_slam::HdlGraphSlamNodelet" base_class_type="nodelet::Nodelet" />
+</library>

package.xml

@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>hdl_graph_slam</name>
+  <version>0.0.0</version>
+  <description>The hdl_graph_slam package</description>
+
+  <maintainer email="koide@aisl.cs.tut.ac.jp">koide</maintainer>
+
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>libg2o</build_depend>
+  <build_depend>message_generation</build_depend>
+  <depend>eigen</depend>
+  <depend>fast_gicp</depend>
+  <depend>geodesy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>interactive_markers</depend>
+  <depend>ndt_omp</depend>
+  <depend>nmea_msgs</depend>
+  <depend>pcl_ros</depend>
+  <depend>roscpp</depend>
+  <depend>rospy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf_conversions</depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>msf_updates</exec_depend>
+  <exec_depend>nodelet</exec_depend>
+  <exec_depend>tf</exec_depend>
+
+  <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-scipy</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-scipy</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-progressbar</exec_depend>
+  <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-progressbar</exec_depend>
+
+  <export>
+    <nodelet plugin="${prefix}/nodelet_plugins.xml" />
+  </export>
+</package>

setup.py

@@ -0,0 +1,12 @@
+## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from setuptools import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup(
+    packages=['hdl_graph_slam'],
+    package_dir={'': 'src'})
+
+setup(**setup_args)
+

src/g2o/robust_kernel_io.cpp

@@ -0,0 +1,156 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#include <g2o/robust_kernel_io.hpp>
+
+#include <fstream>
+#include <iostream>
+#include <g2o/core/robust_kernel.h>
+#include <g2o/core/robust_kernel_impl.h>
+#include <g2o/core/robust_kernel_factory.h>
+#include <g2o/core/sparse_optimizer.h>
+
+namespace g2o {
+
+std::string kernel_type(g2o::RobustKernel* kernel) {
+  if(dynamic_cast<g2o::RobustKernelHuber*>(kernel)) {
+    return "Huber";
+  }
+  if(dynamic_cast<g2o::RobustKernelCauchy*>(kernel)) {
+    return "Cauchy";
+  }
+  if(dynamic_cast<g2o::RobustKernelDCS*>(kernel)) {
+    return "DCS";
+  }
+  if(dynamic_cast<g2o::RobustKernelFair*>(kernel)) {
+    return "Fair";
+  }
+  if(dynamic_cast<g2o::RobustKernelGemanMcClure*>(kernel)) {
+    return "GemanMcClure";
+  }
+  if(dynamic_cast<g2o::RobustKernelPseudoHuber*>(kernel)) {
+    return "PseudoHuber";
+  }
+  if(dynamic_cast<g2o::RobustKernelSaturated*>(kernel)) {
+    return "Saturated";
+  }
+  if(dynamic_cast<g2o::RobustKernelTukey*>(kernel)) {
+    return "Tukey";
+  }
+  if(dynamic_cast<g2o::RobustKernelWelsch*>(kernel)) {
+    return "Welsch";
+  }
+  return "";
+}
+
+bool save_robust_kernels(const std::string& filename, g2o::SparseOptimizer* graph) {
+  std::ofstream ofs(filename);
+  if(!ofs) {
+    std::cerr << "failed to open output stream!!" << std::endl;
+    return false;
+  }
+
+  for(const auto& edge_ : graph->edges()) {
+    g2o::OptimizableGraph::Edge* edge = static_cast<g2o::OptimizableGraph::Edge*>(edge_);
+    g2o::RobustKernel* kernel = edge->robustKernel();
+    if(!kernel) {
+      continue;
+    }
+
+    std::string type = kernel_type(kernel);
+    if(type.empty()) {
+      std::cerr << "unknown kernel type!!" << std::endl;
+      continue;
+    }
+
+    ofs << edge->vertices().size() << " ";
+    for(size_t i = 0; i < edge->vertices().size(); i++) {
+      ofs << edge->vertices()[i]->id() << " ";
+    }
+    ofs << type << " " << kernel->delta() << std::endl;
+  }
+
+  return true;
+}
+
+class KernelData {
+public:
+  KernelData(const std::string& line) {
+    std::stringstream sst(line);
+    size_t num_vertices;
+    sst >> num_vertices;
+
+    vertex_indices.resize(num_vertices);
+    for(size_t i = 0; i < num_vertices; i++) {
+      sst >> vertex_indices[i];
+    }
+
+    sst >> type >> delta;
+  }
+
+  bool match(g2o::OptimizableGraph::Edge* edge) const {
+    if(edge->vertices().size() != vertex_indices.size()) {
+      return false;
+    }
+
+    for(size_t i = 0; i < edge->vertices().size(); i++) {
+      if(edge->vertices()[i]->id() != vertex_indices[i]) {
+        return false;
+      }
+    }
+
+    return true;
+  }
+
+  g2o::RobustKernel* create() const {
+    auto factory = g2o::RobustKernelFactory::instance();
+    g2o::RobustKernel* kernel = factory->construct(type);
+    kernel->setDelta(delta);
+
+    return kernel;
+  }
+
+public:
+  std::vector<int> vertex_indices;
+  std::string type;
+  double delta;
+};
+
+bool load_robust_kernels(const std::string& filename, g2o::SparseOptimizer* graph) {
+  std::ifstream ifs(filename);
+  if(!ifs) {
+    std::cerr << "warning: failed to open input stream!!" << std::endl;
+    return true;
+  }
+
+  std::vector<KernelData> kernels;
+
+  while(!ifs.eof()) {
+    std::string line;
+    std::getline(ifs, line);
+    if(line.empty()) {
+      continue;
+    }
+
+    kernels.push_back(KernelData(line));
+  }
+  std::cout << "kernels: " << kernels.size() << std::endl;
+
+  for(auto& edge_ : graph->edges()) {
+    g2o::OptimizableGraph::Edge* edge = static_cast<g2o::OptimizableGraph::Edge*>(edge_);
+
+    for(auto itr = kernels.begin(); itr != kernels.end(); itr++) {
+      if(itr->match(edge)) {
+        edge->setRobustKernel(itr->create());
+        kernels.erase(itr);
+        break;
+      }
+    }
+  }
+
+  if(kernels.size() != 0) {
+    std::cerr << "warning: there is non-associated kernels!!" << std::endl;
+  }
+  return true;
+}
+
+}  // namespace g2o

src/hdl_graph_slam/bag_player.py

@@ -0,0 +1,223 @@
+#!/usr/bin/python
+# SPDX-License-Identifier: BSD-2-Clause
+import sys
+import yaml
+import time
+import curses
+import argparse
+
+try:
+    from StringIO import StringIO ## for Python 2
+except ImportError:
+    from io import StringIO ## for Python 3
+
+
+import rospy
+import rosbag
+import roslib
+from std_msgs.msg import *
+from sensor_msgs.msg import *
+from rosgraph_msgs.msg import *
+from progressbar import ProgressBar
+
+
+class BagPlayer:
+	def __init__(self, bagfile, start, duration):
+		print('opening...',)
+		self.bag = rosbag.Bag(bagfile, 'r')
+		print('done')
+
+		self.message_generator = self.bag.read_messages()
+
+		info_dict = yaml.safe_load(self.bag._get_yaml_info())
+		self.duration = float(info_dict['duration'])
+		self.endtime = float(info_dict['end'])
+
+		self.progress = ProgressBar(0, self.duration, term_width=80, fd=StringIO())
+
+		self.publishers = {}
+		for con in self.bag._get_connections():
+			msg_class = roslib.message.get_message_class(con.datatype)
+			self.publishers[con.topic] = rospy.Publisher(con.topic, msg_class, queue_size=256)
+		self.clock_pub = rospy.Publisher('/clock', Clock, queue_size=256)
+
+		self.init_time = None
+		self.start = start
+		self.duration = duration
+		self.fail_count = 0
+		self.time_subs = {}
+		self.target_times = {}
+		self.latest_stamps = {}
+
+		self.play()
+
+	def update_time_subs(self):
+		for topic_name, msg_type in rospy.get_published_topics():
+			if 'read_until' in topic_name and 'std_msgs/Header' in msg_type:
+				if topic_name not in self.time_subs:
+					print('ADD', topic_name)
+					self.time_subs[topic_name] = rospy.Subscriber(topic_name, Header, self.time_callback, topic_name)
+
+	def time_callback(self, header_msg, topic_name):
+		if header_msg.frame_id not in self.target_times:
+			self.target_times[header_msg.frame_id] = {}
+
+		if topic_name not in self.target_times[header_msg.frame_id] or self.target_times[header_msg.frame_id][topic_name] < header_msg.stamp:
+			self.target_times[header_msg.frame_id][topic_name] = header_msg.stamp
+
+	def play_realtime(self, duration):
+		topic, msg, stamp = next(self.message_generator)
+		stamp_wall = time.time()
+
+		start_stamp = stamp
+		start_stamp_wall = stamp_wall
+
+		self.start_stamp = start_stamp
+
+		while not rospy.is_shutdown() and (stamp - start_stamp) < duration:
+			stamp_wall = time.time()
+			elapsed_stamp = stamp - start_stamp
+			if (stamp_wall - start_stamp_wall) < (elapsed_stamp.secs + elapsed_stamp.nsecs * 1e-9):
+				time.sleep(1e-6)
+				self.update_time_subs()
+				continue
+
+			clock_msg = Clock()
+			clock_msg.clock = stamp
+
+			if self.init_time is None:
+				self.init_time = stamp
+
+			if self.start and (stamp - self.init_time) < rospy.Duration(float(self.start)):
+				start_stamp = stamp
+			else:
+				self.clock_pub.publish(clock_msg)
+				self.publishers[topic].publish(msg)
+
+			topic, msg, stamp = next(self.message_generator)
+
+		return topic, msg, stamp
+
+	def print_progress(self, stamp):
+		self.stdscr.clear()
+		self.stdscr.addstr(0, 0, 'topic')
+		self.stdscr.addstr(0, 25, 'time')
+
+		line = 1
+		for target in self.target_times:
+			if target not in self.publishers:
+				continue
+
+			for sub_name in self.target_times[target]:
+				target_time = self.target_times[target][sub_name]
+				self.stdscr.addstr(line, 0, sub_name[:-11])
+				self.stdscr.addstr(line, 25, '%.6f' % (target_time.secs + target_time.nsecs * 1e-9))
+
+				residual = target_time - self.latest_stamps[target].stamp
+
+				color = 1 if residual.to_sec() > 0.0 else 2
+				self.stdscr.addstr(line, 50, '%.5f' % residual.to_sec(), curses.color_pair(color))
+				line += 1
+
+		if not hasattr(self, 'prev_stamp'):
+			self.prev_stamp = stamp
+			self.prev_stamp_wall = time.time()
+			self.processing_speed = 1.0
+		elif time.time() - self.prev_stamp_wall > 1.0:
+			sim_duration = (stamp - self.prev_stamp).to_sec()
+			wall_duration = time.time() - self.prev_stamp_wall
+			self.processing_speed = sim_duration / wall_duration
+
+		self.stdscr.addstr(line, 0, 'current_stamp')
+		self.stdscr.addstr(line, 25, '%.6f' % stamp.to_sec())
+		self.stdscr.addstr(line, 50, '(x%.2f)' % self.processing_speed)
+
+		elapsed = (stamp - self.start_stamp).to_sec()
+		self.progress.fd = StringIO()
+		try:
+			self.progress.update(elapsed)
+		except:
+			# nothing to do
+			pass
+		self.stdscr.addstr(line + 1, 0, '----------')
+		self.stdscr.addstr(line + 2, 0, self.progress.fd.getvalue())
+
+		self.stdscr.refresh()
+
+	def check_stamp(self, topic, msg):
+		if topic not in self.target_times:
+			return True
+
+		if self.fail_count > 10:
+			self.fail_count = 0
+			return True
+
+		target_time_map = self.target_times[topic]
+		for sub_name in target_time_map:
+			self.latest_stamps[topic] = msg.header
+			if msg.header.stamp > target_time_map[sub_name]:
+				self.fail_count += 1
+				return False
+
+		self.fail_count = 0
+		return True
+
+	def play(self):
+		print('play realtime for 3.0[sec]')
+		topic, msg, stamp = self.play_realtime(rospy.Duration(5.0))
+		self.update_time_subs()
+
+		print('play as fast as possible')
+		self.stdscr = curses.initscr()
+		curses.start_color()
+		curses.noecho()
+
+		curses.init_pair(1, curses.COLOR_BLUE, curses.COLOR_WHITE)
+		curses.init_pair(2, curses.COLOR_RED, curses.COLOR_WHITE)
+
+		try:
+			while not rospy.is_shutdown():
+				if not self.check_stamp(topic, msg):
+					self.update_time_subs()
+					self.print_progress(stamp)
+					time.sleep(0.1)
+					continue
+
+				clock_msg = Clock()
+				clock_msg.clock = stamp
+
+				if self.duration:
+					if (stamp - self.init_time) > rospy.Duration(float(self.duration)):
+						break
+
+				self.clock_pub.publish(clock_msg)
+				self.publishers[topic].publish(msg)
+				topic, msg, stamp = next(self.message_generator)
+		except:
+			print(sys.exc_info()[0])
+			clock_msg = Clock()
+			clock_msg.clock = stamp + rospy.Duration(30.0)
+			self.clock_pub.publish(clock_msg)
+			time.sleep(0.5)
+
+		curses.echo()
+		curses.endwin()
+
+
+def main():
+	myargv = rospy.myargv(sys.argv)
+	parser = argparse.ArgumentParser()
+	parser.add_argument('input_bag', help='bag file to be played')
+	parser.add_argument('-s', '--start', help='start sec seconds into the bag')
+	parser.add_argument('-u', '--duration', help='play only sec seconds into the bag')
+	args = parser.parse_args(myargv[1:])
+
+	if len(sys.argv) < 2:
+		print('usage bag_player src_bagname')
+		return
+
+	rospy.init_node('bag_player')
+	BagPlayer(args.input_bag, args.start, args.duration)
+
+if __name__ == '__main__':
+	main()

src/hdl_graph_slam/ford2bag.py

@@ -0,0 +1,96 @@
+#!/usr/bin/python
+# SPDX-License-Identifier: BSD-2-Clause
+import re
+import os
+import sys
+import struct
+import numpy
+import scipy.io
+
+import rospy
+import rosbag
+import progressbar
+import sensor_msgs.point_cloud2 as pc2
+
+from sensor_msgs.msg import NavSatFix, NavSatStatus, PointCloud2
+from geographic_msgs.msg import GeoPointStamped
+
+
+def gps2navsat(filename, bag):
+	with open(filename, 'r') as file:
+		try:
+			while True:
+				data = struct.unpack('qddd', file.read(8*4))
+				time = data[0]
+				local = data[1:]
+				lat_lon_el_theta = struct.unpack('dddd', file.read(8*4))
+				gps_cov = struct.unpack('dddddddddddddddd', file.read(8*16))
+
+				if abs(lat_lon_el_theta[0]) < 1e-1:
+					continue
+
+				navsat = NavSatFix()
+				navsat.header.frame_id = 'gps'
+				navsat.header.stamp = rospy.Time.from_sec(time * 1e-6)
+				navsat.status.status = NavSatStatus.STATUS_FIX
+				navsat.status.service = NavSatStatus.SERVICE_GPS
+
+				navsat.latitude = lat_lon_el_theta[0]
+				navsat.longitude = lat_lon_el_theta[1]
+				navsat.altitude = lat_lon_el_theta[2]
+
+				navsat.position_covariance = numpy.array(gps_cov).reshape(4, 4)[:3, :3].flatten().tolist()
+				navsat.position_covariance_type = NavSatFix.COVARIANCE_TYPE_KNOWN
+
+				bag.write('/gps/fix', navsat, navsat.header.stamp)
+
+				geopoint = GeoPointStamped()
+				geopoint.header = navsat.header
+				geopoint.position.latitude = lat_lon_el_theta[0]
+				geopoint.position.longitude = lat_lon_el_theta[1]
+				geopoint.position.altitude = lat_lon_el_theta[2]
+
+				bag.write('/gps/geopoint', geopoint, geopoint.header.stamp)
+
+		except:
+			print 'done'
+
+
+def mat2pointcloud(filename):
+	m = scipy.io.loadmat(filename)
+	scan = numpy.transpose(m['SCAN']['XYZ'][0][0]).astype(numpy.float32)
+	stamp = m['SCAN']['timestamp_laser'][0][0][0][0] * 1e-6
+
+	cloud = PointCloud2()
+	cloud.header.stamp = rospy.Time.from_sec(stamp)
+	cloud.header.frame_id = 'velodyne'
+	cloud = pc2.create_cloud_xyz32(cloud.header, scan)
+	return cloud
+
+
+def main():
+	if len(sys.argv) < 2:
+		print 'usage: ford2bag.py src_dirname output_filename'
+
+	output_filename = sys.argv[1]
+
+	rospy.init_node('bag')
+	filenames = sorted(['SCANS/' + x for x in os.listdir('SCANS') if re.match('Scan[0-9]*\.mat', x)])
+	print filenames
+
+	progress = progressbar.ProgressBar(max_value=len(filenames))
+	pub = rospy.Publisher('/velodyne_points', PointCloud2, queue_size=32)
+	with rosbag.Bag(output_filename, 'w') as bag:
+		gps2navsat('GPS.log', bag)
+		for i, filename in enumerate(filenames):
+			if rospy.is_shutdown():
+				break
+			progress.update(i)
+			cloud = mat2pointcloud(filename)
+			if pub.get_num_connections():
+				pub.publish(cloud)
+			bag.write('/velodyne_points', cloud, cloud.header.stamp)
+
+
+if __name__ == '__main__':
+	main()

src/hdl_graph_slam/graph_slam.cpp

@@ -0,0 +1,351 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#include <hdl_graph_slam/graph_slam.hpp>
+
+#include <boost/format.hpp>
+#include <g2o/stuff/macros.h>
+#include <g2o/core/factory.h>
+#include <g2o/core/block_solver.h>
+#include <g2o/core/linear_solver.h>
+#include <g2o/core/sparse_optimizer.h>
+#include <g2o/core/robust_kernel_factory.h>
+#include <g2o/core/optimization_algorithm.h>
+#include <g2o/core/optimization_algorithm_factory.h>
+#include <g2o/solvers/pcg/linear_solver_pcg.h>
+#include <g2o/types/slam3d/types_slam3d.h>
+#include <g2o/types/slam3d/edge_se3_pointxyz.h>
+#include <g2o/types/slam3d_addons/types_slam3d_addons.h>
+#include <g2o/edge_se3_plane.hpp>
+#include <g2o/edge_se3_priorxy.hpp>
+#include <g2o/edge_se3_priorxyz.hpp>
+#include <g2o/edge_se3_priorvec.hpp>
+#include <g2o/edge_se3_priorquat.hpp>
+#include <g2o/edge_plane_prior.hpp>
+#include <g2o/edge_plane_identity.hpp>
+#include <g2o/edge_plane_parallel.hpp>
+#include <g2o/robust_kernel_io.hpp>
+
+G2O_USE_OPTIMIZATION_LIBRARY(pcg)
+G2O_USE_OPTIMIZATION_LIBRARY(cholmod)  // be aware of that cholmod brings GPL dependency
+G2O_USE_OPTIMIZATION_LIBRARY(csparse)  // be aware of that csparse brings LGPL unless it is dynamically linked
+
+namespace g2o {
+G2O_REGISTER_TYPE(EDGE_SE3_PLANE, EdgeSE3Plane)
+G2O_REGISTER_TYPE(EDGE_SE3_PRIORXY, EdgeSE3PriorXY)
+G2O_REGISTER_TYPE(EDGE_SE3_PRIORXYZ, EdgeSE3PriorXYZ)
+G2O_REGISTER_TYPE(EDGE_SE3_PRIORVEC, EdgeSE3PriorVec)
+G2O_REGISTER_TYPE(EDGE_SE3_PRIORQUAT, EdgeSE3PriorQuat)
+G2O_REGISTER_TYPE(EDGE_PLANE_PRIOR_NORMAL, EdgePlanePriorNormal)
+G2O_REGISTER_TYPE(EDGE_PLANE_PRIOR_DISTANCE, EdgePlanePriorDistance)
+G2O_REGISTER_TYPE(EDGE_PLANE_IDENTITY, EdgePlaneIdentity)
+G2O_REGISTER_TYPE(EDGE_PLANE_PARALLEL, EdgePlaneParallel)
+G2O_REGISTER_TYPE(EDGE_PLANE_PAERPENDICULAR, EdgePlanePerpendicular)
+}  // namespace g2o
+
+namespace hdl_graph_slam {
+
+/**
+ * @brief constructor
+ */
+GraphSLAM::GraphSLAM(const std::string& solver_type) {
+  graph.reset(new g2o::SparseOptimizer());
+  g2o::SparseOptimizer* graph = dynamic_cast<g2o::SparseOptimizer*>(this->graph.get());
+
+  std::cout << "construct solver: " << solver_type << std::endl;
+  g2o::OptimizationAlgorithmFactory* solver_factory = g2o::OptimizationAlgorithmFactory::instance();
+  g2o::OptimizationAlgorithmProperty solver_property;
+  g2o::OptimizationAlgorithm* solver = solver_factory->construct(solver_type, solver_property);
+  graph->setAlgorithm(solver);
+
+  if(!graph->solver()) {
+    std::cerr << std::endl;
+    std::cerr << "error : failed to allocate solver!!" << std::endl;
+    solver_factory->listSolvers(std::cerr);
+    std::cerr << "-------------" << std::endl;
+    std::cin.ignore(1);
+    return;
+  }
+  std::cout << "done" << std::endl;
+
+  robust_kernel_factory = g2o::RobustKernelFactory::instance();
+}
+
+/**
+ * @brief destructor
+ */
+GraphSLAM::~GraphSLAM() {
+  graph.reset();
+}
+
+void GraphSLAM::set_solver(const std::string& solver_type) {
+  g2o::SparseOptimizer* graph = dynamic_cast<g2o::SparseOptimizer*>(this->graph.get());
+
+  std::cout << "construct solver: " << solver_type << std::endl;
+  g2o::OptimizationAlgorithmFactory* solver_factory = g2o::OptimizationAlgorithmFactory::instance();
+  g2o::OptimizationAlgorithmProperty solver_property;
+  g2o::OptimizationAlgorithm* solver = solver_factory->construct(solver_type, solver_property);
+  graph->setAlgorithm(solver);
+
+  if(!graph->solver()) {
+    std::cerr << std::endl;
+    std::cerr << "error : failed to allocate solver!!" << std::endl;
+    solver_factory->listSolvers(std::cerr);
+    std::cerr << "-------------" << std::endl;
+    std::cin.ignore(1);
+    return;
+  }
+  std::cout << "done" << std::endl;
+}
+
+int GraphSLAM::num_vertices() const {
+  return graph->vertices().size();
+}
+int GraphSLAM::num_edges() const {
+  return graph->edges().size();
+}
+
+g2o::VertexSE3* GraphSLAM::add_se3_node(const Eigen::Isometry3d& pose) {
+  g2o::VertexSE3* vertex(new g2o::VertexSE3());
+  vertex->setId(static_cast<int>(graph->vertices().size()));
+  vertex->setEstimate(pose);
+  graph->addVertex(vertex);
+
+  return vertex;
+}
+
+g2o::VertexPlane* GraphSLAM::add_plane_node(const Eigen::Vector4d& plane_coeffs) {
+  g2o::VertexPlane* vertex(new g2o::VertexPlane());
+  vertex->setId(static_cast<int>(graph->vertices().size()));
+  vertex->setEstimate(plane_coeffs);
+  graph->addVertex(vertex);
+
+  return vertex;
+}
+
+g2o::VertexPointXYZ* GraphSLAM::add_point_xyz_node(const Eigen::Vector3d& xyz) {
+  g2o::VertexPointXYZ* vertex(new g2o::VertexPointXYZ());
+  vertex->setId(static_cast<int>(graph->vertices().size()));
+  vertex->setEstimate(xyz);
+  graph->addVertex(vertex);
+
+  return vertex;
+}
+
+g2o::EdgeSE3* GraphSLAM::add_se3_edge(g2o::VertexSE3* v1, g2o::VertexSE3* v2, const Eigen::Isometry3d& relative_pose, const Eigen::MatrixXd& information_matrix) {
+  g2o::EdgeSE3* edge(new g2o::EdgeSE3());
+  edge->setMeasurement(relative_pose);
+  edge->setInformation(information_matrix);
+  edge->vertices()[0] = v1;
+  edge->vertices()[1] = v2;
+  graph->addEdge(edge);
+
+  return edge;
+}
+
+g2o::EdgeSE3Plane* GraphSLAM::add_se3_plane_edge(g2o::VertexSE3* v_se3, g2o::VertexPlane* v_plane, const Eigen::Vector4d& plane_coeffs, const Eigen::MatrixXd& information_matrix) {
+  g2o::EdgeSE3Plane* edge(new g2o::EdgeSE3Plane());
+  edge->setMeasurement(plane_coeffs);
+  edge->setInformation(information_matrix);
+  edge->vertices()[0] = v_se3;
+  edge->vertices()[1] = v_plane;
+  graph->addEdge(edge);
+
+  return edge;
+}
+
+g2o::EdgeSE3PointXYZ* GraphSLAM::add_se3_point_xyz_edge(g2o::VertexSE3* v_se3, g2o::VertexPointXYZ* v_xyz, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix) {
+  g2o::EdgeSE3PointXYZ* edge(new g2o::EdgeSE3PointXYZ());
+  edge->setMeasurement(xyz);
+  edge->setInformation(information_matrix);
+  edge->vertices()[0] = v_se3;
+  edge->vertices()[1] = v_xyz;
+  graph->addEdge(edge);
+
+  return edge;
+}
+
+g2o::EdgePlanePriorNormal* GraphSLAM::add_plane_normal_prior_edge(g2o::VertexPlane* v, const Eigen::Vector3d& normal, const Eigen::MatrixXd& information_matrix) {
+  g2o::EdgePlanePriorNormal* edge(new g2o::EdgePlanePriorNormal());
+  edge->setMeasurement(normal);
+  edge->setInformation(information_matrix);
+  edge->vertices()[0] = v;
+  graph->addEdge(edge);
+
+  return edge;
+}
+
+g2o::EdgePlanePriorDistance* GraphSLAM::add_plane_distance_prior_edge(g2o::VertexPlane* v, double distance, const Eigen::MatrixXd& information_matrix) {
+  g2o::EdgePlanePriorDistance* edge(new g2o::EdgePlanePriorDistance());
+  edge->setMeasurement(distance);
+  edge->setInformation(information_matrix);
+  edge->vertices()[0] = v;
+  graph->addEdge(edge);
+
+  return edge;
+}
+
+g2o::EdgeSE3PriorXY* GraphSLAM::add_se3_prior_xy_edge(g2o::VertexSE3* v_se3, const Eigen::Vector2d& xy, const Eigen::MatrixXd& information_matrix) {
+  g2o::EdgeSE3PriorXY* edge(new g2o::EdgeSE3PriorXY());
+  edge->setMeasurement(xy);
+  edge->setInformation(information_matrix);
+  edge->vertices()[0] = v_se3;
+  graph->addEdge(edge);
+
+  return edge;
+}
+
+g2o::EdgeSE3PriorXYZ* GraphSLAM::add_se3_prior_xyz_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& xyz, const Eigen::MatrixXd& information_matrix) {
+  g2o::EdgeSE3PriorXYZ* edge(new g2o::EdgeSE3PriorXYZ());
+  edge->setMeasurement(xyz);
+  edge->setInformation(information_matrix);
+  edge->vertices()[0] = v_se3;
+  graph->addEdge(edge);
+
+  return edge;
+}
+
+g2o::EdgeSE3PriorVec* GraphSLAM::add_se3_prior_vec_edge(g2o::VertexSE3* v_se3, const Eigen::Vector3d& direction, const Eigen::Vector3d& measurement, const Eigen::MatrixXd& information_matrix) {
+  Eigen::Matrix<double, 6, 1> m;
+  m.head<3>() = direction;
+  m.tail<3>() = measurement;
+
+  g2o::EdgeSE3PriorVec* edge(new g2o::EdgeSE3PriorVec());
+  edge->setMeasurement(m);
+  edge->setInformation(information_matrix);
+  edge->vertices()[0] = v_se3;
+  graph->addEdge(edge);
+
+  return edge;
+}
+
+g2o::EdgeSE3PriorQuat* GraphSLAM::add_se3_prior_quat_edge(g2o::VertexSE3* v_se3, const Eigen::Quaterniond& quat, const Eigen::MatrixXd& information_matrix) {
+  g2o::EdgeSE3PriorQuat* edge(new g2o::EdgeSE3PriorQuat());
+  edge->setMeasurement(quat);
+  edge->setInformation(information_matrix);
+  edge->vertices()[0] = v_se3;
+  graph->addEdge(edge);
+
+  return edge;
+}
+
+g2o::EdgePlane* GraphSLAM::add_plane_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector4d& measurement, const Eigen::Matrix4d& information) {
+  g2o::EdgePlane* edge(new g2o::EdgePlane());
+  edge->setMeasurement(measurement);
+  edge->setInformation(information);
+  edge->vertices()[0] = v_plane1;
+  edge->vertices()[1] = v_plane2;
+  graph->addEdge(edge);
+
+  return edge;
+}
+
+g2o::EdgePlaneIdentity* GraphSLAM::add_plane_identity_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector4d& measurement, const Eigen::Matrix4d& information) {
+  g2o::EdgePlaneIdentity* edge(new g2o::EdgePlaneIdentity());
+  edge->setMeasurement(measurement);
+  edge->setInformation(information);
+  edge->vertices()[0] = v_plane1;
+  edge->vertices()[1] = v_plane2;
+  graph->addEdge(edge);
+
+  return edge;
+}
+
+g2o::EdgePlaneParallel* GraphSLAM::add_plane_parallel_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector3d& measurement, const Eigen::Matrix3d& information) {
+  g2o::EdgePlaneParallel* edge(new g2o::EdgePlaneParallel());
+  edge->setMeasurement(measurement);
+  edge->setInformation(information);
+  edge->vertices()[0] = v_plane1;
+  edge->vertices()[1] = v_plane2;
+  graph->addEdge(edge);
+
+  return edge;
+}
+
+g2o::EdgePlanePerpendicular* GraphSLAM::add_plane_perpendicular_edge(g2o::VertexPlane* v_plane1, g2o::VertexPlane* v_plane2, const Eigen::Vector3d& measurement, const Eigen::MatrixXd& information) {
+  g2o::EdgePlanePerpendicular* edge(new g2o::EdgePlanePerpendicular());
+  edge->setMeasurement(measurement);
+  edge->setInformation(information);
+  edge->vertices()[0] = v_plane1;
+  edge->vertices()[1] = v_plane2;
+  graph->addEdge(edge);
+
+  return edge;
+}
+
+void GraphSLAM::add_robust_kernel(g2o::HyperGraph::Edge* edge, const std::string& kernel_type, double kernel_size) {
+  if(kernel_type == "NONE") {
+    return;
+  }
+
+  g2o::RobustKernel* kernel = robust_kernel_factory->construct(kernel_type);
+  if(kernel == nullptr) {
+    std::cerr << "warning : invalid robust kernel type: " << kernel_type << std::endl;
+    return;
+  }
+
+  kernel->setDelta(kernel_size);
+
+  g2o::OptimizableGraph::Edge* edge_ = dynamic_cast<g2o::OptimizableGraph::Edge*>(edge);
+  edge_->setRobustKernel(kernel);
+}
+
+int GraphSLAM::optimize(int num_iterations) {
+  g2o::SparseOptimizer* graph = dynamic_cast<g2o::SparseOptimizer*>(this->graph.get());
+  if(graph->edges().size() < 10) {
+    return -1;
+  }
+
+  std::cout << std::endl;
+  std::cout << "--- pose graph optimization ---" << std::endl;
+  std::cout << "nodes: " << graph->vertices().size() << "   edges: " << graph->edges().size() << std::endl;
+  std::cout << "optimizing... " << std::flush;
+
+  std::cout << "init" << std::endl;
+  graph->initializeOptimization();
+  graph->setVerbose(true);
+
+  std::cout << "chi2" << std::endl;
+  double chi2 = graph->chi2();
+
+  std::cout << "optimize!!" << std::endl;
+  auto t1 = ros::WallTime::now();
+  int iterations = graph->optimize(num_iterations);
+
+  auto t2 = ros::WallTime::now();
+  std::cout << "done" << std::endl;
+  std::cout << "iterations: " << iterations << " / " << num_iterations << std::endl;
+  std::cout << "chi2: (before)" << chi2 << " -> (after)" << graph->chi2() << std::endl;
+  std::cout << "time: " << boost::format("%.3f") % (t2 - t1).toSec() << "[sec]" << std::endl;
+
+  return iterations;
+}
+
+void GraphSLAM::save(const std::string& filename) {
+  g2o::SparseOptimizer* graph = dynamic_cast<g2o::SparseOptimizer*>(this->graph.get());
+
+  std::ofstream ofs(filename);
+  graph->save(ofs);
+
+  g2o::save_robust_kernels(filename + ".kernels", graph);
+}
+
+bool GraphSLAM::load(const std::string& filename) {
+  std::cout << "loading pose graph..." << std::endl;
+  g2o::SparseOptimizer* graph = dynamic_cast<g2o::SparseOptimizer*>(this->graph.get());
+
+  std::ifstream ifs(filename);
+  if(!graph->load(ifs)) {
+    return false;
+  }
+
+  std::cout << "nodes  : " << graph->vertices().size() << std::endl;
+  std::cout << "edges  : " << graph->edges().size() << std::endl;
+
+  if(!g2o::load_robust_kernels(filename + ".kernels", graph)) {
+    return false;
+  }
+
+  return true;
+}
+
+}  // namespace hdl_graph_slam

src/hdl_graph_slam/information_matrix_calculator.cpp

@@ -0,0 +1,82 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#include <hdl_graph_slam/information_matrix_calculator.hpp>
+
+#include <pcl/search/kdtree.h>
+#include <pcl/common/transforms.h>
+
+namespace hdl_graph_slam {
+
+InformationMatrixCalculator::InformationMatrixCalculator(ros::NodeHandle& nh) {
+  use_const_inf_matrix = nh.param<bool>("use_const_inf_matrix", false);
+  const_stddev_x = nh.param<double>("const_stddev_x", 0.5);
+  const_stddev_q = nh.param<double>("const_stddev_q", 0.1);
+
+  var_gain_a = nh.param<double>("var_gain_a", 20.0);
+  min_stddev_x = nh.param<double>("min_stddev_x", 0.1);
+  max_stddev_x = nh.param<double>("max_stddev_x", 5.0);
+  min_stddev_q = nh.param<double>("min_stddev_q", 0.05);
+  max_stddev_q = nh.param<double>("max_stddev_q", 0.2);
+  fitness_score_thresh = nh.param<double>("fitness_score_thresh", 0.5);
+}
+
+InformationMatrixCalculator::~InformationMatrixCalculator() {}
+
+Eigen::MatrixXd InformationMatrixCalculator::calc_information_matrix(const pcl::PointCloud<PointT>::ConstPtr& cloud1, const pcl::PointCloud<PointT>::ConstPtr& cloud2, const Eigen::Isometry3d& relpose) const {
+  if(use_const_inf_matrix) {
+    Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6);
+    inf.topLeftCorner(3, 3).array() /= const_stddev_x;
+    inf.bottomRightCorner(3, 3).array() /= const_stddev_q;
+    return inf;
+  }
+
+  double fitness_score = calc_fitness_score(cloud1, cloud2, relpose);
+
+  double min_var_x = std::pow(min_stddev_x, 2);
+  double max_var_x = std::pow(max_stddev_x, 2);
+  double min_var_q = std::pow(min_stddev_q, 2);
+  double max_var_q = std::pow(max_stddev_q, 2);
+
+  float w_x = weight(var_gain_a, fitness_score_thresh, min_var_x, max_var_x, fitness_score);
+  float w_q = weight(var_gain_a, fitness_score_thresh, min_var_q, max_var_q, fitness_score);
+
+  Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6);
+  inf.topLeftCorner(3, 3).array() /= w_x;
+  inf.bottomRightCorner(3, 3).array() /= w_q;
+  return inf;
+}
+
+double InformationMatrixCalculator::calc_fitness_score(const pcl::PointCloud<PointT>::ConstPtr& cloud1, const pcl::PointCloud<PointT>::ConstPtr& cloud2, const Eigen::Isometry3d& relpose, double max_range) {
+  pcl::search::KdTree<PointT>::Ptr tree_(new pcl::search::KdTree<PointT>());
+  tree_->setInputCloud(cloud1);
+
+  double fitness_score = 0.0;
+
+  // Transform the input dataset using the final transformation
+  pcl::PointCloud<PointT> input_transformed;
+  pcl::transformPointCloud(*cloud2, input_transformed, relpose.cast<float>());
+
+  std::vector<int> nn_indices(1);
+  std::vector<float> nn_dists(1);
+
+  // For each point in the source dataset
+  int nr = 0;
+  for(size_t i = 0; i < input_transformed.points.size(); ++i) {
+    // Find its nearest neighbor in the target
+    tree_->nearestKSearch(input_transformed.points[i], 1, nn_indices, nn_dists);
+
+    // Deal with occlusions (incomplete targets)
+    if(nn_dists[0] <= max_range) {
+      // Add to the fitness score
+      fitness_score += nn_dists[0];
+      nr++;
+    }
+  }
+
+  if(nr > 0)
+    return (fitness_score / nr);
+  else
+    return (std::numeric_limits<double>::max());
+}
+
+}  // namespace hdl_graph_slam

src/hdl_graph_slam/keyframe.cpp

@@ -0,0 +1,161 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#include <hdl_graph_slam/keyframe.hpp>
+
+#include <boost/filesystem.hpp>
+
+#include <pcl/io/pcd_io.h>
+#include <g2o/core/sparse_optimizer.h>
+#include <g2o/types/slam3d/vertex_se3.h>
+
+namespace hdl_graph_slam {
+
+KeyFrame::KeyFrame(const ros::Time& stamp, const Eigen::Isometry3d& odom, double accum_distance, const pcl::PointCloud<PointT>::ConstPtr& cloud) : stamp(stamp), odom(odom), accum_distance(accum_distance), cloud(cloud), node(nullptr) {}
+
+KeyFrame::KeyFrame(const std::string& directory, g2o::HyperGraph* graph) : stamp(), odom(Eigen::Isometry3d::Identity()), accum_distance(-1), cloud(nullptr), node(nullptr) {
+  load(directory, graph);
+}
+
+KeyFrame::~KeyFrame() {}
+
+void KeyFrame::save(const std::string& directory) {
+  if(!boost::filesystem::is_directory(directory)) {
+    boost::filesystem::create_directory(directory);
+  }
+
+  std::ofstream ofs(directory + "/data");
+  ofs << "stamp " << stamp.sec << " " << stamp.nsec << "\n";
+
+  ofs << "estimate\n";
+  ofs << node->estimate().matrix() << "\n";
+
+  ofs << "odom\n";
+  ofs << odom.matrix() << "\n";
+
+  ofs << "accum_distance " << accum_distance << "\n";
+
+  if(floor_coeffs) {
+    ofs << "floor_coeffs " << floor_coeffs->transpose() << "\n";
+  }
+
+  if(utm_coord) {
+    ofs << "utm_coord " << utm_coord->transpose() << "\n";
+  }
+
+  if(acceleration) {
+    ofs << "acceleration " << acceleration->transpose() << "\n";
+  }
+
+  if(orientation) {
+    ofs << "orientation " << orientation->w() << " " << orientation->x() << " " << orientation->y() << " " << orientation->z() << "\n";
+  }
+
+  if(node) {
+    ofs << "id " << node->id() << "\n";
+  }
+
+  pcl::io::savePCDFileBinary(directory + "/cloud.pcd", *cloud);
+}
+
+bool KeyFrame::load(const std::string& directory, g2o::HyperGraph* graph) {
+  std::ifstream ifs(directory + "/data");
+  if(!ifs) {
+    return false;
+  }
+
+  long node_id = -1;
+  boost::optional<Eigen::Isometry3d> estimate;
+
+  while(!ifs.eof()) {
+    std::string token;
+    ifs >> token;
+
+    if(token == "stamp") {
+      ifs >> stamp.sec >> stamp.nsec;
+    } else if(token == "estimate") {
+      Eigen::Matrix4d mat;
+      for(int i = 0; i < 4; i++) {
+        for(int j = 0; j < 4; j++) {
+          ifs >> mat(i, j);
+        }
+      }
+      estimate = Eigen::Isometry3d::Identity();
+      estimate->linear() = mat.block<3, 3>(0, 0);
+      estimate->translation() = mat.block<3, 1>(0, 3);
+    } else if(token == "odom") {
+      Eigen::Matrix4d odom_mat = Eigen::Matrix4d::Identity();
+      for(int i = 0; i < 4; i++) {
+        for(int j = 0; j < 4; j++) {
+          ifs >> odom_mat(i, j);
+        }
+      }
+
+      odom.setIdentity();
+      odom.linear() = odom_mat.block<3, 3>(0, 0);
+      odom.translation() = odom_mat.block<3, 1>(0, 3);
+    } else if(token == "accum_distance") {
+      ifs >> accum_distance;
+    } else if(token == "floor_coeffs") {
+      Eigen::Vector4d coeffs;
+      ifs >> coeffs[0] >> coeffs[1] >> coeffs[2] >> coeffs[3];
+      floor_coeffs = coeffs;
+    } else if(token == "utm_coord") {
+      Eigen::Vector3d coord;
+      ifs >> coord[0] >> coord[1] >> coord[2];
+      utm_coord = coord;
+    } else if(token == "acceleration") {
+      Eigen::Vector3d acc;
+      ifs >> acc[0] >> acc[1] >> acc[2];
+      acceleration = acc;
+    } else if(token == "orientation") {
+      Eigen::Quaterniond quat;
+      ifs >> quat.w() >> quat.x() >> quat.y() >> quat.z();
+      orientation = quat;
+    } else if(token == "id") {
+      ifs >> node_id;
+    }
+  }
+
+  if(node_id < 0) {
+    ROS_ERROR_STREAM("invalid node id!!");
+    ROS_ERROR_STREAM(directory);
+    return false;
+  }
+
+  if(graph->vertices().find(node_id) == graph->vertices().end()) {
+    ROS_ERROR_STREAM("vertex ID=" << node_id << " does not exist!!");
+    return false;
+  }
+
+  node = dynamic_cast<g2o::VertexSE3*>(graph->vertices()[node_id]);
+  if(node == nullptr) {
+    ROS_ERROR_STREAM("failed to downcast!!");
+    return false;
+  }
+
+  if(estimate) {
+    node->setEstimate(*estimate);
+  }
+
+  pcl::PointCloud<PointT>::Ptr cloud_(new pcl::PointCloud<PointT>());
+  pcl::io::loadPCDFile(directory + "/cloud.pcd", *cloud_);
+  cloud = cloud_;
+
+  return true;
+}
+
+long KeyFrame::id() const {
+  return node->id();
+}
+
+Eigen::Isometry3d KeyFrame::estimate() const {
+  return node->estimate();
+}
+
+KeyFrameSnapshot::KeyFrameSnapshot(const Eigen::Isometry3d& pose, const pcl::PointCloud<PointT>::ConstPtr& cloud) : pose(pose), cloud(cloud) {}
+
+KeyFrameSnapshot::KeyFrameSnapshot(const KeyFrame::Ptr& key) : pose(key->node->estimate()), cloud(key->cloud) {}
+
+KeyFrameSnapshot::~KeyFrameSnapshot() {}
+
+}  // namespace hdl_graph_slam

src/hdl_graph_slam/map2odom_publisher.py

@@ -0,0 +1,48 @@
+#!/usr/bin/python
+# SPDX-License-Identifier: BSD-2-Clause
+import tf
+import rospy
+from geometry_msgs.msg import *
+
+
+class Map2OdomPublisher:
+	def __init__(self, odom_frame_id = 'odom', map_frame_id = 'map'):
+		self.default_odom_frame_id = odom_frame_id
+		self.default_map_frame_id = map_frame_id
+		self.broadcaster = tf.TransformBroadcaster()
+		self.subscriber = rospy.Subscriber('/hdl_graph_slam/odom2pub', TransformStamped, self.callback)
+
+	def callback(self, odom_msg):
+		self.odom_msg = odom_msg
+
+	def spin(self):
+		if not hasattr(self, 'odom_msg'):
+			self.broadcaster.sendTransform((0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), self.default_odom_frame_id, self.default_map_frame_id)
+			return
+
+		pose = self.odom_msg.transform
+		pos = (pose.translation.x, pose.translation.y, pose.translation.z)
+		quat = (pose.rotation.x, pose.rotation.y, pose.rotation.z, pose.rotation.w)
+
+		map_frame_id = self.odom_msg.header.frame_id
+		odom_frame_id = self.odom_msg.child_frame_id
+
+		self.broadcaster.sendTransform(pos, quat, rospy.Time.now(), odom_frame_id, map_frame_id)
+
+
+def main():
+	rospy.init_node('map2odom_publisher')
+
+	# get some parameters to define what default frame_id's should be used while we wait for our first odom message
+	map_frame_id = rospy.get_param('~map_frame_id', 'map')
+	odom_frame_id = rospy.get_param('~odom_frame_id', 'odom')
+
+	node = Map2OdomPublisher(odom_frame_id, map_frame_id)	
+
+	rate = rospy.Rate(10.0)
+	while not rospy.is_shutdown():
+		node.spin()
+		rate.sleep()
+
+if __name__ == '__main__':
+	main()

src/hdl_graph_slam/map_cloud_generator.cpp

@@ -0,0 +1,53 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#include <hdl_graph_slam/map_cloud_generator.hpp>
+
+#include <pcl/octree/octree_search.h>
+
+namespace hdl_graph_slam {
+
+MapCloudGenerator::MapCloudGenerator() {}
+
+MapCloudGenerator::~MapCloudGenerator() {}
+
+pcl::PointCloud<MapCloudGenerator::PointT>::Ptr MapCloudGenerator::generate(const std::vector<KeyFrameSnapshot::Ptr>& keyframes, double resolution) const {
+  if(keyframes.empty()) {
+    std::cerr << "warning: keyframes empty!!" << std::endl;
+    return nullptr;
+  }
+
+  pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
+  cloud->reserve(keyframes.front()->cloud->size() * keyframes.size());
+
+  for(const auto& keyframe : keyframes) {
+    Eigen::Matrix4f pose = keyframe->pose.matrix().cast<float>();
+    for(const auto& src_pt : keyframe->cloud->points) {
+      PointT dst_pt;
+      dst_pt.getVector4fMap() = pose * src_pt.getVector4fMap();
+      dst_pt.intensity = src_pt.intensity;
+      cloud->push_back(dst_pt);
+    }
+  }
+
+  cloud->width = cloud->size();
+  cloud->height = 1;
+  cloud->is_dense = false;
+
+  if (resolution <=0.0)
+    return cloud; // To get unfiltered point cloud with intensity
+
+  pcl::octree::OctreePointCloud<PointT> octree(resolution);
+  octree.setInputCloud(cloud);
+  octree.addPointsFromInputCloud();
+
+  pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
+  octree.getOccupiedVoxelCenters(filtered->points);
+
+  filtered->width = filtered->size();
+  filtered->height = 1;
+  filtered->is_dense = false;
+
+  return filtered;
+}
+
+}  // namespace hdl_graph_slam

src/hdl_graph_slam/registrations.cpp

@@ -0,0 +1,126 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#include <hdl_graph_slam/registrations.hpp>
+
+#include <iostream>
+
+#include <pcl/registration/ndt.h>
+#include <pcl/registration/icp.h>
+#include <pcl/registration/gicp.h>
+
+#include <pclomp/ndt_omp.h>
+#include <pclomp/gicp_omp.h>
+#include <fast_gicp/gicp/fast_gicp.hpp>
+#include <fast_gicp/gicp/fast_vgicp.hpp>
+
+#ifdef USE_VGICP_CUDA
+#include <fast_gicp/gicp/fast_vgicp_cuda.hpp>
+#endif
+
+namespace hdl_graph_slam {
+
+pcl::Registration<pcl::PointXYZI, pcl::PointXYZI>::Ptr select_registration_method(ros::NodeHandle& pnh) {
+  using PointT = pcl::PointXYZI;
+
+  // select a registration method (ICP, GICP, NDT)
+  std::string registration_method = pnh.param<std::string>("registration_method", "NDT_OMP");
+  if(registration_method == "FAST_GICP") {    // 源码是用这个方法
+    std::cout << "registration: FAST_GICP" << std::endl;
+    fast_gicp::FastGICP<PointT, PointT>::Ptr gicp(new fast_gicp::FastGICP<PointT, PointT>());
+    gicp->setNumThreads(pnh.param<int>("reg_num_threads", 0));
+    gicp->setTransformationEpsilon(pnh.param<double>("reg_transformation_epsilon", 0.01));
+    gicp->setMaximumIterations(pnh.param<int>("reg_maximum_iterations", 64));
+    gicp->setMaxCorrespondenceDistance(pnh.param<double>("reg_max_correspondence_distance", 2.5));
+    gicp->setCorrespondenceRandomness(pnh.param<int>("reg_correspondence_randomness", 20));
+    return gicp;
+  }
+#ifdef USE_VGICP_CUDA
+  else if(registration_method == "FAST_VGICP_CUDA") {
+    std::cout << "registration: FAST_VGICP_CUDA" << std::endl;
+    fast_gicp::FastVGICPCuda<PointT, PointT>::Ptr vgicp(new fast_gicp::FastVGICPCuda<PointT, PointT>());
+    vgicp->setResolution(pnh.param<double>("reg_resolution", 1.0));
+    vgicp->setTransformationEpsilon(pnh.param<double>("reg_transformation_epsilon", 0.01));
+    vgicp->setMaximumIterations(pnh.param<int>("reg_maximum_iterations", 64));
+    vgicp->setCorrespondenceRandomness(pnh.param<int>("reg_correspondence_randomness", 20));
+    return vgicp;
+  }
+#endif
+  else if(registration_method == "FAST_VGICP") {
+    std::cout << "registration: FAST_VGICP" << std::endl;
+    fast_gicp::FastVGICP<PointT, PointT>::Ptr vgicp(new fast_gicp::FastVGICP<PointT, PointT>());
+    vgicp->setNumThreads(pnh.param<int>("reg_num_threads", 0));
+    vgicp->setResolution(pnh.param<double>("reg_resolution", 1.0));
+    vgicp->setTransformationEpsilon(pnh.param<double>("reg_transformation_epsilon", 0.01));
+    vgicp->setMaximumIterations(pnh.param<int>("reg_maximum_iterations", 64));
+    vgicp->setCorrespondenceRandomness(pnh.param<int>("reg_correspondence_randomness", 20));
+    return vgicp;
+  } else if(registration_method == "ICP") {
+    std::cout << "registration: ICP" << std::endl;
+    pcl::IterativeClosestPoint<PointT, PointT>::Ptr icp(new pcl::IterativeClosestPoint<PointT, PointT>());
+    icp->setTransformationEpsilon(pnh.param<double>("reg_transformation_epsilon", 0.01));
+    icp->setMaximumIterations(pnh.param<int>("reg_maximum_iterations", 64));
+    icp->setMaxCorrespondenceDistance(pnh.param<double>("reg_max_correspondence_distance", 2.5));
+    icp->setUseReciprocalCorrespondences(pnh.param<bool>("reg_use_reciprocal_correspondences", false));
+    return icp;
+  } else if(registration_method.find("GICP") != std::string::npos) {
+    if(registration_method.find("OMP") == std::string::npos) {
+      std::cout << "registration: GICP" << std::endl;
+      pcl::GeneralizedIterativeClosestPoint<PointT, PointT>::Ptr gicp(new pcl::GeneralizedIterativeClosestPoint<PointT, PointT>());
+      gicp->setTransformationEpsilon(pnh.param<double>("reg_transformation_epsilon", 0.01));
+      gicp->setMaximumIterations(pnh.param<int>("reg_maximum_iterations", 64));
+      gicp->setUseReciprocalCorrespondences(pnh.param<bool>("reg_use_reciprocal_correspondences", false));
+      gicp->setMaxCorrespondenceDistance(pnh.param<double>("reg_max_correspondence_distance", 2.5));
+      gicp->setCorrespondenceRandomness(pnh.param<int>("reg_correspondence_randomness", 20));
+      gicp->setMaximumOptimizerIterations(pnh.param<int>("reg_max_optimizer_iterations", 20));
+      return gicp;
+    } else {
+      std::cout << "registration: GICP_OMP" << std::endl;
+      pclomp::GeneralizedIterativeClosestPoint<PointT, PointT>::Ptr gicp(new pclomp::GeneralizedIterativeClosestPoint<PointT, PointT>());
+      gicp->setTransformationEpsilon(pnh.param<double>("reg_transformation_epsilon", 0.01));
+      gicp->setMaximumIterations(pnh.param<int>("reg_maximum_iterations", 64));
+      gicp->setUseReciprocalCorrespondences(pnh.param<bool>("reg_use_reciprocal_correspondences", false));
+      gicp->setMaxCorrespondenceDistance(pnh.param<double>("reg_max_correspondence_distance", 2.5));
+      gicp->setCorrespondenceRandomness(pnh.param<int>("reg_correspondence_randomness", 20));
+      gicp->setMaximumOptimizerIterations(pnh.param<int>("reg_max_optimizer_iterations", 20));
+      return gicp;
+    }
+  } else {
+    if(registration_method.find("NDT") == std::string::npos) {
+      std::cerr << "warning: unknown registration type(" << registration_method << ")" << std::endl;
+      std::cerr << "       : use NDT" << std::endl;
+    }
+
+    double ndt_resolution = pnh.param<double>("reg_resolution", 0.5);
+    if(registration_method.find("OMP") == std::string::npos) {    // **
+      std::cout << "registration: NDT " << ndt_resolution << std::endl;
+      pcl::NormalDistributionsTransform<PointT, PointT>::Ptr ndt(new pcl::NormalDistributionsTransform<PointT, PointT>());
+      ndt->setTransformationEpsilon(pnh.param<double>("reg_transformation_epsilon", 0.01));
+      ndt->setMaximumIterations(pnh.param<int>("reg_maximum_iterations", 64));
+      ndt->setResolution(ndt_resolution);
+      return ndt;
+    } else {
+      int num_threads = pnh.param<int>("reg_num_threads", 0);
+      std::string nn_search_method = pnh.param<std::string>("reg_nn_search_method", "DIRECT7");
+      std::cout << "registration: NDT_OMP " << nn_search_method << " " << ndt_resolution << " (" << num_threads << " threads)" << std::endl;
+      pclomp::NormalDistributionsTransform<PointT, PointT>::Ptr ndt(new pclomp::NormalDistributionsTransform<PointT, PointT>());
+      if(num_threads > 0) {
+        ndt->setNumThreads(num_threads);
+      }
+      ndt->setTransformationEpsilon(pnh.param<double>("reg_transformation_epsilon", 0.01));
+      ndt->setMaximumIterations(pnh.param<int>("reg_maximum_iterations", 64));
+      ndt->setResolution(ndt_resolution);
+      if(nn_search_method == "KDTREE") {
+        ndt->setNeighborhoodSearchMethod(pclomp::KDTREE);
+      } else if(nn_search_method == "DIRECT1") {
+        ndt->setNeighborhoodSearchMethod(pclomp::DIRECT1);
+      } else {
+        ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7);
+      }
+      return ndt;
+    }
+  }
+
+  return nullptr;
+}
+
+}  // namespace hdl_graph_slam

src/laser_odom copy.cpp

@@ -0,0 +1,323 @@
+#include "laser_odom.hpp"
+#include <memory>
+#include <iostream>
+
+#include <ros/ros.h>
+#include <ros/time.h>
+#include <ros/duration.h>
+#include <pcl_ros/point_cloud.h>
+#include <tf_conversions/tf_eigen.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+
+#include <std_msgs/Time.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/filters/approximate_voxel_grid.h>
+
+#include <hdl_graph_slam/ros_utils.hpp>
+#include <hdl_graph_slam/registrations.hpp>
+#include <hdl_graph_slam/ScanMatchingStatus.h>
+
+#include <laser_geometry/laser_geometry.h>
+
+#include <new>
+
+
+
+  laser_geometry::LaserProjection projector;
+  sensor_msgs::PointCloud2 cloud2;             // 点云数据类型转换的中间变量
+  pcl::PointCloud<PointT>::Ptr cloud;          // 点云数据
+  pcl::PointCloud<PointT>::ConstPtr keyframe;  // 关键帧
+  ros::Time prev_time;
+  Eigen::Matrix4f prev_trans;     // 地图起点到当前   帧  的放射变换
+  Eigen::Matrix4f keyframe_pose;  // 地图起点到当前 关键帧 的仿射变换
+  ros::Time keyframe_stamp;       // 关键帧的时间戳
+  pcl::Registration<PointT, PointT>::Ptr registration;
+  pcl::Filter<PointT>::Ptr downsample_filter;
+
+  double keyframe_delta_trans = 0.25;
+  double keyframe_delta_angle = 0.15;
+  double keyframe_delta_time = 1.0;
+  std::string odom_frame_id = "odom";
+  tf::TransformBroadcaster keyframe_broadcaster;
+  tf::TransformBroadcaster odom_broadcaster;
+
+  // ros::NodeHandle nh;
+
+
+#undef new
+int main(int argc, char* argv[]) {
+  // 执行 ros 节点初始化
+  ros::init(argc, argv, "laser_odom");
+  // 创建 ros 节点句柄
+  
+  Laser_Odom laser_odom;
+
+  // odom_pub = nh.advertise<nav_msgs::Odometry>("/laser_odom", 32);  // 发布机器人的里程计信息
+  ros::spin();
+
+  return 0;
+}
+
+Laser_Odom::Laser_Odom(){
+  cloud.reset(new pcl::PointCloud<PointT>());
+}
+
+Laser_Odom::~Laser_Odom(){
+
+}
+
+void Laser_Odom::init(){
+  ros::Subscriber sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 10, cloud_callback);
+
+
+}
+
+
+
+
+// ros::Publisher odom_pub;    // 里程计发布
+ros::Publisher status_pub;  // 匹配后的点云发布?
+
+//
+//
+//
+//
+//
+//
+//
+//
+//
+//
+//
+//
+//
+//
+//
+//
+//
+//
+//
+//
+//
+//
+//
+
+/**
+ * @brief publish scan matching status
+ */
+// void publish_scan_matching_status(const ros::Time& stamp, const std::string& frame_id, pcl::PointCloud<pcl::PointXYZI>::ConstPtr aligned, const std::string& msf_source, const Eigen::Isometry3f& msf_delta) {
+//   // 发布扫描的状态，包括匹配是否收敛、匹配误差、内点比例、相对位置等信息
+//   if(!status_pub.getNumSubscribers()) {
+//     return;
+//   }
+
+//   ScanMatchingStatus status;
+//   status.header.frame_id = frame_id;
+//   status.header.stamp = stamp;
+//   status.has_converged = registration->hasConverged();
+//   status.matching_error = registration->getFitnessScore();
+
+//   const double max_correspondence_dist = 0.5;
+
+//   int num_inliers = 0;
+//   std::vector<int> k_indices;
+//   std::vector<float> k_sq_dists;
+//   for(int i = 0; i < aligned->size(); i++) {
+//     const auto& pt = aligned->at(i);
+//     registration->getSearchMethodTarget()->nearestKSearch(pt, 1, k_indices, k_sq_dists);
+//     if(k_sq_dists[0] < max_correspondence_dist * max_correspondence_dist) {
+//       num_inliers++;
+//     }
+//   }
+//   status.inlier_fraction = static_cast<float>(num_inliers) / aligned->size();
+
+//   status.relative_pose = isometry2pose(Eigen::Isometry3f(registration->getFinalTransformation()).cast<double>());
+
+//   if(!msf_source.empty()) {
+//     status.prediction_labels.resize(1);
+//     status.prediction_labels[0].data = msf_source;
+
+//     status.prediction_errors.resize(1);
+//     Eigen::Isometry3f error = Eigen::Isometry3f(registration->getFinalTransformation()).inverse() * msf_delta;
+//     status.prediction_errors[0] = isometry2pose(error.cast<double>());
+//   }
+
+//   status_pub.publish(status);
+// }
+
+/**
+ * @brief convert Eigen::Matrix to geometry_msgs::TransformStamped
+ * @param stamp            timestamp
+ * @param pose             Eigen::Matrix to be converted
+ * @param frame_id         tf frame_id
+ * @param child_frame_id   tf child frame_id
+ * @return converted TransformStamped
+ */
+static geometry_msgs::TransformStamped matrix2transform(const ros::Time& stamp, const Eigen::Matrix4f& pose, const std::string& frame_id, const std::string& child_frame_id) {
+  Eigen::Quaternionf quat(pose.block<3, 3>(0, 0));
+  quat.normalize();
+  geometry_msgs::Quaternion odom_quat;
+  odom_quat.w = quat.w();
+  odom_quat.x = quat.x();
+  odom_quat.y = quat.y();
+  odom_quat.z = quat.z();
+
+  geometry_msgs::TransformStamped odom_trans;
+  odom_trans.header.stamp = stamp;
+  odom_trans.header.frame_id = frame_id;
+  odom_trans.child_frame_id = child_frame_id;
+
+  odom_trans.transform.translation.x = pose(0, 3);
+  odom_trans.transform.translation.y = pose(1, 3);
+  odom_trans.transform.translation.z = pose(2, 3);
+  odom_trans.transform.rotation = odom_quat;
+
+  return odom_trans;
+}
+
+/**
+ * @brief publish odometry
+ * @param stamp  timestamp
+ * @param pose   odometry pose to be published
+ */
+void publish_odometry(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose) {  // 发布里程计数据
+  // publish transform stamped for IMU integration
+  geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, odom_frame_id, base_frame_id);
+  // trans_pub.publish(odom_trans);
+
+  // broadcast the transform over tf
+  odom_broadcaster.sendTransform(odom_trans);
+
+  // publish the transform
+  nav_msgs::Odometry odom;
+  odom.header.stamp = stamp;
+  odom.header.frame_id = odom_frame_id;
+
+  odom.pose.pose.position.x = pose(0, 3);
+  odom.pose.pose.position.y = pose(1, 3);
+  odom.pose.pose.position.z = pose(2, 3);
+  odom.pose.pose.orientation = odom_trans.transform.rotation;
+
+  odom.child_frame_id = base_frame_id;
+  odom.twist.twist.linear.x = 0.0;
+  odom.twist.twist.linear.y = 0.0;
+  odom.twist.twist.angular.z = 0.0;
+
+  odom_pub.publish(odom);
+}
+
+/**
+ * @brief downsample a point cloud
+ * @param cloud  input cloud
+ * @return downsampled point cloud
+ */
+pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud) {  // 对点云数据进行向下采样，减少点的数量以提高处理速度
+  if(!downsample_filter) {
+    return cloud;
+  }
+
+  pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());  // 创建一个新的点云对象，用来存储下采样后的点云数据
+  downsample_filter->setInputCloud(cloud);
+  downsample_filter->filter(*filtered);
+
+  return filtered;
+}
+
+/**
+ * @brief estimate the relative pose between an input cloud and a keyframe cloud
+ * @param stamp  the timestamp of the input cloud
+ * @param cloud  the input cloud
+ * @return the relative pose between the input cloud and the keyframe cloud
+ */
+Eigen::Matrix4f matching(const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud) {  // 执行扫描匹配算法，估计输入点云和关键帧之间的相对位置
+
+  if(!keyframe) {                            // 判断 keyframe 是否为空指针，是的话说明还没有初始化关键真
+    prev_time = ros::Time();                 //
+    prev_trans.setIdentity();                // 设置为单位矩阵，表示：与上一次的位资没有发生变化
+    keyframe_pose.setIdentity();             // 关键帧的初始位资
+    keyframe_stamp = stamp;                  // 关键帧的时间戳
+    keyframe = downsample(cloud);            // 对点云数据进行下采样，减少点的数量，提高处理效率和精度
+    registration->setInputTarget(keyframe);  // 将 keyframe 设置成关键帧
+    return Eigen::Matrix4f::Identity();      // 没有之前的位资信息，假设与上次位资没有发生变化，返回单位矩阵
+
+    // registration 对象负责计算输入点云（cloud）与已有关键帧（keyframe）之间的相对变换。这种变换估算允许系统理解传感器（如激光雷达）在两次扫描之间的移动
+  }
+
+  auto filtered = downsample(cloud);       // 下采样
+  registration->setInputSource(filtered);  // 把点云数据给到 registration
+
+  std::string msf_source;                                       // 记录初始位资估计的来源（imu 或者 odometry)
+  Eigen::Isometry3f msf_delta = Eigen::Isometry3f::Identity();  // 三维仿射变换的单位矩阵，用于存储上一帧到当前帧的位资变化，3表示三维
+  // 缩放变换和旋转变换称为线性变换（linear transform）   线性变换和平移变换统称为仿射变换（affine transfrom）
+
+  pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());  // 创建一个新的点云对象aligned，用于存储对齐后的点云数据
+  registration->align(*aligned, prev_trans * msf_delta.matrix());       // 用 registration 来对齐点云, 如果没有imu或者odom信息, msf_delta 是单位矩阵, 即将观测到的点云数据对齐到关键帧?
+
+  // publish_scan_matching_status(stamp, cloud->header.frame_id, aligned, msf_source, msf_delta);  // 发布扫描匹配的状态信息，包括时间戳、坐标帧ID、对齐后的点云、位姿来源和位姿更新
+
+  if(!registration->hasConverged()) {  // 检查扫描匹配是否收敛(是否匹配成功?)。如果没有收敛，输出信息并返回上一次的位姿
+    ROS_ERROR("scan matching has not converged!!");
+    return keyframe_pose * prev_trans;
+  }
+
+  Eigen::Matrix4f trans = registration->getFinalTransformation();  // 获得当前点云和上一帧点云 关键帧 的仿射变换
+  Eigen::Matrix4f odom = keyframe_pose * trans;                    // 算出来 odom
+
+  prev_time = stamp;   // 当前帧的时间戳
+  prev_trans = trans;  // 当前帧的仿射变换
+
+  auto keyframe_trans = matrix2transform(stamp, keyframe_pose, odom_frame_id, "keyframe");  // 将变换矩阵转换为tf::Transform对象，用于发布关键帧的变换
+  keyframe_broadcaster.sendTransform(keyframe_trans);                                       // 发布关键帧的变换(这个是发送到哪里?  )
+
+  double delta_trans = trans.block<3, 1>(0, 3).norm();                                                                // 计算 当前帧 与 关键帧 变换的 平移距离
+  double delta_angle = std::acos(Eigen::Quaternionf(trans.block<3, 3>(0, 0)).w());                                    // 计算 当前帧 与 关键帧 变换的 旋转角度
+  double delta_time = (stamp - keyframe_stamp).toSec();                                                               // 计算 当前帧 与 关键帧 变换的 时间差
+  if(delta_trans > keyframe_delta_trans || delta_angle > keyframe_delta_angle || delta_time > keyframe_delta_time) {  // 如果有一个超过阈值,更新关键帧
+    keyframe = filtered;
+    registration->setInputTarget(keyframe);
+
+    keyframe_pose = odom;
+    keyframe_stamp = stamp;
+    prev_time = stamp;
+    prev_trans.setIdentity();
+  }
+  // 
+
+  return odom;  // 返回里程计
+}
+
+void cloud_callback(const sensor_msgs::LaserScan::ConstPtr& laser_scan_msg) {
+  if(!ros::ok()) {
+    return;
+  }
+  // 消息类型转换
+  projector.projectLaser(*laser_scan_msg, cloud2);
+  pcl::fromROSMsg(cloud2, *cloud);  // 这里发布出去验证一下
+
+  // 匹配
+  Eigen::Matrix4f pose = matching(laser_scan_msg->header.stamp, cloud);                   // 点云匹配函数，返回
+  publish_odometry(laser_scan_msg->header.stamp, laser_scan_msg->header.frame_id, pose);  // 发布激光里程计数据
+}
+
+int main(int argc, char* argv[]) {
+  // 执行 ros 节点初始化
+  ros::init(argc, argv, "laser_odom");
+  // 创建 ros 节点句柄
+  // ros::NodeHandle nh;
+
+  ros::Subscriber sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 10, cloud_callback);
+  // odom_pub = nh.advertise<nav_msgs::Odometry>("/laser_odom", 32);  // 发布机器人的里程计信息
+  ros::spin();
+
+  return 0;
+}

src/laser_odom.cpp

@@ -0,0 +1,273 @@
+#include "laser_odom.hpp"
+#include "hdl_graph_slam/registrations.hpp"
+
+#include <memory>
+#include <iostream>
+
+#include <ros/ros.h>
+#include <ros/time.h>
+#include <ros/duration.h>
+#include <pcl_ros/point_cloud.h>
+#include <tf_conversions/tf_eigen.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+
+#include <std_msgs/Time.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/filters/approximate_voxel_grid.h>
+
+#include <hdl_graph_slam/ros_utils.hpp>
+#include <hdl_graph_slam/registrations.hpp>
+#include <hdl_graph_slam/ScanMatchingStatus.h>
+
+#include <laser_geometry/laser_geometry.h>
+
+
+int main(int argc, char* argv[]) {
+  // 执行 ros 节点初始化
+  ros::init(argc, argv, "laser_odom");
+  // 创建 ros 节点句柄
+
+  Laser_Odom laser_odom;
+  laser_odom.init();
+  // odom_pub = nh.advertise<nav_msgs::Odometry>("/laser_odom", 32);  // 发布机器人的里程计信息
+
+  return 0;
+}
+
+Laser_Odom::Laser_Odom() {
+  laser_odom = this;
+  cloud.reset(new pcl::PointCloud<PointT>());
+  registration.reset(new fast_gicp::FastGICP<PointT, PointT>());
+}
+
+Laser_Odom::~Laser_Odom() {}
+
+void Laser_Odom::init() {
+  auto voxelgrid = new pcl::VoxelGrid<PointT>();
+  voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
+  downsample_filter.reset(voxelgrid);
+  
+  registration->setNumThreads(0);   // 把 registration = hdl_graph_slam::select_registration_method(nh);  核心部分弄出来
+  registration->setTransformationEpsilon(0.1);
+  registration->setMaximumIterations(64);
+  registration->setMaxCorrespondenceDistance(2.0);
+  registration->setCorrespondenceRandomness(20);
+
+  odom_pub = nh.advertise<nav_msgs::Odometry>("/laser_odom", 32);  // 发布机器人的里程计信息
+  // keyframe_pub = nh.advertise<pcl::PointCloud<PointT>>("/keyframe", 32);
+  // read_until_pub = nh.advertise<std_msgs::Header>("/keyframe", 32);
+  aligned_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/aligned_points", 32);                      //
+  map_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/map_points", 1, true);
+
+  ros::Subscriber sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 10, Laser_Odom::cloud_callback);
+  ros::spin();
+}
+
+Laser_Odom* Laser_Odom::laser_odom = nullptr;  // 在 .cpp 文件中定义静态成员变量
+void Laser_Odom::cloud_callback(const sensor_msgs::LaserScan::ConstPtr& laser_scan_msg) {
+  if(!ros::ok()) {
+    return;
+  }
+  // 消息类型转换
+  laser_odom->projector.projectLaser(*laser_scan_msg, laser_odom->cloud2);
+  pcl::fromROSMsg(laser_odom->cloud2, *laser_odom->cloud);
+  
+  // 点云匹配
+  Eigen::Matrix4f pose = laser_odom->matching(laser_scan_msg->header.stamp, laser_odom->cloud);  // 点云匹配函数，返回
+
+  // 发布激光里程计数据
+  laser_odom->publish_odometry(laser_scan_msg->header.stamp, laser_scan_msg->header.frame_id, pose);  
+
+  // 发布对齐后的点晕
+  pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
+  pcl::transformPointCloud(*laser_odom->cloud, *aligned, laser_odom-> odom);
+  aligned->header.frame_id = "odom";
+  laser_odom->aligned_points_pub.publish(*aligned);
+
+  // 发布点云地图
+}
+
+/**
+ * @brief estimate the relative pose between an input cloud and a keyframe cloud
+ * @param stamp  the timestamp of the input cloud
+ * @param cloud  the input cloud
+ * @return the relative pose between the input cloud and the keyframe cloud
+ */
+Eigen::Matrix4f Laser_Odom::matching(const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud) {  // 执行扫描匹配算法，估计输入点云和关键帧之间的相对位置
+
+  if(!keyframe) {                            // 判断 keyframe 是否为空指针，是的话说明还没有初始化关键真
+    prev_time = ros::Time();                 //
+    prev_trans.setIdentity();                // 设置为单位矩阵，表示：与上一次的位资没有发生变化
+    keyframe_pose.setIdentity();             // 关键帧的初始位资
+    keyframe_stamp = stamp;                  // 关键帧的时间戳
+    keyframe = downsample(cloud);            // 对点云数据进行下采样，减少点的数量，提高处理效率和精度
+    registration->setInputTarget(keyframe);  // 将 keyframe 设置成关键帧  
+    return Eigen::Matrix4f::Identity();      // 没有之前的位资信息，假设与上次位资没有发生变化，返回单位矩阵
+
+    // registration 对象负责计算输入点云（cloud）与已有关键帧（keyframe）之间的相对变换。这种变换估算允许系统理解传感器（如激光雷达）在两次扫描之间的移动
+  }
+  auto filtered = downsample(cloud);       // 下采样
+  registration->setInputSource(filtered);  // 把点云数据给到 registration
+
+  std::string msf_source;                                       // 记录初始位资估计的来源（imu 或者 odometry)
+  Eigen::Isometry3f msf_delta = Eigen::Isometry3f::Identity();  // 三维仿射变换的单位矩阵，用于存储上一帧到当前帧的位资变化，3表示三维
+  // 缩放变换和旋转变换称为线性变换（linear transform）   线性变换和平移变换统称为仿射变换（affine transfrom）
+
+  pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());  // 创建一个新的点云对象aligned，用于存储对齐后的点云数据
+  registration->align(*aligned, prev_trans * msf_delta.matrix());       // 用 registration 来对齐点云, 如果没有imu或者odom信息, msf_delta 是单位矩阵, 即将观测到的点云数据对齐到关键帧?
+
+  // publish_scan_matching_status(stamp, cloud->header.frame_id, aligned, msf_source, msf_delta);  // 发布扫描匹配的状态信息，包括时间戳、坐标帧ID、对齐后的点云、位姿来源和位姿更新
+
+  if(!registration->hasConverged()) {  // 检查扫描匹配是否收敛(是否匹配成功?)。如果没有收敛，输出信息并返回上一次的位姿
+    ROS_ERROR("scan matching has not converged!!");
+    return keyframe_pose * prev_trans;
+  }
+
+  Eigen::Matrix4f trans = registration->getFinalTransformation();  // 获得当前点云和上一帧点云 关键帧 的仿射变换
+  odom = keyframe_pose * trans;                    // 算出来 odom
+
+  prev_time = stamp;   // 当前帧的时间戳
+  prev_trans = trans;  // 当前帧的仿射变换
+
+  auto keyframe_trans = matrix2transform(stamp, keyframe_pose, "odom", "keyframe");  // 将变换矩阵转换为tf::Transform对象，用于发布关键帧的变换
+  keyframe_broadcaster.sendTransform(keyframe_trans);                                       // 发布关键帧的变换(这个是发送到哪里?  )
+
+
+  double delta_trans = trans.block<3, 1>(0, 3).norm();                                                                // 计算 当前帧 与 关键帧 变换的 平移距离
+  double delta_angle = std::acos(Eigen::Quaternionf(trans.block<3, 3>(0, 0)).w());                                    // 计算 当前帧 与 关键帧 变换的 旋转角度
+  double delta_time = (stamp - keyframe_stamp).toSec();                                                               // 计算 当前帧 与 关键帧 变换的 时间差
+  if(delta_trans > keyframe_delta_trans || delta_angle > keyframe_delta_angle || delta_time > keyframe_delta_time) {  // 如果有一个超过阈值,更新关键帧
+    keyframe = filtered;
+    registration->setInputTarget(keyframe);
+
+    keyframe_pose = odom;
+    keyframe_stamp = stamp;
+    prev_time = stamp;
+    prev_trans.setIdentity();
+  }
+  //
+  // Eigen::Matrix4f odom;
+  std::cout << "The matrix odom is: \n" << odom << std::endl;
+  return odom;  // 返回里程计
+}
+
+/**
+ * @brief downsample a point cloud
+ * @param cloud  input cloud
+ * @return downsampled point cloud
+ */
+pcl::PointCloud<PointT>::ConstPtr Laser_Odom::downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud) {  // 对点云数据进行向下采样，减少点的数量以提高处理速度
+  if(!downsample_filter) {
+    return cloud;
+  }
+  pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());  // 创建一个新的点云对象，用来存储下采样后的点云数据
+  downsample_filter->setInputCloud(cloud);
+  downsample_filter->filter(*filtered);
+  return filtered;
+}
+
+/**
+ * @brief convert Eigen::Matrix to geometry_msgs::TransformStamped
+ * @param stamp            timestamp
+ * @param pose             Eigen::Matrix to be converted
+ * @param frame_id         tf frame_id
+ * @param child_frame_id   tf child frame_id
+ * @return converted TransformStamped
+ */
+geometry_msgs::TransformStamped Laser_Odom::matrix2transform(const ros::Time& stamp, const Eigen::Matrix4f& pose, const std::string& frame_id, const std::string& child_frame_id) {
+  Eigen::Quaternionf quat(pose.block<3, 3>(0, 0));
+  quat.normalize();
+  geometry_msgs::Quaternion odom_quat;
+  odom_quat.w = quat.w();
+  odom_quat.x = quat.x();
+  odom_quat.y = quat.y();
+  odom_quat.z = quat.z();
+
+  geometry_msgs::TransformStamped odom_trans;
+  odom_trans.header.stamp = stamp;
+  odom_trans.header.frame_id = frame_id;
+  odom_trans.child_frame_id = child_frame_id;
+
+  odom_trans.transform.translation.x = pose(0, 3);
+  odom_trans.transform.translation.y = pose(1, 3);
+  odom_trans.transform.translation.z = pose(2, 3);
+  odom_trans.transform.rotation = odom_quat;
+
+  return odom_trans;
+}
+
+
+  /**
+   * @brief publish odometry
+   * @param stamp  timestamp
+   * @param pose   odometry pose to be published
+   */
+  void Laser_Odom::publish_odometry(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose) {  // 发布里程计数据
+    // publish transform stamped for IMU integration
+    // geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, odom_frame_id, base_frame_id);
+    geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, "odom", base_frame_id);
+    // trans_pub.publish(odom_trans);
+
+    // // broadcast the transform over tf
+    // odom_broadcaster.sendTransform(odom_trans);
+
+    // publish the transform
+    nav_msgs::Odometry odom;
+    odom.header.stamp = stamp;
+    odom.header.frame_id = "odom";
+
+    odom.pose.pose.position.x = pose(0, 3);
+    odom.pose.pose.position.y = pose(1, 3);
+    odom.pose.pose.position.z = pose(2, 3);
+    odom.pose.pose.orientation = odom_trans.transform.rotation;
+
+    odom.child_frame_id = base_frame_id;
+    odom.twist.twist.linear.x = 0.0;
+    odom.twist.twist.linear.y = 0.0;
+    odom.twist.twist.angular.z = 0.0;
+
+    odom_pub.publish(odom);
+  }
+
+
+  // /**
+  //  * @brief publish odometry
+  //  * @param stamp  timestamp
+  //  * @param pose   odometry pose to be published
+  //  */
+  // void Laser_Odom::publish_keyfrrame(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose) {  // 发布里程计数据
+  //   // publish transform stamped for IMU integration
+  //   // geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, odom_frame_id, base_frame_id);
+  //   geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, "odom", base_frame_id);
+  //   // trans_pub.publish(odom_trans);
+
+  //   // // broadcast the transform over tf
+  //   // odom_broadcaster.sendTransform(odom_trans);
+
+  //   // publish the transform
+  //   nav_msgs::Odometry odom;
+  //   odom.header.stamp = stamp;
+  //   odom.header.frame_id = "odom";
+
+  //   odom.pose.pose.position.x = pose(0, 3);
+  //   odom.pose.pose.position.y = pose(1, 3);
+  //   odom.pose.pose.position.z = pose(2, 3);
+  //   odom.pose.pose.orientation = odom_trans.transform.rotation;
+
+  //   odom.child_frame_id = base_frame_id;
+  //   odom.twist.twist.linear.x = 0.0;
+  //   odom.twist.twist.linear.y = 0.0;
+  //   odom.twist.twist.angular.z = 0.0;
+
+  //   odom_pub.publish(odom);
+  // }

srv/DumpGraph.srv

@@ -0,0 +1,4 @@
+
+string destination
+---
+bool success

srv/LoadGraph.srv

@@ -0,0 +1,4 @@
+
+string path
+---
+bool success

srv/SaveMap.srv

@@ -0,0 +1,5 @@
+bool utm
+float32 resolution
+string destination
+---
+bool success