diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..0238ce8
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,3 @@
+.vscode/*
+imgui.ini
+rviz/*
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..a4b66c0
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,166 @@
+# SPDX-License-Identifier: BSD-2-Clause
+cmake_minimum_required(VERSION 2.8.3)
+project(hdl_graph_slam)
+
+# Can we use C++17 in indigo?
+add_definitions(-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2)
+set(CMAKE_CXX_FLAGS "-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
+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
+ roscpp
+ rospy
+ pcl_ros
+ geodesy
+ nmea_msgs
+ sensor_msgs
+ geometry_msgs
+ message_generation
+ interactive_markers
+ ndt_omp
+ fast_gicp
+)
+catkin_python_setup()
+
+find_package(PCL REQUIRED)
+include_directories(${PCL_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS} ${OpenCV_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+find_package(OpenCV 3 REQUIRED)
+
+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
+ DumpGraph.srv
+)
+
+generate_messages(DEPENDENCIES std_msgs geometry_msgs)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES hdl_graph_slam_nodelet
+# CATKIN_DEPENDS pcl_ros roscpp sensor_msgs
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+include_directories(include)
+include_directories(
+ ${PCL_INCLUDE_DIRS}
+ ${catkin_INCLUDE_DIRS}
+ ${OpenCV_INCLUDE_DIRS}
+)
+
+# nodelets
+add_library(prefiltering_nodelet apps/prefiltering_nodelet.cpp)
+target_link_libraries(prefiltering_nodelet
+ ${catkin_LIBRARIES}
+ ${PCL_LIBRARIES}
+ ${OpenCV_LIBRARIES}
+)
+
+
+add_library(floor_detection_nodelet apps/floor_detection_nodelet.cpp)
+target_link_libraries(floor_detection_nodelet
+ ${catkin_LIBRARIES}
+ ${PCL_LIBRARIES}
+ ${OpenCV_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
+ src/hdl_graph_slam/imageProjection.cpp
+ src/hdl_graph_slam/featureAssociation.cpp
+)
+target_link_libraries(scan_matching_odometry_nodelet
+ ${catkin_LIBRARIES}
+ ${PCL_LIBRARIES}
+ ${OpenCV_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}
+)
diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000..612dcee
--- /dev/null
+++ b/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.
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..386d3f8
--- /dev/null
+++ b/README.md
@@ -0,0 +1,53 @@
+#FD-SLAM
+This is an open source ROS package for real-time 6DOF SLAM using a 3D LIDAR.
+
+It is based on hdl_graph_slam and the steps to run our system are same with hdl-graph-slam.
+
+##The names and functions do not change compared with hdl-graph-slam.
+
+We use a novel feature-based Lidar odometry for fast scan-matching, and use a modified GICP for keyframe matching. The backend in hdl-graph-slam is reused. We have tested this package with Velodyne (HDL32e, HDL64,VLP16) and Ouster64 sensors in indoor and outdoor environments. The corresponding configure launch files are provided.
+
+##key modifications
+src/hdl_graph_slam/imageProjection.cpp and src/hdl_graph_slam/featureAssociation.cpp are added for fast scan matching
+
+As for the fast-gicp, key modifications are taken place in fast_gicp_impl.cpp----calculate_covariances()
+
+
+## Nodelets
+***FD-slam*** consists of four nodelets.
+
+- *prefiltering_nodelet*
+- *scan_matching_odometry_nodelet*
+- *floor_detection_nodelet*
+- *hdl_graph_slam_nodelet*
+
+The input point cloud is first downsampled by *prefiltering_nodelet*, and then passed to the next nodelets. While *scan_matching_odometry_nodelet* estimates the sensor pose by iteratively applying a scan matching between consecutive frames (i.e., odometry estimation), *floor_detection_nodelet* detects floor planes by RANSAC. The estimated odometry and the detected floor planes are sent to *hdl_graph_slam*. To compensate the accumulated error of the scan matching, it performs loop detection and optimizes a pose graph which takes various constraints into account.
+
+
+
+## Parameters
+All the configurable parameters are listed in *launch/****.launch* as ros params.
+
+## Services
+- */hdl_graph_slam/dump* (hdl_graph_slam/DumpGraph)
+ - save all the internal data (point clouds, floor coeffs, odoms, and pose graph) to a directory.
+- */hdl_graph_slam/save_map* (hdl_graph_slam/SaveMap)
+ - save the generated map as a PCD file.
+
+## Requirements
+***hdl_graph_slam*** requires the following libraries:
+
+- OpenMP
+- PCL
+- g2o
+- suitesparse
+
+The following ROS packages are required:
+
+- geodesy
+- nmea_msgs
+- pcl_ros
+- [ndt_omp](https://github.com/koide3/ndt_omp)
+- [fast_gicp](https://github.com/SMRT-AIST/fast_gicp)
+
+
diff --git a/README_used in hdl_graph_slam.md b/README_used in hdl_graph_slam.md
new file mode 100644
index 0000000..0c33b95
--- /dev/null
+++ b/README_used in hdl_graph_slam.md
@@ -0,0 +1,225 @@
+# hdl_graph_slam
+***hdl_graph_slam*** is an open source ROS package for real-time 6DOF SLAM using a 3D LIDAR. It is based on 3D Graph SLAM with NDT scan matching-based odometry estimation and loop detection. It also supports several graph constraints, such as GPS, IMU acceleration (gravity vector), IMU orientation (magnetic sensor), and floor plane (detected in a point cloud). We have tested this package with Velodyne (HDL32e, VLP16) and RoboSense (16 channels) sensors in indoor and outdoor environments.
+
+
+
+[video](https://drive.google.com/open?id=0B9f5zFkpn4soSG96Tkt4SFFTbms)
+
+[](https://github.com/koide3/hdl_graph_slam/actions/workflows/build.yml) on melodic & noetic
+
+## Nodelets
+***hdl_graph_slam*** consists of four nodelets.
+
+- *prefiltering_nodelet*
+- *scan_matching_odometry_nodelet*
+- *floor_detection_nodelet*
+- *hdl_graph_slam_nodelet*
+
+The input point cloud is first downsampled by *prefiltering_nodelet*, and then passed to the next nodelets. While *scan_matching_odometry_nodelet* estimates the sensor pose by iteratively applying a scan matching between consecutive frames (i.e., odometry estimation), *floor_detection_nodelet* detects floor planes by RANSAC. The estimated odometry and the detected floor planes are sent to *hdl_graph_slam*. To compensate the accumulated error of the scan matching, it performs loop detection and optimizes a pose graph which takes various constraints into account.
+
+
+
+## Constraints (Edges)
+
+You can enable/disable each constraint by changing params in the launch file, and you can also change the weight (\*_stddev) and the robust kernel (\*_robust_kernel) of each constraint.
+
+- ***Odometry***
+
+- ***Loop closure***
+
+- ***GPS***
+ - */gps/geopoint* (geographic_msgs/GeoPointStamped)
+ - */gps/navsat* (sensor_msgs/NavSatFix)
+ - */gpsimu_driver/nmea_sentence* (nmea_msgs/Sentence)
+
+hdl_graph_slam supports several GPS message types. All the supported types contain (latitude, longitude, and altitude). hdl_graph_slam converts them into [the UTM coordinate](http://wiki.ros.org/geodesy), and adds them into the graph as 3D position constraints. If altitude is set to NaN, the GPS data is treated as a 2D constrait. GeoPoint is the most basic one, which consists of only (lat, lon, alt). Although NavSatFix provides many information, we use only (lat, lon, alt) and ignore all other data. If you're using HDL32e, you can directly connect *hdl_graph_slam* with *velodyne_driver* via */gpsimu_driver/nmea_sentence*.
+
+- ***IMU acceleration (gravity vector)***
+ - */gpsimu_driver/imu_data* (sensor_msgs/Imu)
+
+This constraint rotates each pose node so that the acceleration vector associated with the node becomes vertical (as the gravity vector). This is useful to compensate for accumulated tilt rotation errors of the scan matching. Since we ignore acceleration by sensor motion, you should not give a big weight for this constraint.
+
+- ***IMU orientation (magnetic sensor)***
+ - */gpsimu_driver/imu_data* (sensor_msgs/Imu)
+
+ If your IMU has a reliable magnetic orientation sensor, you can add orientation data to the graph as 3D rotation constraints. Note that, magnetic orientation sensors can be affected by external magnetic disturbances. In such cases, this constraint should be disabled.
+
+- ***Floor plane***
+ - */floor_detection/floor_coeffs* (hdl_graph_slam/FloorCoeffs)
+
+This constraint optimizes the graph so that the floor planes (detected by RANSAC) of the pose nodes becomes the same. This is designed to compensate the accumulated rotation error of the scan matching in large flat indoor environments.
+
+
+## Parameters
+All the configurable parameters are listed in *launch/hdl_graph_slam.launch* as ros params.
+
+## Services
+- */hdl_graph_slam/dump* (hdl_graph_slam/DumpGraph)
+ - save all the internal data (point clouds, floor coeffs, odoms, and pose graph) to a directory.
+- */hdl_graph_slam/save_map* (hdl_graph_slam/SaveMap)
+ - save the generated map as a PCD file.
+
+## Requirements
+***hdl_graph_slam*** requires the following libraries:
+
+- OpenMP
+- PCL
+- g2o
+- suitesparse
+
+The following ROS packages are required:
+
+- geodesy
+- nmea_msgs
+- pcl_ros
+- [ndt_omp](https://github.com/koide3/ndt_omp)
+- [fast_gicp](https://github.com/SMRT-AIST/fast_gicp)
+
+```bash
+# for melodic
+sudo apt-get install ros-melodic-geodesy ros-melodic-pcl-ros ros-melodic-nmea-msgs ros-melodic-libg2o
+cd catkin_ws/src
+git clone https://github.com/koide3/ndt_omp.git -b melodic
+git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive
+git clone https://github.com/koide3/hdl_graph_slam
+
+cd .. && catkin_make -DCMAKE_BUILD_TYPE=Release
+
+# for noetic
+sudo apt-get install ros-noetic-geodesy ros-noetic-pcl-ros ros-noetic-nmea-msgs ros-noetic-libg2o
+
+cd catkin_ws/src
+git clone https://github.com/koide3/ndt_omp.git
+git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive
+git clone https://github.com/koide3/hdl_graph_slam
+
+cd .. && catkin_make -DCMAKE_BUILD_TYPE=Release
+```
+
+**[optional]** *bag_player.py* script requires ProgressBar2.
+```bash
+sudo pip install ProgressBar2
+```
+
+## Example1 (Indoor)
+
+Bag file (recorded in a small room):
+
+- [hdl_501.bag.tar.gz](http://www.aisl.cs.tut.ac.jp/databases/hdl_graph_slam/hdl_501.bag.tar.gz) (raw data, 344MB)
+- [hdl_501_filtered.bag.tar.gz](http://www.aisl.cs.tut.ac.jp/databases/hdl_graph_slam/hdl_501_filtered.bag.tar.gz) (downsampled data, 57MB, **Recommended!**)
+
+```bash
+rosparam set use_sim_time true
+roslaunch hdl_graph_slam hdl_graph_slam_501.launch
+```
+
+```bash
+roscd hdl_graph_slam/rviz
+rviz -d hdl_graph_slam.rviz
+```
+
+```bash
+rosbag play --clock hdl_501_filtered.bag
+```
+
+We also provide bag_player.py which automatically adjusts the playback speed and processes data as fast as possible.
+
+```bash
+rosrun hdl_graph_slam bag_player.py hdl_501_filtered.bag
+```
+
+You'll see a point cloud like:
+
+
+
+You can save the generated map by:
+```bash
+rosservice call /hdl_graph_slam/save_map "resolution: 0.05
+destination: '/full_path_directory/map.pcd'"
+```
+
+## Example2 (Outdoor)
+
+Bag file (recorded in an outdoor environment):
+- [hdl_400.bag.tar.gz](http://www.aisl.cs.tut.ac.jp/databases/hdl_graph_slam/hdl_400.bag.tar.gz) (raw data, about 900MB)
+
+```bash
+rosparam set use_sim_time true
+roslaunch hdl_graph_slam hdl_graph_slam_400.launch
+```
+
+```bash
+roscd hdl_graph_slam/rviz
+rviz -d hdl_graph_slam.rviz
+```
+
+```bash
+rosbag play --clock hdl_400.bag
+```
+
+
+
+## Example with GPS
+Ford Campus Vision and Lidar Data Set [\[URL\]](http://robots.engin.umich.edu/SoftwareData/Ford)
+
+The following script converts the Ford Lidar Dataset to a rosbag and plays it. In this example, ***hdl_graph_slam*** utilizes the GPS data to correct the pose graph.
+
+```bash
+cd IJRR-Dataset-2
+rosrun hdl_graph_slam ford2bag.py dataset-2.bag
+rosrun hdl_graph_slam bag_player.py dataset-2.bag
+```
+
+
+
+## Use hdl_graph_slam in your system
+
+1. Define the transformation between your sensors (LIDAR, IMU, GPS) and base_link of your system using static_transform_publisher (see line #11, hdl_graph_slam.launch). All the sensor data will be transformed into the common base_link frame, and then fed to the SLAM algorithm.
+
+2. Remap the point cloud topic of ***prefiltering_nodelet***. Like:
+
+```bash
+
+ ...
+```
+
+## Common Problems
+
+### Parameter tuning guide
+
+The mapping quality largely depends on the parameter setting. In particular, scan matching parameters have a big impact on the result. Tune the parameters accoding to the following instructions:
+
+- ***registration_method***
+ **[updated] In short, use FAST_GICP for most cases and FAST_VGICP or NDT_OMP if the processing speed matters** This parameter allows to change the registration method to be used for odometry estimation and loop detection. Note that GICP in PCL1.7 (ROS kinetic) or earlier has a bug in the initial guess handling. **If you are on ROS kinectic or earlier, do not use GICP**.
+
+- ***ndt_resolution***
+ This parameter decides the voxel size of NDT. Typically larger values are good for outdoor environements (0.5 - 2.0 [m] for indoor, 2.0 - 10.0 [m] for outdoor). If you chose NDT or NDT_OMP, tweak this parameter so you can obtain a good odometry estimation result.
+
+- ***other parameters***
+ All the configurable parameters are available in the launch file. Copy a template launch file (hdl_graph_slam_501.launch for indoor, hdl_graph_slam_400.launch for outdoor) and tweak parameters in the launch file to adapt it to your application.
+
+## License
+
+This package is released under the BSD-2-Clause License.
+
+
+Note that the cholmod solver in g2o is licensed under GPL. You may need to build g2o without cholmod dependency to avoid the GPL.
+
+## Related packages
+
+- [interactive_slam](https://github.com/koide3/interactive_slam)
+- [hdl_graph_slam](https://github.com/koide3/hdl_graph_slam)
+- [hdl_localization](https://github.com/koide3/hdl_localization)
+- [hdl_people_tracking](https://github.com/koide3/hdl_people_tracking)
+
+
+
+## Papers
+Kenji Koide, Jun Miura, and Emanuele Menegatti, A Portable 3D LIDAR-based System for Long-term and Wide-area People Behavior Measurement, Advanced Robotic Systems, 2019 [[link]](https://www.researchgate.net/publication/331283709_A_Portable_3D_LIDAR-based_System_for_Long-term_and_Wide-area_People_Behavior_Measurement).
+
+## Contact
+Kenji Koide, k.koide@aist.go.jp, https://staff.aist.go.jp/k.koide
+
+Active Intelligent Systems Laboratory, Toyohashi University of Technology, Japan [\[URL\]](http://www.aisl.cs.tut.ac.jp)
+Mobile Robotics Research Team, National Institute of Advanced Industrial Science and Technology (AIST), Japan [\[URL\]](https://unit.aist.go.jp/rirc/en/team/smart_mobility.html)
diff --git a/apps/floor_detection_nodelet.cpp b/apps/floor_detection_nodelet.cpp
new file mode 100644
index 0000000..ad714d9
--- /dev/null
+++ b/apps/floor_detection_nodelet.cpp
@@ -0,0 +1,269 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#include
+#include
+
+#include
+
+#include
+#include
+#include
+
+#include
+#include
+#include
+
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+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("/floor_detection/floor_coeffs", 32);
+
+ read_until_pub = nh.advertise("/floor_detection/read_until", 32);
+ floor_filtered_pub = nh.advertise("/floor_detection/floor_filtered_points", 32);
+ floor_points_pub = nh.advertise("/floor_detection/floor_points", 32);
+ }
+
+private:
+ /**
+ * @brief initialize parameters
+ */
+ void initialize_params() {
+ tilt_deg = private_nh.param("tilt_deg", 0.0); // approximate sensor tilt angle [deg]
+ sensor_height = private_nh.param("sensor_height", 2.0); // approximate sensor height [m]
+ height_clip_range = private_nh.param("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("floor_pts_thresh", 512); // minimum number of support points of RANSAC to accept a detected floor plane
+ floor_normal_thresh = private_nh.param("floor_normal_thresh", 10.0); // verticality check thresold for the detected floor plane [deg]
+ use_normal_filtering = private_nh.param("use_normal_filtering", true); // if true, points with "non-"vertical normals will be filtered before RANSAC
+ normal_filter_thresh = private_nh.param("normal_filter_thresh", 20.0); // "non-"verticality check threshold [deg]
+
+ points_topic = private_nh.param("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::Ptr cloud(new pcl::PointCloud());
+ pcl::fromROSMsg(*cloud_msg, *cloud);
+
+ if(cloud->empty()) {
+ return;
+ }
+
+ // floor detection
+ boost::optional 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 detect(const pcl::PointCloud::Ptr& cloud) const {
+ // 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::Ptr filtered(new pcl::PointCloud);
+ 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(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::Ptr model_p(new pcl::SampleConsensusModelPlane(filtered));
+ pcl::RandomSampleConsensus 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::Ptr inlier_cloud(new pcl::PointCloud);
+ pcl::ExtractIndices 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::Ptr plane_clip(const pcl::PointCloud::Ptr& src_cloud, const Eigen::Vector4f& plane, bool negative) const {
+ pcl::PlaneClipper3D clipper(plane);
+ pcl::PointIndices::Ptr indices(new pcl::PointIndices);
+
+ clipper.clipPointCloud3D(*src_cloud, indices->indices);
+
+ pcl::PointCloud::Ptr dst_cloud(new pcl::PointCloud);
+
+ pcl::ExtractIndices 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::Ptr normal_filtering(const pcl::PointCloud::Ptr& cloud) const {
+ pcl::NormalEstimation ne;
+ ne.setInputCloud(cloud);
+
+ pcl::search::KdTree::Ptr tree(new pcl::search::KdTree);
+ ne.setSearchMethod(tree);
+
+ pcl::PointCloud::Ptr normals(new pcl::PointCloud);
+ ne.setKSearch(10);
+ ne.setViewPoint(0.0f, 0.0f, sensor_height);
+ ne.compute(*normals);
+
+ pcl::PointCloud::Ptr filtered(new pcl::PointCloud);
+ 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)
diff --git a/apps/hdl_graph_slam_nodelet.cpp b/apps/hdl_graph_slam_nodelet.cpp
new file mode 100644
index 0000000..23c9af2
--- /dev/null
+++ b/apps/hdl_graph_slam_nodelet.cpp
@@ -0,0 +1,1056 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+
+#include
+#include
+
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+using namespace std;
+namespace hdl_graph_slam {
+
+class HdlGraphSlamNodelet : public nodelet::Nodelet {
+public:
+ typedef pcl::PointXYZI PointT;
+ typedef message_filters::sync_policies::ApproximateTime ApproxSyncPolicy;
+
+ HdlGraphSlamNodelet() {}
+ virtual ~HdlGraphSlamNodelet() {}
+
+ virtual void onInit() {
+ nh = getNodeHandle();
+ mt_nh = getMTNodeHandle();
+ private_nh = getPrivateNodeHandle();
+
+ // init parameters
+ map_frame_id = private_nh.param("map_frame_id", "map");
+ odom_frame_id = private_nh.param("odom_frame_id", "odom");
+ map_cloud_resolution = private_nh.param("map_cloud_resolution", 0.05);
+ trans_odom2map.setIdentity();
+
+ max_keyframes_per_update = private_nh.param("max_keyframes_per_update", 10);
+
+ //
+ anchor_node = nullptr;
+ anchor_edge = nullptr;
+ floor_plane_node = nullptr;
+ graph_slam.reset(new GraphSLAM(private_nh.param("g2o_solver_type", "lm_var")));
+ keyframe_updater.reset(new KeyframeUpdater(private_nh));
+ loop_detector.reset(new LoopDetector(private_nh));
+ map_cloud_generator.reset(new MapCloudGenerator());
+ inf_calclator.reset(new InformationMatrixCalculator(private_nh));
+ nmea_parser.reset(new NmeaSentenceParser());
+
+ gps_time_offset = private_nh.param("gps_time_offset", 0.0);
+ gps_edge_stddev_xy = private_nh.param("gps_edge_stddev_xy", 10000.0);
+ gps_edge_stddev_z = private_nh.param("gps_edge_stddev_z", 10.0);
+ floor_edge_stddev = private_nh.param("floor_edge_stddev", 10.0);
+ seq_ = private_nh.param("Sequence", "seq");
+ std::cout<<"seq: "<("use_prev_test_", false);
+
+ imu_time_offset = private_nh.param("imu_time_offset", 0.0);
+ enable_imu_orientation = private_nh.param("enable_imu_orientation", false);
+ enable_imu_acceleration = private_nh.param("enable_imu_acceleration", false);
+ imu_orientation_edge_stddev = private_nh.param("imu_orientation_edge_stddev", 0.1);
+ imu_acceleration_edge_stddev = private_nh.param("imu_acceleration_edge_stddev", 3.0);
+
+ points_topic = private_nh.param("points_topic", "/velodyne_points");
+
+ // subscribers
+ odom_sub.reset(new message_filters::Subscriber(mt_nh, "/odom", 256));
+ cloud_sub.reset(new message_filters::Subscriber(mt_nh, "/filtered_points", 32));
+ sync.reset(new message_filters::Synchronizer(ApproxSyncPolicy(32), *odom_sub, *cloud_sub));
+ sync->registerCallback(boost::bind(&HdlGraphSlamNodelet::cloud_callback, this, _1, _2));
+ imu_sub = nh.subscribe("/gpsimu_driver/imu_data", 1024, &HdlGraphSlamNodelet::imu_callback, this);
+ floor_sub = nh.subscribe("/floor_detection/floor_coeffs", 1024, &HdlGraphSlamNodelet::floor_coeffs_callback, this);
+
+ if(private_nh.param("enable_gps", true)) {
+ gps_sub = mt_nh.subscribe("/gps/geopoint", 1024, &HdlGraphSlamNodelet::gps_callback, this);
+ nmea_sub = mt_nh.subscribe("/gpsimu_driver/nmea_sentence", 1024, &HdlGraphSlamNodelet::nmea_callback, this);
+ navsat_sub = mt_nh.subscribe("/gps/navsat", 1024, &HdlGraphSlamNodelet::navsat_callback, this);
+ }
+
+ // publishers
+ markers_pub = mt_nh.advertise("/hdl_graph_slam/markers", 16);
+ odom2map_pub = mt_nh.advertise("/hdl_graph_slam/odom2pub", 16);
+ map_points_pub = mt_nh.advertise("/hdl_graph_slam/map_points", 1, true);
+ read_until_pub = mt_nh.advertise("/hdl_graph_slam/read_until", 32);
+
+ dump_service_server = mt_nh.advertiseService("/hdl_graph_slam/dump", &HdlGraphSlamNodelet::dump_service, this);
+ save_map_service_server = mt_nh.advertiseService("/hdl_graph_slam/save_map", &HdlGraphSlamNodelet::save_map_service, this);
+
+ graph_updated = false;
+ double graph_update_interval = private_nh.param("graph_update_interval", 3.0);
+ double map_cloud_update_interval = private_nh.param("map_cloud_update_interval", 10.0);
+ optimization_timer = mt_nh.createWallTimer(ros::WallDuration(graph_update_interval), &HdlGraphSlamNodelet::optimization_timer_callback, this);
+ map_publish_timer = mt_nh.createWallTimer(ros::WallDuration(map_cloud_update_interval), &HdlGraphSlamNodelet::map_points_publish_timer_callback, this);
+ }
+
+private:
+ /**
+ * @brief received point clouds are pushed to #keyframe_queue
+ * @param odom_msg
+ * @param cloud_msg
+ */
+ void cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg, const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) {
+ const ros::Time& stamp = cloud_msg->header.stamp;
+ Eigen::Isometry3d odom = odom2isometry(odom_msg);
+
+ pcl::PointCloud::Ptr cloud(new pcl::PointCloud());
+ pcl::fromROSMsg(*cloud_msg, *cloud);
+ if(base_frame_id.empty()) {
+ base_frame_id = cloud_msg->header.frame_id;
+ }
+
+ if(!keyframe_updater->update(odom)) {
+ std::lock_guard lock(keyframe_queue_mutex);
+ if(keyframe_queue.empty()) {
+ std_msgs::Header read_until;
+ read_until.stamp = stamp + ros::Duration(10, 0);
+ read_until.frame_id = points_topic;
+ read_until_pub.publish(read_until);
+ read_until.frame_id = "/filtered_points";
+ read_until_pub.publish(read_until);
+ }
+
+ return;
+ }
+
+ double accum_d = keyframe_updater->get_accum_distance();
+
+ pcl::Filter::Ptr keyframe_downsample_filter;
+ double res_ = 0.5;
+ auto keyframe_voxelgrid = new pcl::VoxelGrid();
+ keyframe_voxelgrid->setLeafSize(res_, res_, res_);
+ keyframe_downsample_filter.reset(keyframe_voxelgrid);
+
+ pcl::PointCloud::Ptr filtered(new pcl::PointCloud());
+ keyframe_downsample_filter->setInputCloud(cloud);
+ keyframe_downsample_filter->filter(*filtered);
+ KeyFrame::Ptr keyframe(new KeyFrame(stamp, odom, accum_d, filtered));
+
+ std::lock_guard lock(keyframe_queue_mutex);
+ keyframe_queue.push_back(keyframe);
+ }
+
+ /**
+ * @brief this method adds all the keyframes in #keyframe_queue to the pose graph (odometry edges)
+ * @return if true, at least one keyframe was added to the pose graph
+ */
+ bool flush_keyframe_queue() {
+ std::lock_guard lock(keyframe_queue_mutex);
+
+ if(keyframe_queue.empty()) {
+ return false;
+ }
+
+ trans_odom2map_mutex.lock();
+ Eigen::Isometry3d odom2map(trans_odom2map.cast());
+ trans_odom2map_mutex.unlock();
+
+ int num_processed = 0;
+ for(int i = 0; i < std::min(keyframe_queue.size(), max_keyframes_per_update); i++) {
+ num_processed = i;
+
+ const auto& keyframe = keyframe_queue[i];
+ // new_keyframes will be tested later for loop closure
+ new_keyframes.push_back(keyframe);
+
+ // add pose node
+ Eigen::Isometry3d odom = odom2map * keyframe->odom;
+ keyframe->node = graph_slam->add_se3_node(odom);
+ keyframe_hash[keyframe->stamp] = keyframe;
+
+ // fix the first node
+ if(keyframes.empty() && new_keyframes.size() == 1) {
+ if(private_nh.param("fix_first_node", false)) {
+ Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6);
+ std::stringstream sst(private_nh.param("fix_first_node_stddev", "1 1 1 1 1 1"));
+ for(int i = 0; i < 6; i++) {
+ double stddev = 1.0;
+ sst >> stddev;
+ inf(i, i) = 1.0 / stddev;
+ }
+
+ anchor_node = graph_slam->add_se3_node(Eigen::Isometry3d::Identity());
+ anchor_node->setFixed(true);
+ anchor_edge = graph_slam->add_se3_edge(anchor_node, keyframe->node, Eigen::Isometry3d::Identity(), inf);
+ }
+ }
+
+ if(i == 0 && keyframes.empty()) {
+ continue;
+ }
+
+ // add edge between consecutive keyframes
+ const auto& prev_keyframe = i == 0 ? keyframes.back() : keyframe_queue[i - 1];
+
+ Eigen::Isometry3d relative_pose = keyframe->odom.inverse() * prev_keyframe->odom;
+ Eigen::MatrixXd information = inf_calclator->calc_information_matrix(keyframe->cloud, prev_keyframe->cloud, relative_pose);
+ auto edge = graph_slam->add_se3_edge(keyframe->node, prev_keyframe->node, relative_pose, information);
+ graph_slam->add_robust_kernel(edge, private_nh.param("odometry_edge_robust_kernel", "NONE"), private_nh.param("odometry_edge_robust_kernel_size", 1.0));
+ }
+
+ std_msgs::Header read_until;
+ read_until.stamp = keyframe_queue[num_processed]->stamp + ros::Duration(10, 0);
+ read_until.frame_id = points_topic;
+ read_until_pub.publish(read_until);
+ read_until.frame_id = "/filtered_points";
+ read_until_pub.publish(read_until);
+
+ keyframe_queue.erase(keyframe_queue.begin(), keyframe_queue.begin() + num_processed + 1);
+ return true;
+ }
+
+ void nmea_callback(const nmea_msgs::SentenceConstPtr& nmea_msg) {
+ GPRMC grmc = nmea_parser->parse(nmea_msg->sentence);
+
+ if(grmc.status != 'A') {
+ return;
+ }
+
+ geographic_msgs::GeoPointStampedPtr gps_msg(new geographic_msgs::GeoPointStamped());
+ gps_msg->header = nmea_msg->header;
+ gps_msg->position.latitude = grmc.latitude;
+ gps_msg->position.longitude = grmc.longitude;
+ gps_msg->position.altitude = NAN;
+
+ gps_callback(gps_msg);
+ }
+
+ void navsat_callback(const sensor_msgs::NavSatFixConstPtr& navsat_msg) {
+ geographic_msgs::GeoPointStampedPtr gps_msg(new geographic_msgs::GeoPointStamped());
+ gps_msg->header = navsat_msg->header;
+ gps_msg->position.latitude = navsat_msg->latitude;
+ gps_msg->position.longitude = navsat_msg->longitude;
+ gps_msg->position.altitude = navsat_msg->altitude;
+ gps_callback(gps_msg);
+ }
+
+ /**
+ * @brief received gps data is added to #gps_queue
+ * @param gps_msg
+ */
+ void gps_callback(const geographic_msgs::GeoPointStampedPtr& gps_msg) {
+ std::lock_guard lock(gps_queue_mutex);
+ gps_msg->header.stamp += ros::Duration(gps_time_offset);
+ gps_queue.push_back(gps_msg);
+ }
+
+ /**
+ * @brief
+ * @return
+ */
+ bool flush_gps_queue() {
+ std::lock_guard lock(gps_queue_mutex);
+
+ if(keyframes.empty() || gps_queue.empty()) {
+ return false;
+ }
+
+ bool updated = false;
+ auto gps_cursor = gps_queue.begin();
+
+ for(auto& keyframe : keyframes) {
+ if(keyframe->stamp > gps_queue.back()->header.stamp) {
+ break;
+ }
+
+ if(keyframe->stamp < (*gps_cursor)->header.stamp || keyframe->utm_coord) {
+ continue;
+ }
+
+ // find the gps data which is closest to the keyframe
+ auto closest_gps = gps_cursor;
+ for(auto gps = gps_cursor; gps != gps_queue.end(); gps++) {
+ auto dt = ((*closest_gps)->header.stamp - keyframe->stamp).toSec();
+ auto dt2 = ((*gps)->header.stamp - keyframe->stamp).toSec();
+ if(std::abs(dt) < std::abs(dt2)) {
+ break;
+ }
+
+ closest_gps = gps;
+ }
+
+ // if the time residual between the gps and keyframe is too large, skip it
+ gps_cursor = closest_gps;
+ if(0.2 < std::abs(((*closest_gps)->header.stamp - keyframe->stamp).toSec())) {
+ continue;
+ }
+
+ // convert (latitude, longitude, altitude) -> (easting, northing, altitude) in UTM coordinate
+ geodesy::UTMPoint utm;
+ geodesy::fromMsg((*closest_gps)->position, utm);
+ Eigen::Vector3d xyz(utm.easting, utm.northing, utm.altitude);
+
+ // the first gps data position will be the origin of the map
+ if(!zero_utm) {
+ zero_utm = xyz;
+ }
+ xyz -= (*zero_utm);
+
+ keyframe->utm_coord = xyz;
+
+ g2o::OptimizableGraph::Edge* edge;
+ if(std::isnan(xyz.z())) {
+ Eigen::Matrix2d information_matrix = Eigen::Matrix2d::Identity() / gps_edge_stddev_xy;
+ edge = graph_slam->add_se3_prior_xy_edge(keyframe->node, xyz.head<2>(), information_matrix);
+ } else {
+ Eigen::Matrix3d information_matrix = Eigen::Matrix3d::Identity();
+ information_matrix.block<2, 2>(0, 0) /= gps_edge_stddev_xy;
+ information_matrix(2, 2) /= gps_edge_stddev_z;
+ edge = graph_slam->add_se3_prior_xyz_edge(keyframe->node, xyz, information_matrix);
+ }
+ graph_slam->add_robust_kernel(edge, private_nh.param("gps_edge_robust_kernel", "NONE"), private_nh.param("gps_edge_robust_kernel_size", 1.0));
+
+ updated = true;
+ }
+
+ auto remove_loc = std::upper_bound(gps_queue.begin(), gps_queue.end(), keyframes.back()->stamp, [=](const ros::Time& stamp, const geographic_msgs::GeoPointStampedConstPtr& geopoint) { return stamp < geopoint->header.stamp; });
+ gps_queue.erase(gps_queue.begin(), remove_loc);
+ return updated;
+ }
+
+ void imu_callback(const sensor_msgs::ImuPtr& imu_msg) {
+ if(!enable_imu_orientation && !enable_imu_acceleration) {
+ return;
+ }
+
+ std::lock_guard lock(imu_queue_mutex);
+ imu_msg->header.stamp += ros::Duration(imu_time_offset);
+ imu_queue.push_back(imu_msg);
+ }
+
+ bool flush_imu_queue() {
+ std::lock_guard lock(imu_queue_mutex);
+ if(keyframes.empty() || imu_queue.empty() || base_frame_id.empty()) {
+ return false;
+ }
+
+ bool updated = false;
+ auto imu_cursor = imu_queue.begin();
+
+ for(auto& keyframe : keyframes) {
+ if(keyframe->stamp > imu_queue.back()->header.stamp) {
+ break;
+ }
+
+ if(keyframe->stamp < (*imu_cursor)->header.stamp || keyframe->acceleration) {
+ continue;
+ }
+
+ // find imu data which is closest to the keyframe
+ auto closest_imu = imu_cursor;
+ for(auto imu = imu_cursor; imu != imu_queue.end(); imu++) {
+ auto dt = ((*closest_imu)->header.stamp - keyframe->stamp).toSec();
+ auto dt2 = ((*imu)->header.stamp - keyframe->stamp).toSec();
+ if(std::abs(dt) < std::abs(dt2)) {
+ break;
+ }
+
+ closest_imu = imu;
+ }
+
+ imu_cursor = closest_imu;
+ if(0.2 < std::abs(((*closest_imu)->header.stamp - keyframe->stamp).toSec())) {
+ continue;
+ }
+
+ const auto& imu_ori = (*closest_imu)->orientation;
+ const auto& imu_acc = (*closest_imu)->linear_acceleration;
+
+ geometry_msgs::Vector3Stamped acc_imu;
+ geometry_msgs::Vector3Stamped acc_base;
+ geometry_msgs::QuaternionStamped quat_imu;
+ geometry_msgs::QuaternionStamped quat_base;
+
+ quat_imu.header.frame_id = acc_imu.header.frame_id = (*closest_imu)->header.frame_id;
+ quat_imu.header.stamp = acc_imu.header.stamp = ros::Time(0);
+ acc_imu.vector = (*closest_imu)->linear_acceleration;
+ quat_imu.quaternion = (*closest_imu)->orientation;
+
+ try {
+ tf_listener.transformVector(base_frame_id, acc_imu, acc_base);
+ tf_listener.transformQuaternion(base_frame_id, quat_imu, quat_base);
+ } catch(std::exception& e) {
+ std::cerr << "failed to find transform!!" << std::endl;
+ return false;
+ }
+
+ keyframe->acceleration = Eigen::Vector3d(acc_base.vector.x, acc_base.vector.y, acc_base.vector.z);
+ keyframe->orientation = Eigen::Quaterniond(quat_base.quaternion.w, quat_base.quaternion.x, quat_base.quaternion.y, quat_base.quaternion.z);
+ keyframe->orientation = keyframe->orientation;
+ if(keyframe->orientation->w() < 0.0) {
+ keyframe->orientation->coeffs() = -keyframe->orientation->coeffs();
+ }
+
+ if(enable_imu_orientation) {
+ Eigen::MatrixXd info = Eigen::MatrixXd::Identity(3, 3) / imu_orientation_edge_stddev;
+ auto edge = graph_slam->add_se3_prior_quat_edge(keyframe->node, *keyframe->orientation, info);
+ graph_slam->add_robust_kernel(edge, private_nh.param("imu_orientation_edge_robust_kernel", "NONE"), private_nh.param("imu_orientation_edge_robust_kernel_size", 1.0));
+ }
+
+ if(enable_imu_acceleration) {
+ Eigen::MatrixXd info = Eigen::MatrixXd::Identity(3, 3) / imu_acceleration_edge_stddev;
+ g2o::OptimizableGraph::Edge* edge = graph_slam->add_se3_prior_vec_edge(keyframe->node, -Eigen::Vector3d::UnitZ(), *keyframe->acceleration, info);
+ graph_slam->add_robust_kernel(edge, private_nh.param("imu_acceleration_edge_robust_kernel", "NONE"), private_nh.param("imu_acceleration_edge_robust_kernel_size", 1.0));
+ }
+ updated = true;
+ }
+
+ auto remove_loc = std::upper_bound(imu_queue.begin(), imu_queue.end(), keyframes.back()->stamp, [=](const ros::Time& stamp, const sensor_msgs::ImuConstPtr& imu) { return stamp < imu->header.stamp; });
+ imu_queue.erase(imu_queue.begin(), remove_loc);
+
+ return updated;
+ }
+
+ /**
+ * @brief received floor coefficients are added to #floor_coeffs_queue
+ * @param floor_coeffs_msg
+ */
+ void floor_coeffs_callback(const hdl_graph_slam::FloorCoeffsConstPtr& floor_coeffs_msg) {
+ if(floor_coeffs_msg->coeffs.empty()) {
+ return;
+ }
+
+ std::lock_guard lock(floor_coeffs_queue_mutex);
+ floor_coeffs_queue.push_back(floor_coeffs_msg);
+ }
+
+ /**
+ * @brief this methods associates floor coefficients messages with registered keyframes, and then adds the associated coeffs to the pose graph
+ * @return if true, at least one floor plane edge is added to the pose graph
+ */
+ bool flush_floor_queue() {
+ std::lock_guard lock(floor_coeffs_queue_mutex);
+
+ if(keyframes.empty()) {
+ return false;
+ }
+
+ const auto& latest_keyframe_stamp = keyframes.back()->stamp;
+
+ bool updated = false;
+ for(const auto& floor_coeffs : floor_coeffs_queue) {
+ if(floor_coeffs->header.stamp > latest_keyframe_stamp) {
+ break;
+ }
+
+ auto found = keyframe_hash.find(floor_coeffs->header.stamp);
+ if(found == keyframe_hash.end()) {
+ continue;
+ }
+
+ if(!floor_plane_node) {
+ floor_plane_node = graph_slam->add_plane_node(Eigen::Vector4d(0.0, 0.0, 1.0, 0.0));
+ floor_plane_node->setFixed(true);
+ }
+
+ const auto& keyframe = found->second;
+
+ Eigen::Vector4d coeffs(floor_coeffs->coeffs[0], floor_coeffs->coeffs[1], floor_coeffs->coeffs[2], floor_coeffs->coeffs[3]);
+ Eigen::Matrix3d information = Eigen::Matrix3d::Identity() * (1.0 / floor_edge_stddev);
+ auto edge = graph_slam->add_se3_plane_edge(keyframe->node, floor_plane_node, coeffs, information);
+ graph_slam->add_robust_kernel(edge, private_nh.param("floor_edge_robust_kernel", "NONE"), private_nh.param("floor_edge_robust_kernel_size", 1.0));
+
+ keyframe->floor_coeffs = coeffs;
+
+ updated = true;
+ }
+
+ auto remove_loc = std::upper_bound(floor_coeffs_queue.begin(), floor_coeffs_queue.end(), latest_keyframe_stamp, [=](const ros::Time& stamp, const hdl_graph_slam::FloorCoeffsConstPtr& coeffs) { return stamp < coeffs->header.stamp; });
+ floor_coeffs_queue.erase(floor_coeffs_queue.begin(), remove_loc);
+ return updated;
+ }
+
+ /**
+ * @brief generate map point cloud and publish it
+ * @param event
+ */
+ void map_points_publish_timer_callback(const ros::WallTimerEvent& event) {
+ if(!map_points_pub.getNumSubscribers() || !graph_updated) {
+ return;
+ }
+
+ std::vector snapshot;
+
+ keyframes_snapshot_mutex.lock();
+ snapshot = keyframes_snapshot;
+ keyframes_snapshot_mutex.unlock();
+
+ auto cloud = map_cloud_generator->generate(snapshot, map_cloud_resolution);
+ if(!cloud) {
+ return;
+ }
+
+ cloud->header.frame_id = map_frame_id;
+ cloud->header.stamp = snapshot.back()->cloud->header.stamp;
+
+ sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2());
+ pcl::toROSMsg(*cloud, *cloud_msg);
+
+ map_points_pub.publish(cloud_msg);
+ }
+
+ /**
+ * @brief this methods adds all the data in the queues to the pose graph, and then optimizes the pose graph
+ * @param event
+ */
+ void optimization_timer_callback(const ros::WallTimerEvent& event) {
+ std::lock_guard lock(main_thread_mutex);
+
+ // add keyframes and floor coeffs in the queues to the pose graph
+ bool keyframe_updated = flush_keyframe_queue();
+
+ if(!keyframe_updated) {
+ std_msgs::Header read_until;
+ read_until.stamp = ros::Time::now() + ros::Duration(30, 0);
+ read_until.frame_id = points_topic;
+ read_until_pub.publish(read_until);
+ read_until.frame_id = "/filtered_points";
+ read_until_pub.publish(read_until);
+ }
+
+
+ if(!keyframe_updated & !flush_floor_queue()) {
+ return;
+ }
+
+ // loop detection
+ bool loop_detect_ = true;
+ if(loop_detect_)
+ {
+ std::vector loops = loop_detector->detect(keyframes, new_keyframes, *graph_slam);
+ std::cout<<"loops size: "<relative_pose.cast());
+ Eigen::MatrixXd information_matrix = inf_calclator->calc_information_matrix(loop->key1->cloud, loop->key2->cloud, relpose);
+ auto edge = graph_slam->add_se3_edge(loop->key1->node, loop->key2->node, relpose, information_matrix);
+ graph_slam->add_robust_kernel(edge, private_nh.param("loop_closure_edge_robust_kernel", "NONE"), private_nh.param("loop_closure_edge_robust_kernel_size", 1.0));
+ }
+
+ }
+
+ std::copy(new_keyframes.begin(), new_keyframes.end(), std::back_inserter(keyframes));
+ new_keyframes.clear();
+
+ // move the first node anchor position to the current estimate of the first node pose
+ // so the first node moves freely while trying to stay around the origin
+ if(anchor_node && private_nh.param("fix_first_node_adaptive", true)) {
+ Eigen::Isometry3d anchor_target = static_cast(anchor_edge->vertices()[1])->estimate();
+ anchor_node->setEstimate(anchor_target);
+ }
+
+ // optimize the pose graph
+ bool use_g2o = true;
+ if(use_g2o)
+ {
+ int num_iterations = private_nh.param("g2o_solver_num_iterations", 1024);
+ graph_slam->optimize(num_iterations);
+
+ }
+
+
+ //TO save the trajectory.
+ cout<<"begin save trajectory"<node->estimate();
+ keyframe_M = keyframe_T.matrix();
+ R = keyframe_M.block<3,3>(0,0);
+ q = R;
+ traj<stamp<<" "<get_accum_distance()<node->estimate() * keyframe->odom.inverse();
+ trans_odom2map_mutex.lock();
+ trans_odom2map = trans.matrix().cast();
+ trans_odom2map_mutex.unlock();
+
+ std::vector snapshot(keyframes.size());
+ std::transform(keyframes.begin(), keyframes.end(), snapshot.begin(), [=](const KeyFrame::Ptr& k) { return std::make_shared(k); });
+
+ keyframes_snapshot_mutex.lock();
+ keyframes_snapshot.swap(snapshot);
+ keyframes_snapshot_mutex.unlock();
+ graph_updated = true;
+
+ if(odom2map_pub.getNumSubscribers()) {
+ geometry_msgs::TransformStamped ts = matrix2transform(keyframe->stamp, trans.matrix().cast(), map_frame_id, odom_frame_id);
+ odom2map_pub.publish(ts);
+ }
+
+ if(markers_pub.getNumSubscribers()) {
+ auto markers = create_marker_array(ros::Time::now());
+ markers_pub.publish(markers);
+ }
+ }
+
+ /**
+ * @brief create visualization marker
+ * @param stamp
+ * @return
+ */
+ visualization_msgs::MarkerArray create_marker_array(const ros::Time& stamp) const {
+ visualization_msgs::MarkerArray markers;
+ markers.markers.resize(4);
+
+ // node markers
+ visualization_msgs::Marker& traj_marker = markers.markers[0];
+ traj_marker.header.frame_id = "map";
+ traj_marker.header.stamp = stamp;
+ traj_marker.ns = "nodes";
+ traj_marker.id = 0;
+ traj_marker.type = visualization_msgs::Marker::SPHERE_LIST;
+
+ traj_marker.pose.orientation.w = 1.0;
+ traj_marker.scale.x = traj_marker.scale.y = traj_marker.scale.z = 2.0;//1.0
+
+ visualization_msgs::Marker& imu_marker = markers.markers[1];
+ imu_marker.header = traj_marker.header;
+ imu_marker.ns = "imu";
+ imu_marker.id = 1;
+ imu_marker.type = visualization_msgs::Marker::SPHERE_LIST;
+
+ imu_marker.pose.orientation.w = 1.0;
+ imu_marker.scale.x = imu_marker.scale.y = imu_marker.scale.z = 0.75;
+
+ traj_marker.points.resize(keyframes.size());
+ traj_marker.colors.resize(keyframes.size());
+ for(int i = 0; i < keyframes.size(); i++) {
+ Eigen::Vector3d pos = keyframes[i]->node->estimate().translation();
+ traj_marker.points[i].x = pos.x();
+ traj_marker.points[i].y = pos.y();
+ traj_marker.points[i].z = pos.z();
+
+ double p = static_cast(i) / keyframes.size();
+ traj_marker.colors[i].r = 1.0 - p;
+ traj_marker.colors[i].g = p;
+ traj_marker.colors[i].b = 0.0;
+ traj_marker.colors[i].a = 1.0;
+
+ if(keyframes[i]->acceleration) {
+ Eigen::Vector3d pos = keyframes[i]->node->estimate().translation();
+ geometry_msgs::Point point;
+ point.x = pos.x();
+ point.y = pos.y();
+ point.z = pos.z();
+
+ std_msgs::ColorRGBA color;
+ color.r = 0.0;
+ color.g = 0.0;
+ color.b = 1.0;
+ color.a = 0.1;
+
+ imu_marker.points.push_back(point);
+ imu_marker.colors.push_back(color);
+ }
+ }
+
+ // edge markers
+ visualization_msgs::Marker& edge_marker = markers.markers[2];
+ edge_marker.header.frame_id = "map";
+ edge_marker.header.stamp = stamp;
+ edge_marker.ns = "edges";
+ edge_marker.id = 2;
+ edge_marker.type = visualization_msgs::Marker::LINE_LIST;
+
+ edge_marker.pose.orientation.w = 1.0;
+ edge_marker.scale.x = 2;//0.05
+
+ edge_marker.points.resize(graph_slam->graph->edges().size() * 2);
+ edge_marker.colors.resize(graph_slam->graph->edges().size() * 2);
+
+ auto edge_itr = graph_slam->graph->edges().begin();
+ for(int i = 0; edge_itr != graph_slam->graph->edges().end(); edge_itr++, i++) {
+ g2o::HyperGraph::Edge* edge = *edge_itr;
+ g2o::EdgeSE3* edge_se3 = dynamic_cast(edge);
+ if(edge_se3) {
+ g2o::VertexSE3* v1 = dynamic_cast(edge_se3->vertices()[0]);
+ g2o::VertexSE3* v2 = dynamic_cast(edge_se3->vertices()[1]);
+ Eigen::Vector3d pt1 = v1->estimate().translation();
+ Eigen::Vector3d pt2 = v2->estimate().translation();
+
+ edge_marker.points[i * 2].x = pt1.x();
+ edge_marker.points[i * 2].y = pt1.y();
+ edge_marker.points[i * 2].z = pt1.z();
+ edge_marker.points[i * 2 + 1].x = pt2.x();
+ edge_marker.points[i * 2 + 1].y = pt2.y();
+ edge_marker.points[i * 2 + 1].z = pt2.z();
+
+ double p1 = static_cast(v1->id()) / graph_slam->graph->vertices().size();
+ double p2 = static_cast(v2->id()) / graph_slam->graph->vertices().size();
+
+ edge_marker.colors[i * 2].r = 1.0 - p1;
+ edge_marker.colors[i * 2].g = p1;
+ edge_marker.colors[i * 2].a = 1.0;
+ edge_marker.colors[i * 2 + 1].r = 1.0 - p2;
+ edge_marker.colors[i * 2 + 1].g = p2;
+ edge_marker.colors[i * 2 + 1].a = 1.0;
+
+
+ if(std::abs(v1->id() - v2->id()) > 2) {
+ edge_marker.points[i * 2].z += 0.5;
+ edge_marker.points[i * 2 + 1].z += 0.5;
+ }
+
+ continue;
+ }
+
+ g2o::EdgeSE3Plane* edge_plane = dynamic_cast(edge);
+ if(edge_plane) {
+ g2o::VertexSE3* v1 = dynamic_cast(edge_plane->vertices()[0]);
+ Eigen::Vector3d pt1 = v1->estimate().translation();
+ Eigen::Vector3d pt2(pt1.x(), pt1.y(), 0.0);
+
+ edge_marker.points[i * 2].x = pt1.x();
+ edge_marker.points[i * 2].y = pt1.y();
+ edge_marker.points[i * 2].z = pt1.z();
+ edge_marker.points[i * 2 + 1].x = pt2.x();
+ edge_marker.points[i * 2 + 1].y = pt2.y();
+ edge_marker.points[i * 2 + 1].z = pt2.z();
+
+ edge_marker.colors[i * 2].b = 1.0;
+ edge_marker.colors[i * 2].a = 1.0;
+ edge_marker.colors[i * 2 + 1].b = 1.0;
+ edge_marker.colors[i * 2 + 1].a = 1.0;
+
+ continue;
+ }
+
+ g2o::EdgeSE3PriorXY* edge_priori_xy = dynamic_cast(edge);
+ if(edge_priori_xy) {
+ g2o::VertexSE3* v1 = dynamic_cast(edge_priori_xy->vertices()[0]);
+ Eigen::Vector3d pt1 = v1->estimate().translation();
+ Eigen::Vector3d pt2 = Eigen::Vector3d::Zero();
+ pt2.head<2>() = edge_priori_xy->measurement();
+
+ edge_marker.points[i * 2].x = pt1.x();
+ edge_marker.points[i * 2].y = pt1.y();
+ edge_marker.points[i * 2].z = pt1.z() + 0.5;
+ edge_marker.points[i * 2 + 1].x = pt2.x();
+ edge_marker.points[i * 2 + 1].y = pt2.y();
+ edge_marker.points[i * 2 + 1].z = pt2.z() + 0.5;
+
+ edge_marker.colors[i * 2].r = 1.0;
+ edge_marker.colors[i * 2].a = 1.0;
+ edge_marker.colors[i * 2 + 1].r = 1.0;
+ edge_marker.colors[i * 2 + 1].a = 1.0;
+
+ continue;
+ }
+
+ g2o::EdgeSE3PriorXYZ* edge_priori_xyz = dynamic_cast(edge);
+ if(edge_priori_xyz) {
+ g2o::VertexSE3* v1 = dynamic_cast(edge_priori_xyz->vertices()[0]);
+ Eigen::Vector3d pt1 = v1->estimate().translation();
+ Eigen::Vector3d pt2 = edge_priori_xyz->measurement();
+
+ edge_marker.points[i * 2].x = pt1.x();
+ edge_marker.points[i * 2].y = pt1.y();
+ edge_marker.points[i * 2].z = pt1.z() + 0.5;
+ edge_marker.points[i * 2 + 1].x = pt2.x();
+ edge_marker.points[i * 2 + 1].y = pt2.y();
+ edge_marker.points[i * 2 + 1].z = pt2.z();
+
+ edge_marker.colors[i * 2].r = 1.0;
+ edge_marker.colors[i * 2].a = 1.0;
+ edge_marker.colors[i * 2 + 1].r = 1.0;
+ edge_marker.colors[i * 2 + 1].a = 1.0;
+
+ continue;
+ }
+ }
+
+ // sphere
+ visualization_msgs::Marker& sphere_marker = markers.markers[3];
+ sphere_marker.header.frame_id = "map";
+ sphere_marker.header.stamp = stamp;
+ sphere_marker.ns = "loop_close_radius";
+ sphere_marker.id = 3;
+ sphere_marker.type = visualization_msgs::Marker::SPHERE;
+
+ if(!keyframes.empty()) {
+ Eigen::Vector3d pos = keyframes.back()->node->estimate().translation();
+ sphere_marker.pose.position.x = pos.x();
+ sphere_marker.pose.position.y = pos.y();
+ sphere_marker.pose.position.z = pos.z();
+ }
+ sphere_marker.pose.orientation.w = 1.0;
+ sphere_marker.scale.x = sphere_marker.scale.y = sphere_marker.scale.z = loop_detector->get_distance_thresh() * 2.0;
+
+ sphere_marker.color.r = 1.0;
+ sphere_marker.color.a = 0.3;
+
+ return markers;
+ }
+
+ /**
+ * @brief dump all data to the current directory
+ * @param req
+ * @param res
+ * @return
+ */
+ bool dump_service(hdl_graph_slam::DumpGraphRequest& req, hdl_graph_slam::DumpGraphResponse& res) {
+ std::lock_guard lock(main_thread_mutex);
+
+ std::string directory = req.destination;
+
+ if(directory.empty()) {
+ std::array buffer;
+ buffer.fill(0);
+ time_t rawtime;
+ time(&rawtime);
+ const auto timeinfo = localtime(&rawtime);
+ strftime(buffer.data(), sizeof(buffer), "%d-%m-%Y %H:%M:%S", timeinfo);
+ }
+
+ if(!boost::filesystem::is_directory(directory)) {
+ boost::filesystem::create_directory(directory);
+ }
+
+ std::cout << "all data dumped to:" << directory << std::endl;
+
+ graph_slam->save(directory + "/graph.g2o");
+ for(int i = 0; i < keyframes.size(); i++) {
+ std::stringstream sst;
+ sst << boost::format("%s/%06d") % directory % i;
+
+ keyframes[i]->save(sst.str());
+ }
+
+ if(zero_utm) {
+ std::ofstream zero_utm_ofs(directory + "/zero_utm");
+ zero_utm_ofs << boost::format("%.6f %.6f %.6f") % zero_utm->x() % zero_utm->y() % zero_utm->z() << std::endl;
+ }
+
+ std::ofstream ofs(directory + "/special_nodes.csv");
+ ofs << "anchor_node " << (anchor_node == nullptr ? -1 : anchor_node->id()) << std::endl;
+ ofs << "anchor_edge " << (anchor_edge == nullptr ? -1 : anchor_edge->id()) << std::endl;
+ ofs << "floor_node " << (floor_plane_node == nullptr ? -1 : floor_plane_node->id()) << std::endl;
+
+ res.success = true;
+ return true;
+ }
+
+ /**
+ * @brief save map data as pcd
+ * @param req
+ * @param res
+ * @return
+ */
+ bool save_map_service(hdl_graph_slam::SaveMapRequest& req, hdl_graph_slam::SaveMapResponse& res) {
+ std::vector snapshot;
+
+ keyframes_snapshot_mutex.lock();
+ snapshot = keyframes_snapshot;
+ keyframes_snapshot_mutex.unlock();
+
+ auto cloud = map_cloud_generator->generate(snapshot, req.resolution);
+ if(!cloud) {
+ res.success = false;
+ return true;
+ }
+
+ if(zero_utm && req.utm) {
+ for(auto& pt : cloud->points) {
+ pt.getVector3fMap() += (*zero_utm).cast();
+ }
+ }
+
+ cloud->header.frame_id = map_frame_id;
+ cloud->header.stamp = snapshot.back()->cloud->header.stamp;
+
+ if(zero_utm) {
+ std::ofstream ofs(req.destination + ".utm");
+ ofs << boost::format("%.6f %.6f %.6f") % zero_utm->x() % zero_utm->y() % zero_utm->z() << std::endl;
+ }
+
+ int ret = pcl::io::savePCDFileBinary(req.destination, *cloud);
+ res.success = ret == 0;
+
+ return true;
+ }
+
+private:
+ // ROS
+ ros::NodeHandle nh;
+ ros::NodeHandle mt_nh;
+ ros::NodeHandle private_nh;
+ ros::WallTimer optimization_timer;
+ ros::WallTimer map_publish_timer;
+
+ std::unique_ptr> odom_sub;
+ std::unique_ptr> cloud_sub;
+ std::unique_ptr> sync;
+
+ ros::Subscriber gps_sub;
+ ros::Subscriber nmea_sub;
+ ros::Subscriber navsat_sub;
+
+ ros::Subscriber imu_sub;
+ ros::Subscriber floor_sub;
+
+ ros::Publisher markers_pub;
+
+ std::string map_frame_id;
+ std::string odom_frame_id;
+
+ std::mutex trans_odom2map_mutex;
+ Eigen::Matrix4f trans_odom2map;
+ ros::Publisher odom2map_pub;
+
+ std::string points_topic;
+ ros::Publisher read_until_pub;
+ ros::Publisher map_points_pub;
+
+ tf::TransformListener tf_listener;
+
+ ros::ServiceServer dump_service_server;
+ ros::ServiceServer save_map_service_server;
+
+ // keyframe queue
+ std::string base_frame_id;
+ std::mutex keyframe_queue_mutex;
+ std::deque keyframe_queue;
+
+ // gps queue
+ double gps_time_offset;
+ double gps_edge_stddev_xy;
+ double gps_edge_stddev_z;
+ boost::optional zero_utm;
+ std::mutex gps_queue_mutex;
+ std::deque gps_queue;
+
+ // imu queue
+ double imu_time_offset;
+ bool enable_imu_orientation;
+ double imu_orientation_edge_stddev;
+ bool enable_imu_acceleration;
+ double imu_acceleration_edge_stddev;
+ std::mutex imu_queue_mutex;
+ std::deque imu_queue;
+
+ // floor_coeffs queue
+ double floor_edge_stddev;
+ std::mutex floor_coeffs_queue_mutex;
+ std::deque floor_coeffs_queue;
+
+ // for map cloud generation
+ std::atomic_bool graph_updated;
+ double map_cloud_resolution;
+ std::mutex keyframes_snapshot_mutex;
+ std::vector keyframes_snapshot;
+ std::unique_ptr map_cloud_generator;
+
+ // graph slam
+ // all the below members must be accessed after locking main_thread_mutex
+ std::mutex main_thread_mutex;
+
+ int max_keyframes_per_update;
+ std::deque new_keyframes;
+
+ g2o::VertexSE3* anchor_node;
+ g2o::EdgeSE3* anchor_edge;
+ g2o::VertexPlane* floor_plane_node;
+ std::vector keyframes;
+ std::unordered_map keyframe_hash;
+
+ std::unique_ptr graph_slam;
+ std::unique_ptr loop_detector;
+ std::unique_ptr keyframe_updater;
+ std::unique_ptr nmea_parser;
+
+ std::string seq_;
+ bool use_test_;
+
+
+ std::unique_ptr inf_calclator;
+};
+
+} // namespace hdl_graph_slam
+
+PLUGINLIB_EXPORT_CLASS(hdl_graph_slam::HdlGraphSlamNodelet, nodelet::Nodelet)
diff --git a/apps/prefiltering_nodelet.cpp b/apps/prefiltering_nodelet.cpp
new file mode 100644
index 0000000..07bb648
--- /dev/null
+++ b/apps/prefiltering_nodelet.cpp
@@ -0,0 +1,270 @@
+// SPDX-License-Identifier: BSD-2-Clause
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+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("deskewing", false)) {
+ imu_sub = nh.subscribe("/imu/data", 1, &PrefilteringNodelet::imu_callback, this);
+ }
+
+ points_sub = nh.subscribe("/velodyne_points", 64, &PrefilteringNodelet::cloud_callback, this);
+ points_pub = nh.advertise("/filtered_points", 32);
+ colored_pub = nh.advertise("/colored_points", 32);
+ }
+
+private:
+ void initialize_params() {
+ std::string downsample_method = private_nh.param("downsample_method", "VOXELGRID");
+ double downsample_resolution = private_nh.param("downsample_resolution", 0.1);
+
+ if(downsample_method == "VOXELGRID") {
+ std::cout << "downsample: VOXELGRID " << downsample_resolution << std::endl;
+ auto voxelgrid = new pcl::VoxelGrid();
+ 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::Ptr approx_voxelgrid(new pcl::ApproximateVoxelGrid());
+ 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("outlier_removal_method", "STATISTICAL");
+ if(outlier_removal_method == "STATISTICAL") {
+ int mean_k = private_nh.param("statistical_mean_k", 20);
+ double stddev_mul_thresh = private_nh.param("statistical_stddev", 1.0);
+ std::cout << "outlier_removal: STATISTICAL " << mean_k << " - " << stddev_mul_thresh << std::endl;
+
+ pcl::StatisticalOutlierRemoval