initial commit

CMakeLists.txt

@@ -0,0 +1,114 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(hdl_graph_slam)
+
+# -mavx causes a lot of errors!!
+add_definitions(-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2)
+set(CMAKE_CXX_FLAGS "-std=c++11 -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
+  pcl_ros
+  geodesy
+  nmea_msgs
+  sensor_msgs
+  message_generation
+  ndt_omp
+)
+
+find_package(PCL 1.7 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()
+
+########################
+## message generation ##
+########################
+add_message_files(FILES
+  FloorCoeffs.msg
+)
+
+generate_messages(DEPENDENCIES std_msgs)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  INCLUDE_DIRS include
+#  LIBRARIES hdl_scan_matching_odometry
+#  CATKIN_DEPENDS pcl_ros roscpp sensor_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+# nodelets
+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_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
+)
+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}
+  ${G2O_TYPES_SLAM3D}
+  ${G2O_TYPES_SLAM3D_ADDONS}
+)
+add_dependencies(hdl_graph_slam_nodelet ${PROJECT_NAME}_gencpp)

README.md

@@ -0,0 +1,105 @@
+# hdl_graph_slam
+***hdl_graph_slam*** is an open source ROS package for real-time 3D slam using a 3D LIDAR. It is based on 3D Graph SLAM with NDT scan matching-based odometry estimation and loop detection. It also utilizes floor plane detection to generate an environmental map with a completely flat floor. This package also contains an implementation of multi-threaded NDT (10 times faster than the normal NDT in PCL!). We have tested this packaged mainly in indoor environments, but it can be applied to outdoor environment mapping as well.
+
+<img src="imgs/hdl_graph_slam.png" width="712pix" />
+
+<a href="https://drive.google.com/open?id=0B9f5zFkpn4soSG96Tkt4SFFTbms">video</a> (TODO: upload this video to youtube!)
+
+## 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 odometry, loops, and floor planes into account.<br>
+
+**[Sep/1/2017]** A GPS-based graph optimization has been implemented. *hdl_graph_slam_nodelet* receives GPS data (i.e., latitude, longitude) from */gps/geopoint* and converts them into the UTM coordinate, and then it adds them into the pose graph. It effectively compensate the scan matching error in outdoor environments.
+
+<br>
+<img src="imgs/nodelets.png" width="712pix" />
+
+### Parameters
+All the parameters are listed in *launch/hdl_graph_slam.launch* as ros params.
+
+### Services
+- */hdl_graph_slam/dump*  (std_srvs/Empty)
+  - save all data (point clouds, floor coeffs, odoms, and pose graph) to the current directory.
+
+## Multi-threaded Scan Matching Algorithms
+This package contains implementations of multi-threaded and SSE friendly NDT and GICP algorithms. They are derived from the implementations in PCL. With recent multi-core processors, they may show ten times faster processing speed than the NDT and the GICP in PCL. See *include/pclomp* and *src/pclomp* to use the boosted scan matching algorithms.
+
+## Requirements
+***hdl_graph_slam*** requires the following libraries:
+- OpenMP
+- PCL 1.7
+- g2o
+
+Note that ***hdl_graph_slam*** cannot be built with older g2o libraries (such as ros-indigo-libg2o). Install the latest g2o:
+```bash
+git clone https://github.com/RainerKuemmerle/g2o.git
+cd g2o
+mkdir build && cd build
+cmake .. -DCMAKE_BUILD_TYPE=RELEASE
+make -j8
+sudo make install
+```
+
+The following ros packages are required:
+- geodesy
+- pcl_ros
+```bash
+sudo apt-get install ros-indigo-geodesy ros-indigo-pcl_ros
+```
+
+## Example
+
+Example bag files (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 adjusts the playback speed according to the processing speed of your PC. It allows processing data as fast as possible for your PC.
+
+```bash
+rosrun hdl_graph_slam bag_player.py hdl_501_filtered.bag
+```
+
+You'll see a generated point cloud like:
+
+<img src="imgs/top.png" height="256pix" /> <img src="imgs/birds.png" height="256pix" /> 
+
+
+## Example with GPS feature
+Ford Campus Vision and Lidar Data Set <a href="http://robots.engin.umich.edu/SoftwareData/Ford">[URL]</a>.
+
+The following script converts the Ford Lidar Dataset to a rosbag and plays it. In this example, ***hdl_graph_slam*** utilized 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
+```
+
+<img src="imgs/ford1.png" height="200pix"/> <img src="imgs/ford2.png" height="200pix"/> <img src="imgs/ford3.png" height="200pix"/>
+
+## Papers
+Kenji Koide, Jun Miura, and Emanuele Menegatti, A Portable 3D LIDAR-based System for Long-term and Wide-area People Behavior Measurement, Robotics and Autonomous Systems (under review) [PDF].
+
+## Contact
+Kenji Koide, Active Intelligent Systems Laboratory, Toyohashi University of Technology <a href="http://www.aisl.cs.tut.ac.jp">[URL]</a> <br>
+[email protected]
+