├── CMakeLists.txt
├── README.md
├── docker
├── Dockerfile
├── Makefile
└── run.sh
├── include
├── aloam_velodyne
│ ├── common.h
│ └── tic_toc.h
└── scancontext
│ ├── KDTreeVectorOfVectorsAdaptor.h
│ ├── Scancontext.cpp
│ ├── Scancontext.h
│ └── nanoflann.hpp
├── launch
├── aloam_mulran.launch
├── aloam_velodyne_HDL_32.launch
├── aloam_velodyne_HDL_64.launch
├── aloam_velodyne_VLP_16.launch
└── kitti_helper.launch
├── package.xml
├── picture
├── anypipe.png
├── indoor.png
├── kaist-03-1.png
├── kaist-03-2.png
├── kaist-03-3.png
├── kaist-03-4.png
├── kaist-03-5.png
├── kaist-03-merged.png
├── kitti.png
├── kitti05.png
├── kitti_gif.gif
├── riverside01.png
└── scfastlio.png
├── rviz_cfg
└── aloam_velodyne.rviz
├── src
├── kittiHelper.cpp
├── laserMapping.cpp
├── laserOdometry.cpp
├── laserPosegraphOptimization.cpp
├── lidarFactor.hpp
└── scanRegistration.cpp
└── utils
├── python
├── __pycache__
│ └── pypcdMyUtils.cpython-36.pyc
├── bone_table.npy
├── jet_table.npy
├── makeMergedMap.py
└── pypcdMyUtils.py
└── sample_data
├── KAIST03
├── Scans
│ ├── 000000.pcd
│ ├── 000001.pcd
│ ├── 000002.pcd
│ ├── 000003.pcd
│ ├── 000004.pcd
│ ├── 000005.pcd
│ ├── 000006.pcd
│ ├── 000007.pcd
│ ├── 000008.pcd
│ ├── 000009.pcd
│ ├── 000010.pcd
│ ├── 000011.pcd
│ ├── 000012.pcd
│ ├── 000013.pcd
│ ├── 000014.pcd
│ ├── 000015.pcd
│ ├── 000016.pcd
│ ├── 000017.pcd
│ ├── 000018.pcd
│ ├── 000019.pcd
│ └── 000020.pcd
├── optimized_poses.txt
└── times.txt
└── Seosan01
├── Scans
├── 000000.pcd
├── 000001.pcd
├── 000002.pcd
├── 000003.pcd
├── 000004.pcd
├── 000005.pcd
├── 000006.pcd
├── 000007.pcd
├── 000008.pcd
├── 000009.pcd
├── 000010.pcd
├── 000011.pcd
├── 000012.pcd
├── 000013.pcd
├── 000014.pcd
├── 000015.pcd
├── 000016.pcd
├── 000017.pcd
├── 000018.pcd
├── 000019.pcd
└── 000020.pcd
├── optimized_poses.txt
└── times.txt
/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 2.8.3)
2 | project(aloam_velodyne)
3 |
4 | set(CMAKE_BUILD_TYPE "Release")
5 | # set(CMAKE_CXX_FLAGS "-std=c++11")
6 | set(CMAKE_CXX_STANDARD 17)
7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
8 |
9 | find_package(catkin REQUIRED COMPONENTS
10 | geometry_msgs
11 | nav_msgs
12 | sensor_msgs
13 | roscpp
14 | rospy
15 | rosbag
16 | std_msgs
17 | image_transport
18 | cv_bridge
19 | tf
20 | )
21 |
22 | #find_package(Eigen3 REQUIRED)
23 | find_package(PCL REQUIRED)
24 | find_package(OpenCV REQUIRED)
25 | find_package(Ceres REQUIRED)
26 |
27 | find_package(OpenMP REQUIRED)
28 | find_package(GTSAM REQUIRED QUIET)
29 |
30 | include_directories(
31 | include
32 | ${catkin_INCLUDE_DIRS}
33 | ${PCL_INCLUDE_DIRS}
34 | ${CERES_INCLUDE_DIRS}
35 | ${OpenCV_INCLUDE_DIRS}
36 | ${GTSAM_INCLUDE_DIR}
37 | )
38 |
39 | catkin_package(
40 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs
41 | DEPENDS EIGEN3 PCL
42 | INCLUDE_DIRS include
43 | )
44 |
45 |
46 | add_executable(ascanRegistration src/scanRegistration.cpp)
47 | target_link_libraries(ascanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES})
48 |
49 | add_executable(alaserOdometry src/laserOdometry.cpp)
50 | target_link_libraries(alaserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES})
51 |
52 | add_executable(alaserMapping src/laserMapping.cpp)
53 | target_link_libraries(alaserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES})
54 |
55 | add_executable(alaserPGO
56 | src/laserPosegraphOptimization.cpp
57 | include/scancontext/Scancontext.cpp
58 | )
59 | target_compile_options(alaserPGO
60 | PRIVATE ${OpenMP_CXX_FLAGS}
61 | )
62 | target_link_libraries(alaserPGO
63 | ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}
64 | ${OpenMP_CXX_FLAGS}
65 | gtsam
66 | )
67 |
68 | add_executable(kittiHelper src/kittiHelper.cpp)
69 | target_link_libraries(kittiHelper ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS})
70 |
71 |
72 |
73 |
74 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # SC-A-LOAM
2 |
3 | ## News
4 | - ``2021-07-16``: This repository's easy-to-use plug-and-play loop detection and pose graph optimization module (named [SC-PGO](https://github.com/gisbi-kim/SC-A-LOAM/blob/main/src/laserPosegraphOptimization.cpp)) is also integrated with FAST-LIO2! see [FAST_LIO_SLAM](https://github.com/gisbi-kim/FAST_LIO_SLAM).
5 |
6 | ## What is SC-A-LOAM?
7 | - A real-time LiDAR SLAM package that integrates A-LOAM and ScanContext.
8 | - **A-LOAM** for odometry (i.e., consecutive motion estimation)
9 | - **ScanContext** for coarse global localization that can deal with big drifts (i.e., place recognition as kidnapped robot problem without initial pose)
10 | - and iSAM2 of GTSAM is used for pose-graph optimization.
11 | - This package aims to show ScanContext's handy applicability.
12 | - The only things a user should do is just to include `Scancontext.h`, call `makeAndSaveScancontextAndKeys` and `detectLoopClosureID`.
13 |
14 | ## Features
15 | 1. A strong place recognition and loop closing
16 | - We integrated ScanContext as a loop detector into A-LOAM, and ISAM2-based pose-graph optimization is followed. (see https://youtu.be/okML_zNadhY?t=313 to enjoy the drift-closing moment)
17 | 2. A modular implementation
18 | - The only difference from A-LOAM is the addition of the `laserPosegraphOptimization.cpp` file. In the new file, we subscribe the point cloud topic and odometry topic (as a result of A-LOAM, published from `laserMapping.cpp`). That is, our implementation is generic to any front-end odometry methods. Thus, our pose-graph optimization module (i.e., `laserPosegraphOptimization.cpp`) can easily be integrated with any odometry algorithms such as non-LOAM family or even other sensors (e.g., visual odometry).
19 | - 
20 | 3. (optional) Altitude stabilization using consumer-level GPS
21 | - To make a result more trustworthy, we supports GPS (consumer-level price, such as U-Blox EVK-7P)-based altitude stabilization. The LOAM family of methods are known to be susceptible to z-errors in outdoors. We used the robust loss for only the altitude term. For the details, see the variable `robustGPSNoise` in the `laserPosegraphOptimization.cpp` file.
22 |
23 | ## Prerequisites (dependencies)
24 | - We mainly depend on ROS, Ceres (for A-LOAM), and GTSAM (for pose-graph optimization).
25 | - For the details to install the prerequisites, please follow the A-LOAM and LIO-SAM repositiory.
26 | - The below examples are done under ROS melodic (ubuntu 18) and GTSAM version 4.x.
27 |
28 | ## How to use?
29 | - First, install the abovementioned dependencies, and follow below lines.
30 | ```
31 | mkdir -p ~/catkin_scaloam_ws/src
32 | cd ~/catkin_scaloam_ws/src
33 | git clone https://github.com/gisbi-kim/SC-A-LOAM.git
34 | cd ../
35 | catkin_make
36 | source ~/catkin_scaloam_ws/devel/setup.bash
37 | roslaunch aloam_velodyne aloam_mulran.launch # for MulRan dataset setting
38 | ```
39 |
40 | ## Example Results
41 |
42 | ### Riverside 01, MulRan dataset
43 | - The MulRan dataset provides lidar scans (Ouster OS1-64, horizontally mounted, 10Hz) and consumer level gps (U-Blox EVK-7P, 4Hz) data.
44 | - About how to use (publishing data) data: see here https://github.com/irapkaist/file_player_mulran
45 | - example videos on Riverside 01 sequence.
46 | 1. with consumer level GPS-based altitude stabilization: https://youtu.be/FwAVX5TVm04
47 | 2. without the z stabilization: https://youtu.be/okML_zNadhY
48 | - example result:
49 |
50 | 
51 |
52 | ### KITTI 05
53 | - For KITTI (HDL-64 sensor), run using the command
54 | ```
55 | roslaunch aloam_velodyne aloam_velodyne_HDL_64.launch # for KITTI dataset setting
56 | ```
57 | - To publish KITTI scans, you can use mini-kitti publisher, a simple python script: https://github.com/gisbi-kim/mini-kitti-publisher
58 | - example video (no GPS used here): https://youtu.be/hk3Xx8SKkv4
59 | - example result:
60 |
61 | 
62 |
63 | ### Indoor
64 | - ScanContext also works at indoor environments (use smaller sc_max_radius value).
65 | - example video: https://youtu.be/Uv6_BRmxJho
66 | - example result:
67 | 
68 |
69 | ### For Livox LiDAR
70 | - Scan Context also works for Livox LiDAR data
71 | - In this example, Scan Context is integrated with FAST-LIO (https://github.com/hku-mars/FAST_LIO).
72 | - Note: No additional integration effort is required. A user just run seperately FAST-LIO node and SC-A-LOAM's posegraphoptimization.cpp node!
73 | - example video (tutoial and results): https://youtu.be/Fw9S6D6HozA
74 | - example result:
75 | 
76 |
77 | ### For Navtech Radar
78 | - Scan Context also works for Navtech Radar data!
79 | - For the details, please see
80 | - https://github.com/gisbi-kim/navtech-radar-slam
81 | - used the pose-graph optimization node of this repository (SC-A-LOAM)
82 | - [example video](https://www.youtube.com/watch?v=avtIQ8fesgU&t=128s)
83 |
84 | ## Utilities
85 |
86 | ### Data saver and Map construction
87 | - Similar to the [SC-LIO-SAM's saver utility](https://github.com/gisbi-kim/SC-LIO-SAM#applications), we support pose and scan saver per keyframes. Using these saved data, the map (within ROI) can be constructed offline. See the `utils/python/makeMergedMap.py` and [this tutorial](https://youtu.be/jmR3DH_A4Co).
88 | - Below is the example results of MulRan dataset KAIST 03's merged map, visualized using CloudCompare ([download the map data here](https://www.dropbox.com/sh/96jrpx3x6hh316j/AACb07kGbocnQWMIpksmU6MQa?dl=0)).
89 |
90 | 
91 |
92 | - A user also can remove dynamic points using these saved keyframe poses and scans. See [this tutorial](https://www.youtube.com/watch?v=UiYYrPMcIRU) and our [Removert project](https://github.com/irapkaist/removert).
93 |
94 | ## Acknowledgements
95 | - Thanks to LOAM, A-LOAM, and LIO-SAM code authors. The major codes in this repository are borrowed from their efforts.
96 |
97 | ## Maintainer
98 | - please contact me through `paulgkim@kaist.ac.kr`
99 |
100 | ## TODO
101 | - Delayed RS loop closings
102 | - SLAM with multi-session localization
103 | - More examples on other datasets (KITTI, complex urban dataset, etc.)
104 |
--------------------------------------------------------------------------------
/docker/Dockerfile:
--------------------------------------------------------------------------------
1 | FROM ros:kinetic-perception
2 |
3 | ENV CERES_VERSION="1.12.0"
4 | ENV PCL_VERSION="1.8.0"
5 | ENV CATKIN_WS=/root/catkin_ws
6 |
7 | # setup processors number used to compile library
8 | RUN if [ "x$(nproc)" = "x1" ] ; then export USE_PROC=1 ; else export USE_PROC=$(($(nproc)/2)) ; fi && \
9 | # Install dependencies
10 | apt-get update && apt-get install -y \
11 | cmake \
12 | libatlas-base-dev \
13 | libeigen3-dev \
14 | libgoogle-glog-dev \
15 | libsuitesparse-dev \
16 | python-catkin-tools \
17 | ros-${ROS_DISTRO}-cv-bridge \
18 | ros-${ROS_DISTRO}-image-transport \
19 | ros-${ROS_DISTRO}-message-filters \
20 | ros-${ROS_DISTRO}-tf && \
21 | rm -rf /var/lib/apt/lists/* && \
22 | # Build and install Ceres
23 | git clone https://ceres-solver.googlesource.com/ceres-solver && \
24 | cd ceres-solver && \
25 | git checkout tags/${CERES_VERSION} && \
26 | mkdir build && cd build && \
27 | cmake .. && \
28 | make -j${USE_PROC} install && \
29 | cd ../.. && \
30 | rm -rf ./ceres-solver && \
31 | # Build and install pcl
32 | git clone https://github.com/PointCloudLibrary/pcl.git && \
33 | cd pcl && \
34 | git checkout tags/pcl-${PCL_VERSION} && \
35 | mkdir build && cd build && \
36 | cmake .. && \
37 | make -j${USE_PROC} install && \
38 | cd ../.. && \
39 | rm -rf ./pcl && \
40 | # Setup catkin workspace
41 | mkdir -p $CATKIN_WS/src/A-LOAM/
42 |
43 | # WORKDIR $CATKIN_WS/src
44 |
45 | # Copy A-LOAM
46 | COPY ./ $CATKIN_WS/src/A-LOAM/
47 | # use the following line if you only have this dockerfile
48 | # RUN git clone https://github.com/HKUST-Aerial-Robotics/A-LOAM.git
49 |
50 | # Build A-LOAM
51 | WORKDIR $CATKIN_WS
52 | ENV TERM xterm
53 | ENV PYTHONIOENCODING UTF-8
54 | RUN catkin config \
55 | --extend /opt/ros/$ROS_DISTRO \
56 | --cmake-args \
57 | -DCMAKE_BUILD_TYPE=Release && \
58 | catkin build && \
59 | sed -i '/exec "$@"/i \
60 | source "/root/catkin_ws/devel/setup.bash"' /ros_entrypoint.sh
61 |
--------------------------------------------------------------------------------
/docker/Makefile:
--------------------------------------------------------------------------------
1 | all: help
2 |
3 | help:
4 | @echo ""
5 | @echo "-- Help Menu"
6 | @echo ""
7 | @echo " 1. make build - build all images"
8 | # @echo " 1. make pull - pull all images"
9 | @echo " 1. make clean - remove all images"
10 | @echo ""
11 |
12 | build:
13 | @docker build --tag ros:aloam -f ./Dockerfile ..
14 |
15 | clean:
16 | @docker rmi -f ros:aloam
17 |
--------------------------------------------------------------------------------
/docker/run.sh:
--------------------------------------------------------------------------------
1 | #!/bin/bash
2 | trap : SIGTERM SIGINT
3 |
4 | function abspath() {
5 | # generate absolute path from relative path
6 | # $1 : relative filename
7 | # return : absolute path
8 | if [ -d "$1" ]; then
9 | # dir
10 | (cd "$1"; pwd)
11 | elif [ -f "$1" ]; then
12 | # file
13 | if [[ $1 = /* ]]; then
14 | echo "$1"
15 | elif [[ $1 == */* ]]; then
16 | echo "$(cd "${1%/*}"; pwd)/${1##*/}"
17 | else
18 | echo "$(pwd)/$1"
19 | fi
20 | fi
21 | }
22 |
23 | if [ "$#" -ne 1 ]; then
24 | echo "Usage: $0 LIDAR_SCAN_NUMBER" >&2
25 | exit 1
26 | fi
27 |
28 | roscore &
29 | ROSCORE_PID=$!
30 | sleep 1
31 |
32 | rviz -d ../rviz_cfg/aloam_velodyne.rviz &
33 | RVIZ_PID=$!
34 |
35 | A_LOAM_DIR=$(abspath "..")
36 |
37 | if [ "$1" -eq 16 ]; then
38 | docker run \
39 | -it \
40 | --rm \
41 | --net=host \
42 | -v ${A_LOAM_DIR}:/root/catkin_ws/src/A-LOAM/ \
43 | ros:aloam \
44 | /bin/bash -c \
45 | "cd /root/catkin_ws/; \
46 | catkin config \
47 | --cmake-args \
48 | -DCMAKE_BUILD_TYPE=Release; \
49 | catkin build; \
50 | source devel/setup.bash; \
51 | roslaunch aloam_velodyne aloam_velodyne_VLP_16.launch rviz:=false"
52 | elif [ "$1" -eq "32" ]; then
53 | docker run \
54 | -it \
55 | --rm \
56 | --net=host \
57 | -v ${A_LOAM_DIR}:/root/catkin_ws/src/A-LOAM/ \
58 | ros:aloam \
59 | /bin/bash -c \
60 | "cd /root/catkin_ws/; \
61 | catkin config \
62 | --cmake-args \
63 | -DCMAKE_BUILD_TYPE=Release; \
64 | catkin build; \
65 | source devel/setup.bash; \
66 | roslaunch aloam_velodyne aloam_velodyne_HDL_32.launch rviz:=false"
67 | elif [ "$1" -eq "64" ]; then
68 | docker run \
69 | -it \
70 | --rm \
71 | --net=host \
72 | -v ${A_LOAM_DIR}:/root/catkin_ws/src/A-LOAM/ \
73 | ros:aloam \
74 | /bin/bash -c \
75 | "cd /root/catkin_ws/; \
76 | catkin config \
77 | --cmake-args \
78 | -DCMAKE_BUILD_TYPE=Release; \
79 | catkin build; \
80 | source devel/setup.bash; \
81 | roslaunch aloam_velodyne aloam_velodyne_HDL_64.launch rviz:=false"
82 | fi
83 |
84 | wait $ROSCORE_PID
85 | wait $RVIZ_PID
86 |
87 | if [[ $? -gt 128 ]]
88 | then
89 | kill $ROSCORE_PID
90 | kill $RVIZ_PID
91 | fi
--------------------------------------------------------------------------------
/include/aloam_velodyne/common.h:
--------------------------------------------------------------------------------
1 | // This is an advanced implementation of the algorithm described in the following paper:
2 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
3 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
4 |
5 | // Modifier: Tong Qin qintonguav@gmail.com
6 | // Shaozu Cao saozu.cao@connect.ust.hk
7 |
8 |
9 | // Copyright 2013, Ji Zhang, Carnegie Mellon University
10 | // Further contributions copyright (c) 2016, Southwest Research Institute
11 | // All rights reserved.
12 | //
13 | // Redistribution and use in source and binary forms, with or without
14 | // modification, are permitted provided that the following conditions are met:
15 | //
16 | // 1. Redistributions of source code must retain the above copyright notice,
17 | // this list of conditions and the following disclaimer.
18 | // 2. Redistributions in binary form must reproduce the above copyright notice,
19 | // this list of conditions and the following disclaimer in the documentation
20 | // and/or other materials provided with the distribution.
21 | // 3. Neither the name of the copyright holder nor the names of its
22 | // contributors may be used to endorse or promote products derived from this
23 | // software without specific prior written permission.
24 | //
25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
26 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
27 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
28 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
29 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
30 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
31 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
32 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
33 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
34 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 | // POSSIBILITY OF SUCH DAMAGE.
36 |
37 | #pragma once
38 |
39 | #include
40 |
41 | #include
42 |
43 | typedef pcl::PointXYZI PointType;
44 |
45 | inline double rad2deg(double radians)
46 | {
47 | return radians * 180.0 / M_PI;
48 | }
49 |
50 | inline double deg2rad(double degrees)
51 | {
52 | return degrees * M_PI / 180.0;
53 | }
54 |
55 | struct Pose6D {
56 | double x;
57 | double y;
58 | double z;
59 | double roll;
60 | double pitch;
61 | double yaw;
62 | };
--------------------------------------------------------------------------------
/include/aloam_velodyne/tic_toc.h:
--------------------------------------------------------------------------------
1 | // Author: Tong Qin qintonguav@gmail.com
2 | // Shaozu Cao saozu.cao@connect.ust.hk
3 |
4 | #pragma once
5 |
6 | #include
7 | #include
8 | #include
9 |
10 | class TicToc
11 | {
12 | public:
13 | TicToc()
14 | {
15 | tic();
16 | }
17 |
18 | void tic()
19 | {
20 | start = std::chrono::system_clock::now();
21 | }
22 |
23 | double toc()
24 | {
25 | end = std::chrono::system_clock::now();
26 | std::chrono::duration elapsed_seconds = end - start;
27 | return elapsed_seconds.count() * 1000;
28 | }
29 |
30 | private:
31 | std::chrono::time_point start, end;
32 | };
33 |
34 | class TicTocV2
35 | {
36 | public:
37 | TicTocV2()
38 | {
39 | tic();
40 | }
41 |
42 | TicTocV2( bool _disp )
43 | {
44 | disp_ = _disp;
45 | tic();
46 | }
47 |
48 | void tic()
49 | {
50 | start = std::chrono::system_clock::now();
51 | }
52 |
53 | void toc( std::string _about_task )
54 | {
55 | end = std::chrono::system_clock::now();
56 | std::chrono::duration elapsed_seconds = end - start;
57 | double elapsed_ms = elapsed_seconds.count() * 1000;
58 |
59 | if( disp_ )
60 | {
61 | std::cout.precision(3); // 10 for sec, 3 for ms
62 | std::cout << _about_task << ": " << elapsed_ms << " msec." << std::endl;
63 | }
64 | }
65 |
66 | private:
67 | std::chrono::time_point start, end;
68 | bool disp_ = false;
69 | };
--------------------------------------------------------------------------------
/include/scancontext/KDTreeVectorOfVectorsAdaptor.h:
--------------------------------------------------------------------------------
1 | /***********************************************************************
2 | * Software License Agreement (BSD License)
3 | *
4 | * Copyright 2011-16 Jose Luis Blanco (joseluisblancoc@gmail.com).
5 | * All rights reserved.
6 | *
7 | * Redistribution and use in source and binary forms, with or without
8 | * modification, are permitted provided that the following conditions
9 | * are met:
10 | *
11 | * 1. Redistributions of source code must retain the above copyright
12 | * notice, this list of conditions and the following disclaimer.
13 | * 2. Redistributions in binary form must reproduce the above copyright
14 | * notice, this list of conditions and the following disclaimer in the
15 | * documentation and/or other materials provided with the distribution.
16 | *
17 | * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
18 | * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
19 | * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
20 | * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
21 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
22 | * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
26 | * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 | *************************************************************************/
28 |
29 | #pragma once
30 |
31 | #include "scancontext/nanoflann.hpp"
32 |
33 | #include
34 |
35 | // ===== This example shows how to use nanoflann with these types of containers: =======
36 | //typedef std::vector > my_vector_of_vectors_t;
37 | //typedef std::vector my_vector_of_vectors_t; // This requires #include
38 | // =====================================================================================
39 |
40 |
41 | /** A simple vector-of-vectors adaptor for nanoflann, without duplicating the storage.
42 | * The i'th vector represents a point in the state space.
43 | *
44 | * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations.
45 | * \tparam num_t The type of the point coordinates (typically, double or float).
46 | * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc.
47 | * \tparam IndexType The type for indices in the KD-tree index (typically, size_t of int)
48 | */
49 | template
50 | struct KDTreeVectorOfVectorsAdaptor
51 | {
52 | typedef KDTreeVectorOfVectorsAdaptor self_t;
53 | typedef typename Distance::template traits::distance_t metric_t;
54 | typedef nanoflann::KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType> index_t;
55 |
56 | index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index.
57 |
58 | /// Constructor: takes a const ref to the vector of vectors object with the data points
59 | KDTreeVectorOfVectorsAdaptor(const size_t /* dimensionality */, const VectorOfVectorsType &mat, const int leaf_max_size = 10) : m_data(mat)
60 | {
61 | assert(mat.size() != 0 && mat[0].size() != 0);
62 | const size_t dims = mat[0].size();
63 | if (DIM>0 && static_cast(dims) != DIM)
64 | throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument");
65 | index = new index_t( static_cast(dims), *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size ) );
66 | index->buildIndex();
67 | }
68 |
69 | ~KDTreeVectorOfVectorsAdaptor() {
70 | delete index;
71 | }
72 |
73 | const VectorOfVectorsType &m_data;
74 |
75 | /** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]).
76 | * Note that this is a short-cut method for index->findNeighbors().
77 | * The user can also call index->... methods as desired.
78 | * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface.
79 | */
80 | inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq, const int nChecks_IGNORED = 10) const
81 | {
82 | nanoflann::KNNResultSet resultSet(num_closest);
83 | resultSet.init(out_indices, out_distances_sq);
84 | index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
85 | }
86 |
87 | /** @name Interface expected by KDTreeSingleIndexAdaptor
88 | * @{ */
89 |
90 | const self_t & derived() const {
91 | return *this;
92 | }
93 | self_t & derived() {
94 | return *this;
95 | }
96 |
97 | // Must return the number of data points
98 | inline size_t kdtree_get_point_count() const {
99 | return m_data.size();
100 | }
101 |
102 | // Returns the dim'th component of the idx'th point in the class:
103 | inline num_t kdtree_get_pt(const size_t idx, const size_t dim) const {
104 | return m_data[idx][dim];
105 | }
106 |
107 | // Optional bounding-box computation: return false to default to a standard bbox computation loop.
108 | // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
109 | // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
110 | template
111 | bool kdtree_get_bbox(BBOX & /*bb*/) const {
112 | return false;
113 | }
114 |
115 | /** @} */
116 |
117 | }; // end of KDTreeVectorOfVectorsAdaptor
118 |
--------------------------------------------------------------------------------
/include/scancontext/Scancontext.cpp:
--------------------------------------------------------------------------------
1 | #include "scancontext/Scancontext.h"
2 |
3 | // namespace SC2
4 | // {
5 |
6 | void coreImportTest (void)
7 | {
8 | cout << "scancontext lib is successfully imported." << endl;
9 | } // coreImportTest
10 |
11 |
12 | float rad2deg(float radians)
13 | {
14 | return radians * 180.0 / M_PI;
15 | }
16 |
17 | float deg2rad(float degrees)
18 | {
19 | return degrees * M_PI / 180.0;
20 | }
21 |
22 |
23 | float xy2theta( const float & _x, const float & _y )
24 | {
25 | if ( (_x >= 0) & (_y >= 0))
26 | return (180/M_PI) * atan(_y / _x);
27 |
28 | if ( (_x < 0) & (_y >= 0))
29 | return 180 - ( (180/M_PI) * atan(_y / (-_x)) );
30 |
31 | if ( (_x < 0) & (_y < 0))
32 | return 180 + ( (180/M_PI) * atan(_y / _x) );
33 |
34 | if ( (_x >= 0) & (_y < 0))
35 | return 360 - ( (180/M_PI) * atan((-_y) / _x) );
36 | } // xy2theta
37 |
38 |
39 | MatrixXd circshift( MatrixXd &_mat, int _num_shift )
40 | {
41 | // shift columns to right direction
42 | assert(_num_shift >= 0);
43 |
44 | if( _num_shift == 0 )
45 | {
46 | MatrixXd shifted_mat( _mat );
47 | return shifted_mat; // Early return
48 | }
49 |
50 | MatrixXd shifted_mat = MatrixXd::Zero( _mat.rows(), _mat.cols() );
51 | for ( int col_idx = 0; col_idx < _mat.cols(); col_idx++ )
52 | {
53 | int new_location = (col_idx + _num_shift) % _mat.cols();
54 | shifted_mat.col(new_location) = _mat.col(col_idx);
55 | }
56 |
57 | return shifted_mat;
58 |
59 | } // circshift
60 |
61 |
62 | std::vector eig2stdvec( MatrixXd _eigmat )
63 | {
64 | std::vector vec( _eigmat.data(), _eigmat.data() + _eigmat.size() );
65 | return vec;
66 | } // eig2stdvec
67 |
68 |
69 | double SCManager::distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 )
70 | {
71 | int num_eff_cols = 0; // i.e., to exclude all-nonzero sector
72 | double sum_sector_similarity = 0;
73 | for ( int col_idx = 0; col_idx < _sc1.cols(); col_idx++ )
74 | {
75 | VectorXd col_sc1 = _sc1.col(col_idx);
76 | VectorXd col_sc2 = _sc2.col(col_idx);
77 |
78 | if( (col_sc1.norm() == 0) | (col_sc2.norm() == 0) )
79 | continue; // don't count this sector pair.
80 |
81 | double sector_similarity = col_sc1.dot(col_sc2) / (col_sc1.norm() * col_sc2.norm());
82 |
83 | sum_sector_similarity = sum_sector_similarity + sector_similarity;
84 | num_eff_cols = num_eff_cols + 1;
85 | }
86 |
87 | double sc_sim = sum_sector_similarity / num_eff_cols;
88 | return 1.0 - sc_sim;
89 |
90 | } // distDirectSC
91 |
92 |
93 | int SCManager::fastAlignUsingVkey( MatrixXd & _vkey1, MatrixXd & _vkey2)
94 | {
95 | int argmin_vkey_shift = 0;
96 | double min_veky_diff_norm = 10000000;
97 | for ( int shift_idx = 0; shift_idx < _vkey1.cols(); shift_idx++ )
98 | {
99 | MatrixXd vkey2_shifted = circshift(_vkey2, shift_idx);
100 |
101 | MatrixXd vkey_diff = _vkey1 - vkey2_shifted;
102 |
103 | double cur_diff_norm = vkey_diff.norm();
104 | if( cur_diff_norm < min_veky_diff_norm )
105 | {
106 | argmin_vkey_shift = shift_idx;
107 | min_veky_diff_norm = cur_diff_norm;
108 | }
109 | }
110 |
111 | return argmin_vkey_shift;
112 |
113 | } // fastAlignUsingVkey
114 |
115 |
116 | std::pair SCManager::distanceBtnScanContext( MatrixXd &_sc1, MatrixXd &_sc2 )
117 | {
118 | // 1. fast align using variant key (not in original IROS18)
119 | MatrixXd vkey_sc1 = makeSectorkeyFromScancontext( _sc1 );
120 | MatrixXd vkey_sc2 = makeSectorkeyFromScancontext( _sc2 );
121 | int argmin_vkey_shift = fastAlignUsingVkey( vkey_sc1, vkey_sc2 );
122 |
123 | const int SEARCH_RADIUS = round( 0.5 * SEARCH_RATIO * _sc1.cols() ); // a half of search range
124 | std::vector shift_idx_search_space { argmin_vkey_shift };
125 | for ( int ii = 1; ii < SEARCH_RADIUS + 1; ii++ )
126 | {
127 | shift_idx_search_space.push_back( (argmin_vkey_shift + ii + _sc1.cols()) % _sc1.cols() );
128 | shift_idx_search_space.push_back( (argmin_vkey_shift - ii + _sc1.cols()) % _sc1.cols() );
129 | }
130 | std::sort(shift_idx_search_space.begin(), shift_idx_search_space.end());
131 |
132 | // 2. fast columnwise diff
133 | int argmin_shift = 0;
134 | double min_sc_dist = 10000000;
135 | for ( int num_shift: shift_idx_search_space )
136 | {
137 | MatrixXd sc2_shifted = circshift(_sc2, num_shift);
138 | double cur_sc_dist = distDirectSC( _sc1, sc2_shifted );
139 | if( cur_sc_dist < min_sc_dist )
140 | {
141 | argmin_shift = num_shift;
142 | min_sc_dist = cur_sc_dist;
143 | }
144 | }
145 |
146 | return make_pair(min_sc_dist, argmin_shift);
147 |
148 | } // distanceBtnScanContext
149 |
150 |
151 | MatrixXd SCManager::makeScancontext( pcl::PointCloud & _scan_down )
152 | {
153 | TicTocV2 t_making_desc;
154 |
155 | int num_pts_scan_down = _scan_down.points.size();
156 |
157 | // main
158 | const int NO_POINT = -1000;
159 | MatrixXd desc = NO_POINT * MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR);
160 |
161 | SCPointType pt;
162 | float azim_angle, azim_range; // wihtin 2d plane
163 | int ring_idx, sctor_idx;
164 | for (int pt_idx = 0; pt_idx < num_pts_scan_down; pt_idx++)
165 | {
166 | pt.x = _scan_down.points[pt_idx].x;
167 | pt.y = _scan_down.points[pt_idx].y;
168 | pt.z = _scan_down.points[pt_idx].z + LIDAR_HEIGHT; // naive adding is ok (all points should be > 0).
169 |
170 | // xyz to ring, sector
171 | azim_range = sqrt(pt.x * pt.x + pt.y * pt.y);
172 | azim_angle = xy2theta(pt.x, pt.y);
173 |
174 | // if range is out of roi, pass
175 | if( azim_range > PC_MAX_RADIUS )
176 | continue;
177 |
178 | ring_idx = std::max( std::min( PC_NUM_RING, int(ceil( (azim_range / PC_MAX_RADIUS) * PC_NUM_RING )) ), 1 );
179 | sctor_idx = std::max( std::min( PC_NUM_SECTOR, int(ceil( (azim_angle / 360.0) * PC_NUM_SECTOR )) ), 1 );
180 |
181 | // taking maximum z
182 | if ( desc(ring_idx-1, sctor_idx-1) < pt.z ) // -1 means cpp starts from 0
183 | desc(ring_idx-1, sctor_idx-1) = pt.z; // update for taking maximum value at that bin
184 | }
185 |
186 | // reset no points to zero (for cosine dist later)
187 | for ( int row_idx = 0; row_idx < desc.rows(); row_idx++ )
188 | for ( int col_idx = 0; col_idx < desc.cols(); col_idx++ )
189 | if( desc(row_idx, col_idx) == NO_POINT )
190 | desc(row_idx, col_idx) = 0;
191 |
192 | t_making_desc.toc("PolarContext making");
193 |
194 | return desc;
195 | } // SCManager::makeScancontext
196 |
197 |
198 | MatrixXd SCManager::makeRingkeyFromScancontext( Eigen::MatrixXd &_desc )
199 | {
200 | /*
201 | * summary: rowwise mean vector
202 | */
203 | Eigen::MatrixXd invariant_key(_desc.rows(), 1);
204 | for ( int row_idx = 0; row_idx < _desc.rows(); row_idx++ )
205 | {
206 | Eigen::MatrixXd curr_row = _desc.row(row_idx);
207 | invariant_key(row_idx, 0) = curr_row.mean();
208 | }
209 |
210 | return invariant_key;
211 | } // SCManager::makeRingkeyFromScancontext
212 |
213 |
214 | MatrixXd SCManager::makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc )
215 | {
216 | /*
217 | * summary: columnwise mean vector
218 | */
219 | Eigen::MatrixXd variant_key(1, _desc.cols());
220 | for ( int col_idx = 0; col_idx < _desc.cols(); col_idx++ )
221 | {
222 | Eigen::MatrixXd curr_col = _desc.col(col_idx);
223 | variant_key(0, col_idx) = curr_col.mean();
224 | }
225 |
226 | return variant_key;
227 | } // SCManager::makeSectorkeyFromScancontext
228 |
229 |
230 | const Eigen::MatrixXd& SCManager::getConstRefRecentSCD(void)
231 | {
232 | return polarcontexts_.back();
233 | }
234 |
235 |
236 | void SCManager::saveScancontextAndKeys( Eigen::MatrixXd _scd )
237 | {
238 | Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( _scd );
239 | Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( _scd );
240 | std::vector polarcontext_invkey_vec = eig2stdvec( ringkey );
241 |
242 | polarcontexts_.push_back( _scd );
243 | polarcontext_invkeys_.push_back( ringkey );
244 | polarcontext_vkeys_.push_back( sectorkey );
245 | polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec );
246 | } // SCManager::makeAndSaveScancontextAndKeys
247 |
248 |
249 | void SCManager::makeAndSaveScancontextAndKeys( pcl::PointCloud & _scan_down )
250 | {
251 | Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1
252 | Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( sc );
253 | Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( sc );
254 | std::vector polarcontext_invkey_vec = eig2stdvec( ringkey );
255 |
256 | polarcontexts_.push_back( sc );
257 | polarcontext_invkeys_.push_back( ringkey );
258 | polarcontext_vkeys_.push_back( sectorkey );
259 | polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec );
260 | } // SCManager::makeAndSaveScancontextAndKeys
261 |
262 | void SCManager::setSCdistThres(double _new_thres)
263 | {
264 | SC_DIST_THRES = _new_thres;
265 | } // SCManager::setThres
266 |
267 | void SCManager::setMaximumRadius(double _max_r)
268 | {
269 | PC_MAX_RADIUS = _max_r;
270 | } // SCManager::setMaximumRadius
271 |
272 | std::pair SCManager::detectLoopClosureIDBetweenSession (std::vector& _curr_key, Eigen::MatrixXd& _curr_desc)
273 | {
274 | int loop_id { -1 }; // init with -1, -1 means no loop (== LeGO-LOAM's variable "closestHistoryFrameID")
275 |
276 | auto& curr_key = _curr_key;
277 | auto& curr_desc = _curr_desc; // current observation (query)
278 |
279 | // step 0: if first, construct the tree in batch
280 | if( ! is_tree_batch_made ) // run only once
281 | {
282 | polarcontext_invkeys_to_search_.clear();
283 | polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() ) ;
284 |
285 | polarcontext_tree_batch_.reset();
286 | polarcontext_tree_batch_ = std::make_unique(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ );
287 |
288 | is_tree_batch_made = true; // for running this block only once
289 | }
290 |
291 | double min_dist = 10000000; // init with somthing large
292 | int nn_align = 0;
293 | int nn_idx = 0;
294 |
295 | // step 1: knn search
296 | std::vector candidate_indexes( NUM_CANDIDATES_FROM_TREE );
297 | std::vector out_dists_sqr( NUM_CANDIDATES_FROM_TREE );
298 |
299 | nanoflann::KNNResultSet knnsearch_result( NUM_CANDIDATES_FROM_TREE );
300 | knnsearch_result.init( &candidate_indexes[0], &out_dists_sqr[0] );
301 | polarcontext_tree_batch_->index->findNeighbors( knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10) ); // error here
302 |
303 | // step 2: pairwise distance (find optimal columnwise best-fit using cosine distance)
304 | TicTocV2 t_calc_dist;
305 | for ( int candidate_iter_idx = 0; candidate_iter_idx < NUM_CANDIDATES_FROM_TREE; candidate_iter_idx++ )
306 | {
307 | MatrixXd polarcontext_candidate = polarcontexts_[ candidate_indexes[candidate_iter_idx] ];
308 | std::pair sc_dist_result = distanceBtnScanContext( curr_desc, polarcontext_candidate );
309 |
310 | double candidate_dist = sc_dist_result.first;
311 | int candidate_align = sc_dist_result.second;
312 |
313 | if( candidate_dist < min_dist )
314 | {
315 | min_dist = candidate_dist;
316 | nn_align = candidate_align;
317 |
318 | nn_idx = candidate_indexes[candidate_iter_idx];
319 | }
320 | }
321 | t_calc_dist.toc("Distance calc");
322 |
323 | // step 3: similarity threshold
324 | if( min_dist < SC_DIST_THRES )
325 | loop_id = nn_idx;
326 |
327 | // To do: return also nn_align (i.e., yaw diff)
328 | float yaw_diff_rad = deg2rad(nn_align * PC_UNIT_SECTORANGLE);
329 | std::pair result {loop_id, yaw_diff_rad};
330 |
331 | return result;
332 |
333 | } // SCManager::detectLoopClosureIDBetweenSession
334 |
335 |
336 | std::pair SCManager::detectLoopClosureID ( void )
337 | {
338 | int loop_id { -1 }; // init with -1, -1 means no loop (== LeGO-LOAM's variable "closestHistoryFrameID")
339 |
340 | auto curr_key = polarcontext_invkeys_mat_.back(); // current observation (query)
341 | auto curr_desc = polarcontexts_.back(); // current observation (query)
342 |
343 | /*
344 | * step 1: candidates from ringkey tree_
345 | */
346 | if( (int)polarcontext_invkeys_mat_.size() < NUM_EXCLUDE_RECENT + 1)
347 | {
348 | std::pair result {loop_id, 0.0};
349 | return result; // Early return
350 | }
351 |
352 | // tree_ reconstruction (not mandatory to make everytime)
353 | if( tree_making_period_conter % TREE_MAKING_PERIOD_ == 0) // to save computation cost
354 | {
355 | TicTocV2 t_tree_construction;
356 |
357 | polarcontext_invkeys_to_search_.clear();
358 | polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() - NUM_EXCLUDE_RECENT ) ;
359 |
360 | polarcontext_tree_.reset();
361 | polarcontext_tree_ = std::make_unique(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ );
362 | // tree_ptr_->index->buildIndex(); // inernally called in the constructor of InvKeyTree (for detail, refer the nanoflann and KDtreeVectorOfVectorsAdaptor)
363 | t_tree_construction.toc("Tree construction");
364 | }
365 | tree_making_period_conter = tree_making_period_conter + 1;
366 |
367 | double min_dist = 10000000; // init with somthing large
368 | int nn_align = 0;
369 | int nn_idx = 0;
370 |
371 | // knn search
372 | std::vector candidate_indexes( NUM_CANDIDATES_FROM_TREE );
373 | std::vector out_dists_sqr( NUM_CANDIDATES_FROM_TREE );
374 |
375 | TicTocV2 t_tree_search;
376 | nanoflann::KNNResultSet knnsearch_result( NUM_CANDIDATES_FROM_TREE );
377 | knnsearch_result.init( &candidate_indexes[0], &out_dists_sqr[0] );
378 | polarcontext_tree_->index->findNeighbors( knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10) );
379 | t_tree_search.toc("Tree search");
380 |
381 | /*
382 | * step 2: pairwise distance (find optimal columnwise best-fit using cosine distance)
383 | */
384 | TicTocV2 t_calc_dist;
385 | for ( int candidate_iter_idx = 0; candidate_iter_idx < NUM_CANDIDATES_FROM_TREE; candidate_iter_idx++ )
386 | {
387 | MatrixXd polarcontext_candidate = polarcontexts_[ candidate_indexes[candidate_iter_idx] ];
388 | std::pair sc_dist_result = distanceBtnScanContext( curr_desc, polarcontext_candidate );
389 |
390 | double candidate_dist = sc_dist_result.first;
391 | int candidate_align = sc_dist_result.second;
392 |
393 | if( candidate_dist < min_dist )
394 | {
395 | min_dist = candidate_dist;
396 | nn_align = candidate_align;
397 |
398 | nn_idx = candidate_indexes[candidate_iter_idx];
399 | }
400 | }
401 | t_calc_dist.toc("Distance calc");
402 |
403 | /*
404 | * loop threshold check
405 | */
406 | if( min_dist < SC_DIST_THRES )
407 | {
408 | loop_id = nn_idx;
409 |
410 | // std::cout.precision(3);
411 | cout << "[Loop found] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl;
412 | // cout << "[Loop found] yaw diff: " << nn_align * PC_UNIT_SECTORANGLE << " deg." << endl;
413 | }
414 | else
415 | {
416 | std::cout.precision(3);
417 | cout << "[Not loop] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl;
418 | // cout << "[Not loop] yaw diff: " << nn_align * PC_UNIT_SECTORANGLE << " deg." << endl;
419 | }
420 |
421 | // To do: return also nn_align (i.e., yaw diff)
422 | float yaw_diff_rad = deg2rad(nn_align * PC_UNIT_SECTORANGLE);
423 | std::pair result {loop_id, yaw_diff_rad};
424 |
425 | return result;
426 |
427 | } // SCManager::detectLoopClosureID
428 |
429 | // } // namespace SC2
--------------------------------------------------------------------------------
/include/scancontext/Scancontext.h:
--------------------------------------------------------------------------------
1 | #pragma once
2 |
3 | #include
4 | #include
5 | #include
6 | #include
7 | #include
8 | #include
9 | #include
10 | #include
11 | #include
12 |
13 | #include
14 |
15 | #include
16 | #include
17 | #include
18 | #include
19 |
20 | #include
21 | #include
22 | #include
23 | #include
24 |
25 | #include "scancontext/nanoflann.hpp"
26 | #include "scancontext/KDTreeVectorOfVectorsAdaptor.h"
27 | #include "aloam_velodyne/tic_toc.h"
28 |
29 | using namespace Eigen;
30 | using namespace nanoflann;
31 |
32 | using std::cout;
33 | using std::endl;
34 | using std::make_pair;
35 |
36 | using std::atan2;
37 | using std::cos;
38 | using std::sin;
39 |
40 | using SCPointType = pcl::PointXYZI; // using xyz only. but a user can exchange the original bin encoding function (i.e., max hegiht) to max intensity (for detail, refer 20 ICRA Intensity Scan Context)
41 | using KeyMat = std::vector >;
42 | using InvKeyTree = KDTreeVectorOfVectorsAdaptor< KeyMat, float >;
43 |
44 |
45 | // namespace SC2
46 | // {
47 |
48 | void coreImportTest ( void );
49 |
50 |
51 | // sc param-independent helper functions
52 | float xy2theta( const float & _x, const float & _y );
53 | MatrixXd circshift( MatrixXd &_mat, int _num_shift );
54 | std::vector eig2stdvec( MatrixXd _eigmat );
55 |
56 |
57 | class SCManager
58 | {
59 | public:
60 | SCManager( ) = default; // reserving data space (of std::vector) could be considered. but the descriptor is lightweight so don't care.
61 |
62 | Eigen::MatrixXd makeScancontext( pcl::PointCloud & _scan_down );
63 | Eigen::MatrixXd makeRingkeyFromScancontext( Eigen::MatrixXd &_desc );
64 | Eigen::MatrixXd makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc );
65 |
66 | int fastAlignUsingVkey ( MatrixXd & _vkey1, MatrixXd & _vkey2 );
67 | double distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "d" (eq 5) in the original paper (IROS 18)
68 | std::pair distanceBtnScanContext ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "D" (eq 6) in the original paper (IROS 18)
69 |
70 | // User-side API
71 | void makeAndSaveScancontextAndKeys( pcl::PointCloud & _scan_down );
72 | std::pair detectLoopClosureID( void ); // int: nearest node index, float: relative yaw
73 |
74 | // for ltslam
75 | // User-side API for multi-session
76 | void saveScancontextAndKeys( Eigen::MatrixXd _scd );
77 | std::pair detectLoopClosureIDBetweenSession ( std::vector& curr_key, Eigen::MatrixXd& curr_desc);
78 |
79 | const Eigen::MatrixXd& getConstRefRecentSCD(void);
80 |
81 | public:
82 | // hyper parameters ()
83 | const double LIDAR_HEIGHT = 2.0; // lidar height : add this for simply directly using lidar scan in the lidar local coord (not robot base coord) / if you use robot-coord-transformed lidar scans, just set this as 0.
84 |
85 | const int PC_NUM_RING = 20; // 20 in the original paper (IROS 18)
86 | const int PC_NUM_SECTOR = 60; // 60 in the original paper (IROS 18)
87 | double PC_MAX_RADIUS = 80.0; // 80 meter max in the original paper (IROS 18)
88 | const double PC_UNIT_SECTORANGLE = 360.0 / double(PC_NUM_SECTOR);
89 | const double PC_UNIT_RINGGAP = PC_MAX_RADIUS / double(PC_NUM_RING);
90 |
91 | // tree
92 | const int NUM_EXCLUDE_RECENT = 30; // simply just keyframe gap (related with loopClosureFrequency in yaml), but node position distance-based exclusion is ok.
93 | const int NUM_CANDIDATES_FROM_TREE = 3; // 10 is enough. (refer the IROS 18 paper)
94 |
95 | // loop thres
96 | const double SEARCH_RATIO = 0.1; // for fast comparison, no Brute-force, but search 10 % is okay. // not was in the original conf paper, but improved ver.
97 | // const double SC_DIST_THRES = 0.13; // empirically 0.1-0.2 is fine (rare false-alarms) for 20x60 polar context (but for 0.15 <, DCS or ICP fit score check (e.g., in LeGO-LOAM) should be required for robustness)
98 |
99 | double SC_DIST_THRES = 0.2; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15
100 | // const double SC_DIST_THRES = 0.7; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold / if not, recommend 0.1-0.15
101 |
102 | // config
103 | const int TREE_MAKING_PERIOD_ = 30; // i.e., remaking tree frequency, to avoid non-mandatory every remaking, to save time cost / in the LeGO-LOAM integration, it is synchronized with the loop detection callback (which is 1Hz) so it means the tree is updated evrey 10 sec. But you can use the smaller value because it is enough fast ~ 5-50ms wrt N.
104 | int tree_making_period_conter = 0;
105 |
106 | // setter
107 | void setSCdistThres(double _new_thres);
108 | void setMaximumRadius(double _max_r);
109 |
110 | // data
111 | std::vector polarcontexts_timestamp_; // optional.
112 | std::vector polarcontexts_;
113 | std::vector polarcontext_invkeys_;
114 | std::vector polarcontext_vkeys_;
115 |
116 | KeyMat polarcontext_invkeys_mat_;
117 | KeyMat polarcontext_invkeys_to_search_;
118 | std::unique_ptr polarcontext_tree_;
119 |
120 | bool is_tree_batch_made = false;
121 | std::unique_ptr polarcontext_tree_batch_;
122 |
123 | }; // SCManager
124 |
125 | // } // namespace SC2
126 |
--------------------------------------------------------------------------------
/launch/aloam_mulran.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
9 |
10 |
11 |
12 |
13 |
14 |
15 |
16 |
17 |
18 |
19 |
20 |
21 |
22 |
23 |
24 |
25 |
26 |
27 |
28 |
29 |
30 |
31 |
32 |
33 |
34 |
35 |
36 |
37 |
38 |
39 |
40 |
41 |
42 |
--------------------------------------------------------------------------------
/launch/aloam_velodyne_HDL_32.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
9 |
10 |
11 |
12 |
13 |
14 |
15 |
16 |
17 |
18 |
19 |
20 |
21 |
22 |
23 |
24 |
25 |
26 |
27 |
28 |
29 |
30 |
31 |
32 |
33 |
34 |
35 |
36 |
37 |
--------------------------------------------------------------------------------
/launch/aloam_velodyne_HDL_64.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
9 |
10 |
11 |
12 |
13 |
14 |
15 |
16 |
17 |
18 |
19 |
20 |
21 |
22 |
23 |
24 |
25 |
26 |
27 |
28 |
29 |
30 |
31 |
32 |
33 |
34 |
35 |
36 |
37 |
38 |
39 |
40 |
--------------------------------------------------------------------------------
/launch/aloam_velodyne_VLP_16.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
9 |
10 |
11 |
12 |
13 |
14 |
15 |
16 |
17 |
18 |
19 |
20 |
21 |
22 |
23 |
24 |
25 |
26 |
27 |
28 |
29 |
30 |
31 |
32 |
33 |
34 |
35 |
36 |
37 |
38 |
39 |
40 |
41 |
42 |
--------------------------------------------------------------------------------
/launch/kitti_helper.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
9 |
10 |
11 |
--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 | aloam_velodyne
4 | 0.0.0
5 |
6 |
7 | This is an advanced implentation of LOAM.
8 | LOAM is described in the following paper:
9 | J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
10 | Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
11 |
12 |
13 | qintong
14 |
15 | BSD
16 |
17 | Ji Zhang
18 |
19 | catkin
20 | geometry_msgs
21 | nav_msgs
22 | roscpp
23 | rospy
24 | std_msgs
25 | rosbag
26 | sensor_msgs
27 | tf
28 | image_transport
29 |
30 | geometry_msgs
31 | nav_msgs
32 | sensor_msgs
33 | roscpp
34 | rospy
35 | std_msgs
36 | rosbag
37 | tf
38 | image_transport
39 |
40 |
41 |
42 |
43 |
--------------------------------------------------------------------------------
/picture/anypipe.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gisbi-kim/SC-A-LOAM/321cb4b07b7815c114237ce5d87014d2e46b972c/picture/anypipe.png
--------------------------------------------------------------------------------
/picture/indoor.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gisbi-kim/SC-A-LOAM/321cb4b07b7815c114237ce5d87014d2e46b972c/picture/indoor.png
--------------------------------------------------------------------------------
/picture/kaist-03-1.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gisbi-kim/SC-A-LOAM/321cb4b07b7815c114237ce5d87014d2e46b972c/picture/kaist-03-1.png
--------------------------------------------------------------------------------
/picture/kaist-03-2.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gisbi-kim/SC-A-LOAM/321cb4b07b7815c114237ce5d87014d2e46b972c/picture/kaist-03-2.png
--------------------------------------------------------------------------------
/picture/kaist-03-3.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gisbi-kim/SC-A-LOAM/321cb4b07b7815c114237ce5d87014d2e46b972c/picture/kaist-03-3.png
--------------------------------------------------------------------------------
/picture/kaist-03-4.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gisbi-kim/SC-A-LOAM/321cb4b07b7815c114237ce5d87014d2e46b972c/picture/kaist-03-4.png
--------------------------------------------------------------------------------
/picture/kaist-03-5.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gisbi-kim/SC-A-LOAM/321cb4b07b7815c114237ce5d87014d2e46b972c/picture/kaist-03-5.png
--------------------------------------------------------------------------------
/picture/kaist-03-merged.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gisbi-kim/SC-A-LOAM/321cb4b07b7815c114237ce5d87014d2e46b972c/picture/kaist-03-merged.png
--------------------------------------------------------------------------------
/picture/kitti.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gisbi-kim/SC-A-LOAM/321cb4b07b7815c114237ce5d87014d2e46b972c/picture/kitti.png
--------------------------------------------------------------------------------
/picture/kitti05.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gisbi-kim/SC-A-LOAM/321cb4b07b7815c114237ce5d87014d2e46b972c/picture/kitti05.png
--------------------------------------------------------------------------------
/picture/kitti_gif.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gisbi-kim/SC-A-LOAM/321cb4b07b7815c114237ce5d87014d2e46b972c/picture/kitti_gif.gif
--------------------------------------------------------------------------------
/picture/riverside01.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gisbi-kim/SC-A-LOAM/321cb4b07b7815c114237ce5d87014d2e46b972c/picture/riverside01.png
--------------------------------------------------------------------------------
/picture/scfastlio.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gisbi-kim/SC-A-LOAM/321cb4b07b7815c114237ce5d87014d2e46b972c/picture/scfastlio.png
--------------------------------------------------------------------------------
/rviz_cfg/aloam_velodyne.rviz:
--------------------------------------------------------------------------------
1 | Panels:
2 | - Class: rviz/Displays
3 | Help Height: 78
4 | Name: Displays
5 | Property Tree Widget:
6 | Expanded:
7 | - /odometry1/odomPath1
8 | - /mapping1
9 | - /pgoOdom1/Shape1
10 | Splitter Ratio: 0.49117645621299744
11 | Tree Height: 746
12 | - Class: rviz/Selection
13 | Name: Selection
14 | - Class: rviz/Tool Properties
15 | Expanded:
16 | - /2D Pose Estimate1
17 | - /2D Nav Goal1
18 | - /Publish Point1
19 | Name: Tool Properties
20 | Splitter Ratio: 0.5886790156364441
21 | - Class: rviz/Views
22 | Expanded:
23 | - /Current View1
24 | Name: Views
25 | Splitter Ratio: 0.5
26 | - Class: rviz/Time
27 | Experimental: false
28 | Name: Time
29 | SyncMode: 0
30 | SyncSource: pgoMap
31 | - Class: rviz/Views
32 | Expanded:
33 | - /Current View1
34 | Name: pgo Views
35 | Splitter Ratio: 0.5
36 | - Class: rviz/Views
37 | Expanded:
38 | - /Current View1
39 | Name: Views
40 | Splitter Ratio: 0.5
41 | Preferences:
42 | PromptSaveOnExit: true
43 | Toolbars:
44 | toolButtonStyle: 2
45 | Visualization Manager:
46 | Class: ""
47 | Displays:
48 | - Alpha: 0.5
49 | Cell Size: 1
50 | Class: rviz/Grid
51 | Color: 160; 160; 164
52 | Enabled: false
53 | Line Style:
54 | Line Width: 0.029999999329447746
55 | Value: Lines
56 | Name: Grid
57 | Normal Cell Count: 0
58 | Offset:
59 | X: 0
60 | Y: 0
61 | Z: 0
62 | Plane: XY
63 | Plane Cell Count: 160
64 | Reference Frame:
65 | Value: false
66 | - Class: rviz/Axes
67 | Enabled: true
68 | Length: 1
69 | Name: Axes
70 | Radius: 0.10000000149011612
71 | Reference Frame: camera_init
72 | Value: true
73 | - Class: rviz/Group
74 | Displays:
75 | - Alpha: 1
76 | Buffer Length: 1
77 | Class: rviz/Path
78 | Color: 255; 170; 0
79 | Enabled: false
80 | Head Diameter: 0.30000001192092896
81 | Head Length: 0.20000000298023224
82 | Length: 0.30000001192092896
83 | Line Style: Lines
84 | Line Width: 0.029999999329447746
85 | Name: gtPathlc
86 | Offset:
87 | X: 0
88 | Y: 0
89 | Z: 0
90 | Pose Color: 255; 85; 255
91 | Pose Style: None
92 | Radius: 0.029999999329447746
93 | Shaft Diameter: 0.10000000149011612
94 | Shaft Length: 0.10000000149011612
95 | Topic: /path_gt
96 | Unreliable: false
97 | Value: false
98 | - Alpha: 1
99 | Buffer Length: 1
100 | Class: rviz/Path
101 | Color: 255; 170; 0
102 | Enabled: true
103 | Head Diameter: 0.30000001192092896
104 | Head Length: 0.20000000298023224
105 | Length: 0.30000001192092896
106 | Line Style: Lines
107 | Line Width: 0.029999999329447746
108 | Name: gtPathLidar
109 | Offset:
110 | X: 0
111 | Y: 0
112 | Z: 0
113 | Pose Color: 255; 85; 255
114 | Pose Style: None
115 | Radius: 0.029999999329447746
116 | Shaft Diameter: 0.10000000149011612
117 | Shaft Length: 0.10000000149011612
118 | Topic: /path_gt_lidar
119 | Unreliable: false
120 | Value: true
121 | Enabled: false
122 | Name: GT
123 | - Class: rviz/Group
124 | Displays:
125 | - Alpha: 1
126 | Buffer Length: 1
127 | Class: rviz/Path
128 | Color: 204; 0; 0
129 | Enabled: true
130 | Head Diameter: 0.30000001192092896
131 | Head Length: 0.20000000298023224
132 | Length: 0.30000001192092896
133 | Line Style: Billboards
134 | Line Width: 0.20000000298023224
135 | Name: odomPath
136 | Offset:
137 | X: 0
138 | Y: 0
139 | Z: 0
140 | Pose Color: 255; 85; 255
141 | Pose Style: None
142 | Radius: 0.029999999329447746
143 | Shaft Diameter: 0.10000000149011612
144 | Shaft Length: 0.10000000149011612
145 | Topic: /laser_odom_path
146 | Unreliable: false
147 | Value: true
148 | - Alpha: 1
149 | Autocompute Intensity Bounds: true
150 | Autocompute Value Bounds:
151 | Max Value: 10
152 | Min Value: -10
153 | Value: true
154 | Axis: Z
155 | Channel Name: intensity
156 | Class: rviz/PointCloud2
157 | Color: 255; 255; 255
158 | Color Transformer: FlatColor
159 | Decay Time: 0
160 | Enabled: false
161 | Invert Rainbow: false
162 | Max Color: 255; 255; 255
163 | Max Intensity: 63.10047912597656
164 | Min Color: 0; 0; 0
165 | Min Intensity: -0.0067862942814826965
166 | Name: PointCloud2
167 | Position Transformer: XYZ
168 | Queue Size: 10
169 | Selectable: true
170 | Size (Pixels): 3
171 | Size (m): 0.05000000074505806
172 | Style: Flat Squares
173 | Topic: /velodyne_cloud_2
174 | Unreliable: false
175 | Use Fixed Frame: true
176 | Use rainbow: true
177 | Value: false
178 | Enabled: true
179 | Name: odometry
180 | - Class: rviz/Group
181 | Displays:
182 | - Alpha: 1
183 | Buffer Length: 1
184 | Class: rviz/Path
185 | Color: 4; 69; 246
186 | Enabled: true
187 | Head Diameter: 0.30000001192092896
188 | Head Length: 0.20000000298023224
189 | Length: 0.30000001192092896
190 | Line Style: Billboards
191 | Line Width: 0.30000001192092896
192 | Name: mappingPath
193 | Offset:
194 | X: 0
195 | Y: 0
196 | Z: 0
197 | Pose Color: 255; 85; 255
198 | Pose Style: None
199 | Radius: 0.029999999329447746
200 | Shaft Diameter: 0.10000000149011612
201 | Shaft Length: 0.10000000149011612
202 | Topic: /aft_mapped_path
203 | Unreliable: false
204 | Value: true
205 | - Alpha: 1
206 | Autocompute Intensity Bounds: true
207 | Autocompute Value Bounds:
208 | Max Value: 10
209 | Min Value: -10
210 | Value: true
211 | Axis: Z
212 | Channel Name: intensity
213 | Class: rviz/PointCloud2
214 | Color: 255; 255; 255
215 | Color Transformer: Intensity
216 | Decay Time: 0
217 | Enabled: false
218 | Invert Rainbow: false
219 | Max Color: 255; 255; 255
220 | Max Intensity: 50.14332962036133
221 | Min Color: 0; 0; 0
222 | Min Intensity: -0.04392780363559723
223 | Name: allMapCloud
224 | Position Transformer: XYZ
225 | Queue Size: 10
226 | Selectable: true
227 | Size (Pixels): 4
228 | Size (m): 0.20000000298023224
229 | Style: Points
230 | Topic: /laser_cloud_map
231 | Unreliable: false
232 | Use Fixed Frame: true
233 | Use rainbow: false
234 | Value: false
235 | - Alpha: 1
236 | Autocompute Intensity Bounds: true
237 | Autocompute Value Bounds:
238 | Max Value: 10
239 | Min Value: -10
240 | Value: true
241 | Axis: Z
242 | Channel Name: intensity
243 | Class: rviz/PointCloud2
244 | Color: 255; 255; 255
245 | Color Transformer: FlatColor
246 | Decay Time: 0
247 | Enabled: false
248 | Invert Rainbow: false
249 | Max Color: 255; 255; 255
250 | Max Intensity: 15
251 | Min Color: 0; 0; 0
252 | Min Intensity: 0
253 | Name: surround
254 | Position Transformer: XYZ
255 | Queue Size: 1
256 | Selectable: true
257 | Size (Pixels): 1
258 | Size (m): 0.05000000074505806
259 | Style: Squares
260 | Topic: /laser_cloud_surround
261 | Unreliable: false
262 | Use Fixed Frame: true
263 | Use rainbow: true
264 | Value: false
265 | - Alpha: 1
266 | Autocompute Intensity Bounds: true
267 | Autocompute Value Bounds:
268 | Max Value: 10
269 | Min Value: -10
270 | Value: true
271 | Axis: Z
272 | Channel Name: intensity
273 | Class: rviz/PointCloud2
274 | Color: 255; 255; 255
275 | Color Transformer: Intensity
276 | Decay Time: 0
277 | Enabled: false
278 | Invert Rainbow: false
279 | Max Color: 255; 255; 255
280 | Max Intensity: 50.141441345214844
281 | Min Color: 0; 0; 0
282 | Min Intensity: -0.024373823776841164
283 | Name: currPoints
284 | Position Transformer: XYZ
285 | Queue Size: 10
286 | Selectable: true
287 | Size (Pixels): 1.5
288 | Size (m): 0.009999999776482582
289 | Style: Points
290 | Topic: /velodyne_cloud_registered
291 | Unreliable: false
292 | Use Fixed Frame: true
293 | Use rainbow: true
294 | Value: false
295 | - Angle Tolerance: 0.10000000149011612
296 | Class: rviz/Odometry
297 | Covariance:
298 | Orientation:
299 | Alpha: 0.5
300 | Color: 255; 255; 127
301 | Color Style: Unique
302 | Frame: Local
303 | Offset: 1
304 | Scale: 1
305 | Value: true
306 | Position:
307 | Alpha: 0.30000001192092896
308 | Color: 204; 51; 204
309 | Scale: 1
310 | Value: true
311 | Value: true
312 | Enabled: false
313 | Keep: 1
314 | Name: Odometry
315 | Position Tolerance: 0.10000000149011612
316 | Shape:
317 | Alpha: 1
318 | Axes Length: 1.5
319 | Axes Radius: 0.5
320 | Color: 255; 25; 0
321 | Head Length: 0.10000000149011612
322 | Head Radius: 2
323 | Shaft Length: 0.10000000149011612
324 | Shaft Radius: 20
325 | Value: Axes
326 | Topic: /aft_mapped_to_init
327 | Unreliable: false
328 | Value: false
329 | - Class: rviz/TF
330 | Enabled: false
331 | Frame Timeout: 15
332 | Frames:
333 | All Enabled: true
334 | Marker Scale: 1
335 | Name: TF
336 | Show Arrows: true
337 | Show Axes: true
338 | Show Names: true
339 | Tree:
340 | {}
341 | Update Interval: 0
342 | Value: false
343 | Enabled: true
344 | Name: mapping
345 | - Alpha: 1
346 | Buffer Length: 1
347 | Class: rviz/Path
348 | Color: 25; 255; 0
349 | Enabled: true
350 | Head Diameter: 0.30000001192092896
351 | Head Length: 0.20000000298023224
352 | Length: 1
353 | Line Style: Billboards
354 | Line Width: 0.4000000059604645
355 | Name: pgoPath
356 | Offset:
357 | X: 0
358 | Y: 0
359 | Z: 0
360 | Pose Color: 255; 85; 255
361 | Pose Style: Axes
362 | Radius: 0.30000001192092896
363 | Shaft Diameter: 0.10000000149011612
364 | Shaft Length: 0.10000000149011612
365 | Topic: /aft_pgo_path
366 | Unreliable: false
367 | Value: true
368 | - Angle Tolerance: 0.10000000149011612
369 | Class: rviz/Odometry
370 | Covariance:
371 | Orientation:
372 | Alpha: 0.5
373 | Color: 255; 255; 127
374 | Color Style: Unique
375 | Frame: Local
376 | Offset: 1
377 | Scale: 1
378 | Value: true
379 | Position:
380 | Alpha: 0.30000001192092896
381 | Color: 204; 51; 204
382 | Scale: 1
383 | Value: true
384 | Value: true
385 | Enabled: false
386 | Keep: 1
387 | Name: pgoOdom
388 | Position Tolerance: 0.10000000149011612
389 | Shape:
390 | Alpha: 1
391 | Axes Length: 5
392 | Axes Radius: 1
393 | Color: 255; 25; 0
394 | Head Length: 0.30000001192092896
395 | Head Radius: 0.10000000149011612
396 | Shaft Length: 1
397 | Shaft Radius: 0.05000000074505806
398 | Value: Axes
399 | Topic: /aft_pgo_odom
400 | Unreliable: false
401 | Value: false
402 | - Alpha: 0.5
403 | Autocompute Intensity Bounds: true
404 | Autocompute Value Bounds:
405 | Max Value: 10
406 | Min Value: -10
407 | Value: true
408 | Axis: Z
409 | Channel Name: intensity
410 | Class: rviz/PointCloud2
411 | Color: 255; 255; 255
412 | Color Transformer: Intensity
413 | Decay Time: 0
414 | Enabled: true
415 | Invert Rainbow: false
416 | Max Color: 255; 255; 255
417 | Max Intensity: 15.099973678588867
418 | Min Color: 0; 0; 0
419 | Min Intensity: 0.00020467743161134422
420 | Name: pgoMap
421 | Position Transformer: XYZ
422 | Queue Size: 1
423 | Selectable: true
424 | Size (Pixels): 3
425 | Size (m): 0.10000000149011612
426 | Style: Points
427 | Topic: /aft_pgo_map
428 | Unreliable: false
429 | Use Fixed Frame: true
430 | Use rainbow: true
431 | Value: true
432 | - Alpha: 1
433 | Autocompute Intensity Bounds: true
434 | Autocompute Value Bounds:
435 | Max Value: 10
436 | Min Value: -10
437 | Value: true
438 | Axis: Z
439 | Channel Name: intensity
440 | Class: rviz/PointCloud2
441 | Color: 255; 255; 255
442 | Color Transformer: FlatColor
443 | Decay Time: 0
444 | Enabled: false
445 | Invert Rainbow: false
446 | Max Color: 255; 255; 255
447 | Max Intensity: 20.06364631652832
448 | Min Color: 0; 0; 0
449 | Min Intensity: 3.003571033477783
450 | Name: loop scan
451 | Position Transformer: XYZ
452 | Queue Size: 10
453 | Selectable: true
454 | Size (Pixels): 3
455 | Size (m): 0.5
456 | Style: Spheres
457 | Topic: /loop_scan_local
458 | Unreliable: false
459 | Use Fixed Frame: true
460 | Use rainbow: true
461 | Value: false
462 | - Alpha: 1
463 | Autocompute Intensity Bounds: true
464 | Autocompute Value Bounds:
465 | Max Value: 32.84956741333008
466 | Min Value: -16.756437301635742
467 | Value: true
468 | Axis: Z
469 | Channel Name: intensity
470 | Class: rviz/PointCloud2
471 | Color: 255; 255; 255
472 | Color Transformer: Intensity
473 | Decay Time: 0
474 | Enabled: false
475 | Invert Rainbow: false
476 | Max Color: 255; 255; 255
477 | Max Intensity: 20.100271224975586
478 | Min Color: 0; 0; 0
479 | Min Intensity: 3.0008726119995117
480 | Name: loop submap
481 | Position Transformer: XYZ
482 | Queue Size: 10
483 | Selectable: true
484 | Size (Pixels): 2
485 | Size (m): 0.20000000298023224
486 | Style: Spheres
487 | Topic: /loop_submap_local
488 | Unreliable: false
489 | Use Fixed Frame: true
490 | Use rainbow: true
491 | Value: false
492 | Enabled: true
493 | Global Options:
494 | Background Color: 0; 0; 0
495 | Default Light: true
496 | Fixed Frame: camera_init
497 | Frame Rate: 30
498 | Name: root
499 | Tools:
500 | - Class: rviz/Interact
501 | Hide Inactive Objects: true
502 | - Class: rviz/MoveCamera
503 | - Class: rviz/Select
504 | - Class: rviz/FocusCamera
505 | - Class: rviz/Measure
506 | - Class: rviz/SetInitialPose
507 | Theta std deviation: 0.2617993950843811
508 | Topic: /initialpose
509 | X std deviation: 0.5
510 | Y std deviation: 0.5
511 | - Class: rviz/SetGoal
512 | Topic: /move_base_simple/goal
513 | - Class: rviz/PublishPoint
514 | Single click: true
515 | Topic: /clicked_point
516 | Value: true
517 | Views:
518 | Current:
519 | Class: rviz/Orbit
520 | Distance: 10
521 | Enable Stereo Rendering:
522 | Stereo Eye Separation: 0.05999999865889549
523 | Stereo Focal Distance: 1
524 | Swap Stereo Eyes: false
525 | Value: false
526 | Focal Point:
527 | X: 0
528 | Y: 0
529 | Z: 0
530 | Focal Shape Fixed Size: false
531 | Focal Shape Size: 0.05000000074505806
532 | Invert Z Axis: false
533 | Name: Current View
534 | Near Clip Distance: 0.009999999776482582
535 | Pitch: 0.7853981852531433
536 | Target Frame: aft_pgo
537 | Value: Orbit (rviz)
538 | Yaw: 0.7853981852531433
539 | Saved: ~
540 | Window Geometry:
541 | Displays:
542 | collapsed: false
543 | Height: 1043
544 | Hide Left Dock: false
545 | Hide Right Dock: false
546 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000375fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002ef000000c30000000000000000fb0000001200700067006f00200056006900650077007300000002db000000d7000000a400ffffff000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fc0000003d00000375000000c10100001cfa000000000100000002fb0000000a005600690065007700730100000000ffffffff0000010000fffffffb0000000a0056006900650077007301000006710000010f0000010000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
547 | Selection:
548 | collapsed: false
549 | Time:
550 | collapsed: false
551 | Tool Properties:
552 | collapsed: false
553 | Views:
554 | collapsed: false
555 | Width: 1920
556 | X: 3840
557 | Y: 0
558 | pgo Views:
559 | collapsed: false
560 |
--------------------------------------------------------------------------------
/src/kittiHelper.cpp:
--------------------------------------------------------------------------------
1 | // Author: Tong Qin qintonguav@gmail.com
2 | // Shaozu Cao saozu.cao@connect.ust.hk
3 |
4 | #include
5 | #include
6 | #include
7 | #include
8 | #include
9 | #include
10 | #include
11 | #include
12 | #include
13 | #include
14 | #include
15 | #include
16 | #include
17 | #include
18 | #include
19 | #include
20 | #include
21 | #include
22 | #include
23 | #include
24 |
25 | std::vector read_lidar_data(const std::string lidar_data_path)
26 | {
27 | std::ifstream lidar_data_file(lidar_data_path, std::ifstream::in | std::ifstream::binary);
28 | lidar_data_file.seekg(0, std::ios::end);
29 | const size_t num_elements = lidar_data_file.tellg() / sizeof(float);
30 | lidar_data_file.seekg(0, std::ios::beg);
31 |
32 | std::vector lidar_data_buffer(num_elements);
33 | lidar_data_file.read(reinterpret_cast(&lidar_data_buffer[0]), num_elements*sizeof(float));
34 | return lidar_data_buffer;
35 | }
36 |
37 | int main(int argc, char** argv)
38 | {
39 | ros::init(argc, argv, "kitti_helper");
40 | ros::NodeHandle n("~");
41 | std::string dataset_folder, sequence_number, output_bag_file;
42 | n.getParam("dataset_folder", dataset_folder);
43 | n.getParam("sequence_number", sequence_number);
44 | std::cout << "Reading sequence " << sequence_number << " from " << dataset_folder << '\n';
45 | bool to_bag;
46 | n.getParam("to_bag", to_bag);
47 | if (to_bag)
48 | n.getParam("output_bag_file", output_bag_file);
49 | int publish_delay;
50 | n.getParam("publish_delay", publish_delay);
51 | publish_delay = publish_delay <= 0 ? 1 : publish_delay;
52 |
53 | ros::Publisher pub_laser_cloud = n.advertise("/velodyne_points", 2);
54 |
55 | image_transport::ImageTransport it(n);
56 | image_transport::Publisher pub_image_left = it.advertise("/image_left", 2);
57 | image_transport::Publisher pub_image_right = it.advertise("/image_right", 2);
58 |
59 | ros::Publisher pubOdomGT = n.advertise ("/odometry_gt", 5);
60 | nav_msgs::Odometry odomGT;
61 | odomGT.header.frame_id = "/camera_init";
62 | odomGT.child_frame_id = "/ground_truth";
63 |
64 | ros::Publisher pubPathGT = n.advertise ("/path_gt", 5);
65 | nav_msgs::Path pathGT;
66 | pathGT.header.frame_id = "/camera_init";
67 |
68 | std::string timestamp_path = "sequences/" + sequence_number + "/times.txt";
69 | std::ifstream timestamp_file(dataset_folder + timestamp_path, std::ifstream::in);
70 |
71 | std::string ground_truth_path = "results/" + sequence_number + ".txt";
72 | std::ifstream ground_truth_file(dataset_folder + ground_truth_path, std::ifstream::in);
73 |
74 | rosbag::Bag bag_out;
75 | if (to_bag)
76 | bag_out.open(output_bag_file, rosbag::bagmode::Write);
77 |
78 | Eigen::Matrix3d R_transform;
79 | R_transform << 0, 0, 1, -1, 0, 0, 0, -1, 0;
80 | Eigen::Quaterniond q_transform(R_transform);
81 |
82 | std::string line;
83 | std::size_t line_num = 0;
84 |
85 | ros::Rate r(10.0 / publish_delay);
86 | while (std::getline(timestamp_file, line) && ros::ok())
87 | {
88 | float timestamp = stof(line);
89 | std::stringstream left_image_path, right_image_path;
90 | left_image_path << dataset_folder << "sequences/" + sequence_number + "/image_0/" << std::setfill('0') << std::setw(6) << line_num << ".png";
91 | cv::Mat left_image = cv::imread(left_image_path.str(), CV_LOAD_IMAGE_GRAYSCALE);
92 | right_image_path << dataset_folder << "sequences/" + sequence_number + "/image_1/" << std::setfill('0') << std::setw(6) << line_num << ".png";
93 | cv::Mat right_image = cv::imread(left_image_path.str(), CV_LOAD_IMAGE_GRAYSCALE);
94 |
95 | std::getline(ground_truth_file, line);
96 | std::stringstream pose_stream(line);
97 | std::string s;
98 | Eigen::Matrix gt_pose;
99 | for (std::size_t i = 0; i < 3; ++i)
100 | {
101 | for (std::size_t j = 0; j < 4; ++j)
102 | {
103 | std::getline(pose_stream, s, ' ');
104 | gt_pose(i, j) = stof(s);
105 | }
106 | }
107 |
108 | Eigen::Quaterniond q_w_i(gt_pose.topLeftCorner<3, 3>());
109 | Eigen::Quaterniond q = q_transform * q_w_i;
110 | q.normalize();
111 | Eigen::Vector3d t = q_transform * gt_pose.topRightCorner<3, 1>();
112 |
113 | odomGT.header.stamp = ros::Time().fromSec(timestamp);
114 | odomGT.pose.pose.orientation.x = q.x();
115 | odomGT.pose.pose.orientation.y = q.y();
116 | odomGT.pose.pose.orientation.z = q.z();
117 | odomGT.pose.pose.orientation.w = q.w();
118 | odomGT.pose.pose.position.x = t(0);
119 | odomGT.pose.pose.position.y = t(1);
120 | odomGT.pose.pose.position.z = t(2);
121 | pubOdomGT.publish(odomGT);
122 |
123 | geometry_msgs::PoseStamped poseGT;
124 | poseGT.header = odomGT.header;
125 | poseGT.pose = odomGT.pose.pose;
126 | pathGT.header.stamp = odomGT.header.stamp;
127 | pathGT.poses.push_back(poseGT);
128 | pubPathGT.publish(pathGT);
129 |
130 | // read lidar point cloud
131 | std::stringstream lidar_data_path;
132 | lidar_data_path << dataset_folder << "velodyne/sequences/" + sequence_number + "/velodyne/"
133 | << std::setfill('0') << std::setw(6) << line_num << ".bin";
134 | std::vector lidar_data = read_lidar_data(lidar_data_path.str());
135 | std::cout << "totally " << lidar_data.size() / 4.0 << " points in this lidar frame \n";
136 |
137 | std::vector lidar_points;
138 | std::vector lidar_intensities;
139 | pcl::PointCloud laser_cloud;
140 | for (std::size_t i = 0; i < lidar_data.size(); i += 4)
141 | {
142 | lidar_points.emplace_back(lidar_data[i], lidar_data[i+1], lidar_data[i+2]);
143 | lidar_intensities.push_back(lidar_data[i+3]);
144 |
145 | pcl::PointXYZI point;
146 | point.x = lidar_data[i];
147 | point.y = lidar_data[i + 1];
148 | point.z = lidar_data[i + 2];
149 | point.intensity = lidar_data[i + 3];
150 | laser_cloud.push_back(point);
151 | }
152 |
153 | sensor_msgs::PointCloud2 laser_cloud_msg;
154 | pcl::toROSMsg(laser_cloud, laser_cloud_msg);
155 | laser_cloud_msg.header.stamp = ros::Time().fromSec(timestamp);
156 | laser_cloud_msg.header.frame_id = "/camera_init";
157 | pub_laser_cloud.publish(laser_cloud_msg);
158 |
159 | sensor_msgs::ImagePtr image_left_msg = cv_bridge::CvImage(laser_cloud_msg.header, "mono8", left_image).toImageMsg();
160 | sensor_msgs::ImagePtr image_right_msg = cv_bridge::CvImage(laser_cloud_msg.header, "mono8", right_image).toImageMsg();
161 | pub_image_left.publish(image_left_msg);
162 | pub_image_right.publish(image_right_msg);
163 |
164 | if (to_bag)
165 | {
166 | bag_out.write("/image_left", ros::Time::now(), image_left_msg);
167 | bag_out.write("/image_right", ros::Time::now(), image_right_msg);
168 | bag_out.write("/velodyne_points", ros::Time::now(), laser_cloud_msg);
169 | bag_out.write("/path_gt", ros::Time::now(), pathGT);
170 | bag_out.write("/odometry_gt", ros::Time::now(), odomGT);
171 | }
172 |
173 | line_num ++;
174 | r.sleep();
175 | }
176 | bag_out.close();
177 | std::cout << "Done \n";
178 |
179 |
180 | return 0;
181 | }
--------------------------------------------------------------------------------
/src/laserMapping.cpp:
--------------------------------------------------------------------------------
1 | // This is an advanced implementation of the algorithm described in the following paper:
2 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
3 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
4 |
5 | // Modifier: Tong Qin qintonguav@gmail.com
6 | // Shaozu Cao saozu.cao@connect.ust.hk
7 |
8 |
9 | // Copyright 2013, Ji Zhang, Carnegie Mellon University
10 | // Further contributions copyright (c) 2016, Southwest Research Institute
11 | // All rights reserved.
12 | //
13 | // Redistribution and use in source and binary forms, with or without
14 | // modification, are permitted provided that the following conditions are met:
15 | //
16 | // 1. Redistributions of source code must retain the above copyright notice,
17 | // this list of conditions and the following disclaimer.
18 | // 2. Redistributions in binary form must reproduce the above copyright notice,
19 | // this list of conditions and the following disclaimer in the documentation
20 | // and/or other materials provided with the distribution.
21 | // 3. Neither the name of the copyright holder nor the names of its
22 | // contributors may be used to endorse or promote products derived from this
23 | // software without specific prior written permission.
24 | //
25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
26 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
27 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
28 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
29 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
30 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
31 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
32 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
33 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
34 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 | // POSSIBILITY OF SUCH DAMAGE.
36 |
37 | #include
38 | #include
39 | #include
40 | #include
41 | #include
42 | #include
43 | #include
44 | #include
45 | #include
46 | #include
47 | #include
48 | #include
49 | #include
50 | #include
51 | #include
52 | #include
53 | #include
54 | #include
55 | #include
56 | #include
57 | #include
58 | #include
59 | #include
60 |
61 | #include "lidarFactor.hpp"
62 | #include "aloam_velodyne/common.h"
63 | #include "aloam_velodyne/tic_toc.h"
64 |
65 |
66 | int frameCount = 0;
67 |
68 | double timeLaserCloudCornerLast = 0;
69 | double timeLaserCloudSurfLast = 0;
70 | double timeLaserCloudFullRes = 0;
71 | double timeLaserOdometry = 0;
72 |
73 |
74 | int laserCloudCenWidth = 10;
75 | int laserCloudCenHeight = 10;
76 | int laserCloudCenDepth = 5;
77 | const int laserCloudWidth = 21;
78 | const int laserCloudHeight = 21;
79 | const int laserCloudDepth = 11;
80 |
81 |
82 | const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth; //4851
83 |
84 |
85 | int laserCloudValidInd[125];
86 | int laserCloudSurroundInd[125];
87 |
88 | // input: from odom
89 | pcl::PointCloud::Ptr laserCloudCornerLast(new pcl::PointCloud());
90 | pcl::PointCloud::Ptr laserCloudSurfLast(new pcl::PointCloud());
91 |
92 | // ouput: all visualble cube points
93 | pcl::PointCloud::Ptr laserCloudSurround(new pcl::PointCloud());
94 |
95 | // surround points in map to build tree
96 | pcl::PointCloud::Ptr laserCloudCornerFromMap(new pcl::PointCloud());
97 | pcl::PointCloud::Ptr laserCloudSurfFromMap(new pcl::PointCloud());
98 |
99 | //input & output: points in one frame. local --> global
100 | pcl::PointCloud::Ptr laserCloudFullRes(new pcl::PointCloud());
101 |
102 | // points in every cube
103 | pcl::PointCloud::Ptr laserCloudCornerArray[laserCloudNum];
104 | pcl::PointCloud::Ptr laserCloudSurfArray[laserCloudNum];
105 |
106 | //kd-tree
107 | pcl::KdTreeFLANN::Ptr kdtreeCornerFromMap(new pcl::KdTreeFLANN());
108 | pcl::KdTreeFLANN::Ptr kdtreeSurfFromMap(new pcl::KdTreeFLANN());
109 |
110 | double parameters[7] = {0, 0, 0, 1, 0, 0, 0};
111 | Eigen::Map q_w_curr(parameters);
112 | Eigen::Map t_w_curr(parameters + 4);
113 |
114 | // wmap_T_odom * odom_T_curr = wmap_T_curr;
115 | // transformation between odom's world and map's world frame
116 | Eigen::Quaterniond q_wmap_wodom(1, 0, 0, 0);
117 | Eigen::Vector3d t_wmap_wodom(0, 0, 0);
118 |
119 | Eigen::Quaterniond q_wodom_curr(1, 0, 0, 0);
120 | Eigen::Vector3d t_wodom_curr(0, 0, 0);
121 |
122 |
123 | std::queue cornerLastBuf;
124 | std::queue surfLastBuf;
125 | std::queue fullResBuf;
126 | std::queue odometryBuf;
127 | std::mutex mBuf;
128 |
129 | pcl::VoxelGrid downSizeFilterCorner;
130 | pcl::VoxelGrid downSizeFilterSurf;
131 |
132 | std::vector pointSearchInd;
133 | std::vector pointSearchSqDis;
134 |
135 | PointType pointOri, pointSel;
136 |
137 | ros::Publisher pubLaserCloudSurround, pubLaserCloudMap, pubLaserCloudFullRes, pubOdomAftMapped, pubOdomAftMappedHighFrec, pubLaserAfterMappedPath;
138 | ros::Publisher pubLaserCloudFullResLocal;
139 |
140 | nav_msgs::Path laserAfterMappedPath;
141 |
142 | // set initial guess
143 | void transformAssociateToMap()
144 | {
145 | q_w_curr = q_wmap_wodom * q_wodom_curr;
146 | t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
147 | }
148 |
149 | void transformUpdate()
150 | {
151 | q_wmap_wodom = q_w_curr * q_wodom_curr.inverse();
152 | t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr;
153 | }
154 |
155 | void pointAssociateToMap(PointType const *const pi, PointType *const po)
156 | {
157 | Eigen::Vector3d point_curr(pi->x, pi->y, pi->z);
158 | Eigen::Vector3d point_w = q_w_curr * point_curr + t_w_curr;
159 | po->x = point_w.x();
160 | po->y = point_w.y();
161 | po->z = point_w.z();
162 | po->intensity = pi->intensity;
163 | //po->intensity = 1.0;
164 | }
165 |
166 | void pointAssociateTobeMapped(PointType const *const pi, PointType *const po)
167 | {
168 | Eigen::Vector3d point_w(pi->x, pi->y, pi->z);
169 | Eigen::Vector3d point_curr = q_w_curr.inverse() * (point_w - t_w_curr);
170 | po->x = point_curr.x();
171 | po->y = point_curr.y();
172 | po->z = point_curr.z();
173 | po->intensity = pi->intensity;
174 | }
175 |
176 | void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudCornerLast2)
177 | {
178 | mBuf.lock();
179 | cornerLastBuf.push(laserCloudCornerLast2);
180 | mBuf.unlock();
181 | }
182 |
183 | void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudSurfLast2)
184 | {
185 | mBuf.lock();
186 | surfLastBuf.push(laserCloudSurfLast2);
187 | mBuf.unlock();
188 | }
189 |
190 | void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2)
191 | {
192 | mBuf.lock();
193 | fullResBuf.push(laserCloudFullRes2);
194 | mBuf.unlock();
195 | }
196 |
197 | //receive odomtry
198 | void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &laserOdometry)
199 | {
200 | mBuf.lock();
201 | odometryBuf.push(laserOdometry);
202 | mBuf.unlock();
203 |
204 | // high frequence publish
205 | Eigen::Quaterniond q_wodom_curr;
206 | Eigen::Vector3d t_wodom_curr;
207 | q_wodom_curr.x() = laserOdometry->pose.pose.orientation.x;
208 | q_wodom_curr.y() = laserOdometry->pose.pose.orientation.y;
209 | q_wodom_curr.z() = laserOdometry->pose.pose.orientation.z;
210 | q_wodom_curr.w() = laserOdometry->pose.pose.orientation.w;
211 | t_wodom_curr.x() = laserOdometry->pose.pose.position.x;
212 | t_wodom_curr.y() = laserOdometry->pose.pose.position.y;
213 | t_wodom_curr.z() = laserOdometry->pose.pose.position.z;
214 |
215 | Eigen::Quaterniond q_w_curr = q_wmap_wodom * q_wodom_curr;
216 | Eigen::Vector3d t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
217 |
218 | nav_msgs::Odometry odomAftMapped;
219 | odomAftMapped.header.frame_id = "/camera_init";
220 | odomAftMapped.child_frame_id = "/aft_mapped";
221 | odomAftMapped.header.stamp = laserOdometry->header.stamp;
222 | odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
223 | odomAftMapped.pose.pose.orientation.y = q_w_curr.y();
224 | odomAftMapped.pose.pose.orientation.z = q_w_curr.z();
225 | odomAftMapped.pose.pose.orientation.w = q_w_curr.w();
226 | odomAftMapped.pose.pose.position.x = t_w_curr.x();
227 | odomAftMapped.pose.pose.position.y = t_w_curr.y();
228 | odomAftMapped.pose.pose.position.z = t_w_curr.z();
229 | pubOdomAftMappedHighFrec.publish(odomAftMapped);
230 | }
231 |
232 | void process()
233 | {
234 | while(1)
235 | {
236 | while (!cornerLastBuf.empty() && !surfLastBuf.empty() &&
237 | !fullResBuf.empty() && !odometryBuf.empty())
238 | {
239 | mBuf.lock();
240 | while (!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
241 | odometryBuf.pop();
242 | if (odometryBuf.empty())
243 | {
244 | mBuf.unlock();
245 | break;
246 | }
247 |
248 | while (!surfLastBuf.empty() && surfLastBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
249 | surfLastBuf.pop();
250 | if (surfLastBuf.empty())
251 | {
252 | mBuf.unlock();
253 | break;
254 | }
255 |
256 | while (!fullResBuf.empty() && fullResBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
257 | fullResBuf.pop();
258 | if (fullResBuf.empty())
259 | {
260 | mBuf.unlock();
261 | break;
262 | }
263 |
264 | timeLaserCloudCornerLast = cornerLastBuf.front()->header.stamp.toSec();
265 | timeLaserCloudSurfLast = surfLastBuf.front()->header.stamp.toSec();
266 | timeLaserCloudFullRes = fullResBuf.front()->header.stamp.toSec();
267 | timeLaserOdometry = odometryBuf.front()->header.stamp.toSec();
268 |
269 | if (timeLaserCloudCornerLast != timeLaserOdometry ||
270 | timeLaserCloudSurfLast != timeLaserOdometry ||
271 | timeLaserCloudFullRes != timeLaserOdometry)
272 | {
273 | //printf("time corner %f surf %f full %f odom %f \n", timeLaserCloudCornerLast, timeLaserCloudSurfLast, timeLaserCloudFullRes, timeLaserOdometry);
274 | //printf("unsync messeage!");
275 | mBuf.unlock();
276 | break;
277 | }
278 |
279 | laserCloudCornerLast->clear();
280 | pcl::fromROSMsg(*cornerLastBuf.front(), *laserCloudCornerLast);
281 | cornerLastBuf.pop();
282 |
283 | laserCloudSurfLast->clear();
284 | pcl::fromROSMsg(*surfLastBuf.front(), *laserCloudSurfLast);
285 | surfLastBuf.pop();
286 |
287 | laserCloudFullRes->clear();
288 | pcl::fromROSMsg(*fullResBuf.front(), *laserCloudFullRes);
289 | fullResBuf.pop();
290 |
291 | q_wodom_curr.x() = odometryBuf.front()->pose.pose.orientation.x;
292 | q_wodom_curr.y() = odometryBuf.front()->pose.pose.orientation.y;
293 | q_wodom_curr.z() = odometryBuf.front()->pose.pose.orientation.z;
294 | q_wodom_curr.w() = odometryBuf.front()->pose.pose.orientation.w;
295 | t_wodom_curr.x() = odometryBuf.front()->pose.pose.position.x;
296 | t_wodom_curr.y() = odometryBuf.front()->pose.pose.position.y;
297 | t_wodom_curr.z() = odometryBuf.front()->pose.pose.position.z;
298 | odometryBuf.pop();
299 |
300 | while(!cornerLastBuf.empty())
301 | {
302 | cornerLastBuf.pop();
303 | //printf("drop lidar frame in mapping for real time performance \n");
304 | }
305 |
306 | mBuf.unlock();
307 |
308 | TicToc t_whole;
309 |
310 | transformAssociateToMap();
311 |
312 | TicToc t_shift;
313 | int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth;
314 | int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight;
315 | int centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth;
316 |
317 | if (t_w_curr.x() + 25.0 < 0)
318 | centerCubeI--;
319 | if (t_w_curr.y() + 25.0 < 0)
320 | centerCubeJ--;
321 | if (t_w_curr.z() + 25.0 < 0)
322 | centerCubeK--;
323 |
324 | while (centerCubeI < 3)
325 | {
326 | for (int j = 0; j < laserCloudHeight; j++)
327 | {
328 | for (int k = 0; k < laserCloudDepth; k++)
329 | {
330 | int i = laserCloudWidth - 1;
331 | pcl::PointCloud::Ptr laserCloudCubeCornerPointer =
332 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
333 | pcl::PointCloud::Ptr laserCloudCubeSurfPointer =
334 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
335 | for (; i >= 1; i--)
336 | {
337 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
338 | laserCloudCornerArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
339 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
340 | laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
341 | }
342 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
343 | laserCloudCubeCornerPointer;
344 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
345 | laserCloudCubeSurfPointer;
346 | laserCloudCubeCornerPointer->clear();
347 | laserCloudCubeSurfPointer->clear();
348 | }
349 | }
350 |
351 | centerCubeI++;
352 | laserCloudCenWidth++;
353 | }
354 |
355 | while (centerCubeI >= laserCloudWidth - 3)
356 | {
357 | for (int j = 0; j < laserCloudHeight; j++)
358 | {
359 | for (int k = 0; k < laserCloudDepth; k++)
360 | {
361 | int i = 0;
362 | pcl::PointCloud::Ptr laserCloudCubeCornerPointer =
363 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
364 | pcl::PointCloud::Ptr laserCloudCubeSurfPointer =
365 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
366 | for (; i < laserCloudWidth - 1; i++)
367 | {
368 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
369 | laserCloudCornerArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
370 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
371 | laserCloudSurfArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
372 | }
373 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
374 | laserCloudCubeCornerPointer;
375 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
376 | laserCloudCubeSurfPointer;
377 | laserCloudCubeCornerPointer->clear();
378 | laserCloudCubeSurfPointer->clear();
379 | }
380 | }
381 |
382 | centerCubeI--;
383 | laserCloudCenWidth--;
384 | }
385 |
386 | while (centerCubeJ < 3)
387 | {
388 | for (int i = 0; i < laserCloudWidth; i++)
389 | {
390 | for (int k = 0; k < laserCloudDepth; k++)
391 | {
392 | int j = laserCloudHeight - 1;
393 | pcl::PointCloud::Ptr laserCloudCubeCornerPointer =
394 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
395 | pcl::PointCloud::Ptr laserCloudCubeSurfPointer =
396 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
397 | for (; j >= 1; j--)
398 | {
399 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
400 | laserCloudCornerArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];
401 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
402 | laserCloudSurfArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];
403 | }
404 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
405 | laserCloudCubeCornerPointer;
406 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
407 | laserCloudCubeSurfPointer;
408 | laserCloudCubeCornerPointer->clear();
409 | laserCloudCubeSurfPointer->clear();
410 | }
411 | }
412 |
413 | centerCubeJ++;
414 | laserCloudCenHeight++;
415 | }
416 |
417 | while (centerCubeJ >= laserCloudHeight - 3)
418 | {
419 | for (int i = 0; i < laserCloudWidth; i++)
420 | {
421 | for (int k = 0; k < laserCloudDepth; k++)
422 | {
423 | int j = 0;
424 | pcl::PointCloud::Ptr laserCloudCubeCornerPointer =
425 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
426 | pcl::PointCloud::Ptr laserCloudCubeSurfPointer =
427 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
428 | for (; j < laserCloudHeight - 1; j++)
429 | {
430 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
431 | laserCloudCornerArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];
432 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
433 | laserCloudSurfArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];
434 | }
435 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
436 | laserCloudCubeCornerPointer;
437 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
438 | laserCloudCubeSurfPointer;
439 | laserCloudCubeCornerPointer->clear();
440 | laserCloudCubeSurfPointer->clear();
441 | }
442 | }
443 |
444 | centerCubeJ--;
445 | laserCloudCenHeight--;
446 | }
447 |
448 | while (centerCubeK < 3)
449 | {
450 | for (int i = 0; i < laserCloudWidth; i++)
451 | {
452 | for (int j = 0; j < laserCloudHeight; j++)
453 | {
454 | int k = laserCloudDepth - 1;
455 | pcl::PointCloud::Ptr laserCloudCubeCornerPointer =
456 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
457 | pcl::PointCloud::Ptr laserCloudCubeSurfPointer =
458 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
459 | for (; k >= 1; k--)
460 | {
461 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
462 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];
463 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
464 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];
465 | }
466 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
467 | laserCloudCubeCornerPointer;
468 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
469 | laserCloudCubeSurfPointer;
470 | laserCloudCubeCornerPointer->clear();
471 | laserCloudCubeSurfPointer->clear();
472 | }
473 | }
474 |
475 | centerCubeK++;
476 | laserCloudCenDepth++;
477 | }
478 |
479 | while (centerCubeK >= laserCloudDepth - 3)
480 | {
481 | for (int i = 0; i < laserCloudWidth; i++)
482 | {
483 | for (int j = 0; j < laserCloudHeight; j++)
484 | {
485 | int k = 0;
486 | pcl::PointCloud::Ptr laserCloudCubeCornerPointer =
487 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
488 | pcl::PointCloud::Ptr laserCloudCubeSurfPointer =
489 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
490 | for (; k < laserCloudDepth - 1; k++)
491 | {
492 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
493 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];
494 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
495 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];
496 | }
497 | laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
498 | laserCloudCubeCornerPointer;
499 | laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
500 | laserCloudCubeSurfPointer;
501 | laserCloudCubeCornerPointer->clear();
502 | laserCloudCubeSurfPointer->clear();
503 | }
504 | }
505 |
506 | centerCubeK--;
507 | laserCloudCenDepth--;
508 | }
509 |
510 | int laserCloudValidNum = 0;
511 | int laserCloudSurroundNum = 0;
512 |
513 | for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++)
514 | {
515 | for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++)
516 | {
517 | for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++)
518 | {
519 | if (i >= 0 && i < laserCloudWidth &&
520 | j >= 0 && j < laserCloudHeight &&
521 | k >= 0 && k < laserCloudDepth)
522 | {
523 | laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
524 | laserCloudValidNum++;
525 | laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
526 | laserCloudSurroundNum++;
527 | }
528 | }
529 | }
530 | }
531 |
532 | laserCloudCornerFromMap->clear();
533 | laserCloudSurfFromMap->clear();
534 | for (int i = 0; i < laserCloudValidNum; i++)
535 | {
536 | *laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
537 | *laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
538 | }
539 | int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size();
540 | int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size();
541 |
542 |
543 | pcl::PointCloud::Ptr laserCloudCornerStack(new pcl::PointCloud());
544 | downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
545 | downSizeFilterCorner.filter(*laserCloudCornerStack);
546 | int laserCloudCornerStackNum = laserCloudCornerStack->points.size();
547 |
548 | pcl::PointCloud::Ptr laserCloudSurfStack(new pcl::PointCloud());
549 | downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
550 | downSizeFilterSurf.filter(*laserCloudSurfStack);
551 | int laserCloudSurfStackNum = laserCloudSurfStack->points.size();
552 |
553 | //printf("map prepare time %f ms\n", t_shift.toc());
554 | //printf("map corner num %d surf num %d \n", laserCloudCornerFromMapNum, laserCloudSurfFromMapNum);
555 | if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 50)
556 | {
557 | TicToc t_opt;
558 | TicToc t_tree;
559 | kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);
560 | kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);
561 | //printf("build tree time %f ms \n", t_tree.toc());
562 |
563 | for (int iterCount = 0; iterCount < 2; iterCount++)
564 | {
565 | //ceres::LossFunction *loss_function = NULL;
566 | ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
567 | ceres::LocalParameterization *q_parameterization =
568 | new ceres::EigenQuaternionParameterization();
569 | ceres::Problem::Options problem_options;
570 |
571 | ceres::Problem problem(problem_options);
572 | problem.AddParameterBlock(parameters, 4, q_parameterization);
573 | problem.AddParameterBlock(parameters + 4, 3);
574 |
575 | TicToc t_data;
576 | int corner_num = 0;
577 |
578 | for (int i = 0; i < laserCloudCornerStackNum; i++)
579 | {
580 | pointOri = laserCloudCornerStack->points[i];
581 | //double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;
582 | pointAssociateToMap(&pointOri, &pointSel);
583 | kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
584 |
585 | if (pointSearchSqDis[4] < 1.0)
586 | {
587 | std::vector nearCorners;
588 | Eigen::Vector3d center(0, 0, 0);
589 | for (int j = 0; j < 5; j++)
590 | {
591 | Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,
592 | laserCloudCornerFromMap->points[pointSearchInd[j]].y,
593 | laserCloudCornerFromMap->points[pointSearchInd[j]].z);
594 | center = center + tmp;
595 | nearCorners.push_back(tmp);
596 | }
597 | center = center / 5.0;
598 |
599 | Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
600 | for (int j = 0; j < 5; j++)
601 | {
602 | Eigen::Matrix tmpZeroMean = nearCorners[j] - center;
603 | covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
604 | }
605 |
606 | Eigen::SelfAdjointEigenSolver saes(covMat);
607 |
608 | // if is indeed line feature
609 | // note Eigen library sort eigenvalues in increasing order
610 | Eigen::Vector3d unit_direction = saes.eigenvectors().col(2);
611 | Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
612 | if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])
613 | {
614 | Eigen::Vector3d point_on_line = center;
615 | Eigen::Vector3d point_a, point_b;
616 | point_a = 0.1 * unit_direction + point_on_line;
617 | point_b = -0.1 * unit_direction + point_on_line;
618 |
619 | ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);
620 | problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
621 | corner_num++;
622 | }
623 | }
624 | /*
625 | else if(pointSearchSqDis[4] < 0.01 * sqrtDis)
626 | {
627 | Eigen::Vector3d center(0, 0, 0);
628 | for (int j = 0; j < 5; j++)
629 | {
630 | Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,
631 | laserCloudCornerFromMap->points[pointSearchInd[j]].y,
632 | laserCloudCornerFromMap->points[pointSearchInd[j]].z);
633 | center = center + tmp;
634 | }
635 | center = center / 5.0;
636 | Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
637 | ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);
638 | problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
639 | }
640 | */
641 | }
642 |
643 | int surf_num = 0;
644 | for (int i = 0; i < laserCloudSurfStackNum; i++)
645 | {
646 | pointOri = laserCloudSurfStack->points[i];
647 | //double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;
648 | pointAssociateToMap(&pointOri, &pointSel);
649 | kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
650 |
651 | Eigen::Matrix matA0;
652 | Eigen::Matrix matB0 = -1 * Eigen::Matrix::Ones();
653 | if (pointSearchSqDis[4] < 1.0)
654 | {
655 |
656 | for (int j = 0; j < 5; j++)
657 | {
658 | matA0(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;
659 | matA0(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;
660 | matA0(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;
661 | ////printf(" pts %f %f %f ", matA0(j, 0), matA0(j, 1), matA0(j, 2));
662 | }
663 | // find the norm of plane
664 | Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0);
665 | double negative_OA_dot_norm = 1 / norm.norm();
666 | norm.normalize();
667 |
668 | // Here n(pa, pb, pc) is unit norm of plane
669 | bool planeValid = true;
670 | for (int j = 0; j < 5; j++)
671 | {
672 | // if OX * n > 0.2, then plane is not fit well
673 | if (fabs(norm(0) * laserCloudSurfFromMap->points[pointSearchInd[j]].x +
674 | norm(1) * laserCloudSurfFromMap->points[pointSearchInd[j]].y +
675 | norm(2) * laserCloudSurfFromMap->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2)
676 | {
677 | planeValid = false;
678 | break;
679 | }
680 | }
681 | Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
682 | if (planeValid)
683 | {
684 | ceres::CostFunction *cost_function = LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm);
685 | problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
686 | surf_num++;
687 | }
688 | }
689 | /*
690 | else if(pointSearchSqDis[4] < 0.01 * sqrtDis)
691 | {
692 | Eigen::Vector3d center(0, 0, 0);
693 | for (int j = 0; j < 5; j++)
694 | {
695 | Eigen::Vector3d tmp(laserCloudSurfFromMap->points[pointSearchInd[j]].x,
696 | laserCloudSurfFromMap->points[pointSearchInd[j]].y,
697 | laserCloudSurfFromMap->points[pointSearchInd[j]].z);
698 | center = center + tmp;
699 | }
700 | center = center / 5.0;
701 | Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
702 | ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);
703 | problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
704 | }
705 | */
706 | }
707 |
708 | ////printf("corner num %d used corner num %d \n", laserCloudCornerStackNum, corner_num);
709 | ////printf("surf num %d used surf num %d \n", laserCloudSurfStackNum, surf_num);
710 |
711 | //printf("mapping data assosiation time %f ms \n", t_data.toc());
712 |
713 | TicToc t_solver;
714 | ceres::Solver::Options options;
715 | options.linear_solver_type = ceres::DENSE_QR;
716 | options.max_num_iterations = 4;
717 | options.minimizer_progress_to_stdout = false;
718 | options.check_gradients = false;
719 | options.gradient_check_relative_precision = 1e-4;
720 | ceres::Solver::Summary summary;
721 | ceres::Solve(options, &problem, &summary);
722 | //printf("mapping solver time %f ms \n", t_solver.toc());
723 |
724 | ////printf("time %f \n", timeLaserOdometry);
725 | ////printf("corner factor num %d surf factor num %d\n", corner_num, surf_num);
726 | ////printf("result q %f %f %f %f result t %f %f %f\n", parameters[3], parameters[0], parameters[1], parameters[2],
727 | // parameters[4], parameters[5], parameters[6]);
728 | }
729 | //printf("mapping optimization time %f \n", t_opt.toc());
730 | }
731 | else
732 | {
733 | ROS_WARN("time Map corner and surf num are not enough");
734 | }
735 | transformUpdate();
736 |
737 | TicToc t_add;
738 | for (int i = 0; i < laserCloudCornerStackNum; i++)
739 | {
740 | pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);
741 |
742 | int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
743 | int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
744 | int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;
745 |
746 | if (pointSel.x + 25.0 < 0)
747 | cubeI--;
748 | if (pointSel.y + 25.0 < 0)
749 | cubeJ--;
750 | if (pointSel.z + 25.0 < 0)
751 | cubeK--;
752 |
753 | if (cubeI >= 0 && cubeI < laserCloudWidth &&
754 | cubeJ >= 0 && cubeJ < laserCloudHeight &&
755 | cubeK >= 0 && cubeK < laserCloudDepth)
756 | {
757 | int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
758 | laserCloudCornerArray[cubeInd]->push_back(pointSel);
759 | }
760 | }
761 |
762 | for (int i = 0; i < laserCloudSurfStackNum; i++)
763 | {
764 | pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);
765 |
766 | int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
767 | int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
768 | int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;
769 |
770 | if (pointSel.x + 25.0 < 0)
771 | cubeI--;
772 | if (pointSel.y + 25.0 < 0)
773 | cubeJ--;
774 | if (pointSel.z + 25.0 < 0)
775 | cubeK--;
776 |
777 | if (cubeI >= 0 && cubeI < laserCloudWidth &&
778 | cubeJ >= 0 && cubeJ < laserCloudHeight &&
779 | cubeK >= 0 && cubeK < laserCloudDepth)
780 | {
781 | int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
782 | laserCloudSurfArray[cubeInd]->push_back(pointSel);
783 | }
784 | }
785 | //printf("add points time %f ms\n", t_add.toc());
786 |
787 |
788 | TicToc t_filter;
789 | for (int i = 0; i < laserCloudValidNum; i++)
790 | {
791 | int ind = laserCloudValidInd[i];
792 |
793 | pcl::PointCloud::Ptr tmpCorner(new pcl::PointCloud());
794 | downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);
795 | downSizeFilterCorner.filter(*tmpCorner);
796 | laserCloudCornerArray[ind] = tmpCorner;
797 |
798 | pcl::PointCloud::Ptr tmpSurf(new pcl::PointCloud());
799 | downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);
800 | downSizeFilterSurf.filter(*tmpSurf);
801 | laserCloudSurfArray[ind] = tmpSurf;
802 | }
803 | //printf("filter time %f ms \n", t_filter.toc());
804 |
805 | TicToc t_pub;
806 | //publish surround map for every 5 frame
807 | if (frameCount % 5 == 0)
808 | {
809 | laserCloudSurround->clear();
810 | for (int i = 0; i < laserCloudSurroundNum; i++)
811 | {
812 | int ind = laserCloudSurroundInd[i];
813 | *laserCloudSurround += *laserCloudCornerArray[ind];
814 | *laserCloudSurround += *laserCloudSurfArray[ind];
815 | }
816 |
817 | sensor_msgs::PointCloud2 laserCloudSurround3;
818 | pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);
819 | laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
820 | laserCloudSurround3.header.frame_id = "/camera_init";
821 | pubLaserCloudSurround.publish(laserCloudSurround3);
822 | }
823 |
824 | if (frameCount % 20 == 0)
825 | {
826 | pcl::PointCloud laserCloudMap;
827 | for (int i = 0; i < 4851; i++)
828 | {
829 | laserCloudMap += *laserCloudCornerArray[i];
830 | laserCloudMap += *laserCloudSurfArray[i];
831 | }
832 | sensor_msgs::PointCloud2 laserCloudMsg;
833 | pcl::toROSMsg(laserCloudMap, laserCloudMsg);
834 | laserCloudMsg.header.stamp = ros::Time().fromSec(timeLaserOdometry);
835 | laserCloudMsg.header.frame_id = "/camera_init";
836 | pubLaserCloudMap.publish(laserCloudMsg);
837 | }
838 |
839 | sensor_msgs::PointCloud2 laserCloudFullRes3Local;
840 | pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3Local);
841 | laserCloudFullRes3Local.header.stamp = ros::Time().fromSec(timeLaserOdometry);
842 | laserCloudFullRes3Local.header.frame_id = "/camera_init";
843 | pubLaserCloudFullResLocal.publish(laserCloudFullRes3Local);
844 |
845 | int laserCloudFullResNum = laserCloudFullRes->points.size();
846 | for (int i = 0; i < laserCloudFullResNum; i++)
847 | {
848 | pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
849 | }
850 |
851 | sensor_msgs::PointCloud2 laserCloudFullRes3;
852 | pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
853 | laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
854 | laserCloudFullRes3.header.frame_id = "/camera_init";
855 | pubLaserCloudFullRes.publish(laserCloudFullRes3);
856 |
857 | //printf("mapping pub time %f ms \n", t_pub.toc());
858 |
859 | //printf("whole mapping time %f ms ++++++++++\n", t_whole.toc());
860 |
861 | nav_msgs::Odometry odomAftMapped;
862 | odomAftMapped.header.frame_id = "/camera_init";
863 | odomAftMapped.child_frame_id = "/aft_mapped";
864 | odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);
865 | odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
866 | odomAftMapped.pose.pose.orientation.y = q_w_curr.y();
867 | odomAftMapped.pose.pose.orientation.z = q_w_curr.z();
868 | odomAftMapped.pose.pose.orientation.w = q_w_curr.w();
869 | odomAftMapped.pose.pose.position.x = t_w_curr.x();
870 | odomAftMapped.pose.pose.position.y = t_w_curr.y();
871 | odomAftMapped.pose.pose.position.z = t_w_curr.z();
872 | pubOdomAftMapped.publish(odomAftMapped);
873 |
874 | // double roll, pitch, yaw;
875 | // geometry_msgs::Quaternion quat = odomAftMapped.pose.pose.orientation;
876 | // tf::Matrix3x3(tf::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(roll, pitch, yaw);
877 | // std::cout << "pub - roll, pitch, yaw (deg): " << rad2deg(roll) << ", " << rad2deg(pitch) << ", " << rad2deg(yaw) << std::endl;
878 |
879 | geometry_msgs::PoseStamped laserAfterMappedPose;
880 | laserAfterMappedPose.header = odomAftMapped.header;
881 | laserAfterMappedPose.pose = odomAftMapped.pose.pose;
882 |
883 | laserAfterMappedPath.header.stamp = odomAftMapped.header.stamp;
884 | laserAfterMappedPath.header.frame_id = "/camera_init";
885 | laserAfterMappedPath.poses.push_back(laserAfterMappedPose);
886 | pubLaserAfterMappedPath.publish(laserAfterMappedPath);
887 |
888 | static tf::TransformBroadcaster br;
889 | tf::Transform transform;
890 | tf::Quaternion q;
891 | transform.setOrigin(tf::Vector3(t_w_curr(0),
892 | t_w_curr(1),
893 | t_w_curr(2)));
894 | q.setW(q_w_curr.w());
895 | q.setX(q_w_curr.x());
896 | q.setY(q_w_curr.y());
897 | q.setZ(q_w_curr.z());
898 | transform.setRotation(q);
899 | br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "/camera_init", "/aft_mapped"));
900 |
901 | frameCount++;
902 | }
903 | std::chrono::milliseconds dura(2);
904 | std::this_thread::sleep_for(dura);
905 | }
906 | }
907 |
908 | int main(int argc, char **argv)
909 | {
910 | ros::init(argc, argv, "laserMapping");
911 | ros::NodeHandle nh;
912 |
913 | float lineRes = 0;
914 | float planeRes = 0;
915 | nh.param("mapping_line_resolution", lineRes, 0.4);
916 | nh.param("mapping_plane_resolution", planeRes, 0.8);
917 | //printf("line resolution %f plane resolution %f \n", lineRes, planeRes);
918 | downSizeFilterCorner.setLeafSize(lineRes, lineRes,lineRes);
919 | downSizeFilterSurf.setLeafSize(planeRes, planeRes, planeRes);
920 |
921 | ros::Subscriber subLaserCloudCornerLast = nh.subscribe("/laser_cloud_corner_last", 100, laserCloudCornerLastHandler);
922 |
923 | ros::Subscriber subLaserCloudSurfLast = nh.subscribe("/laser_cloud_surf_last", 100, laserCloudSurfLastHandler);
924 |
925 | ros::Subscriber subLaserOdometry = nh.subscribe("/laser_odom_to_init", 100, laserOdometryHandler);
926 |
927 | ros::Subscriber subLaserCloudFullRes = nh.subscribe("/velodyne_cloud_3", 100, laserCloudFullResHandler);
928 |
929 | pubLaserCloudSurround = nh.advertise("/laser_cloud_surround", 100);
930 |
931 | pubLaserCloudMap = nh.advertise("/laser_cloud_map", 100);
932 |
933 | pubLaserCloudFullRes = nh.advertise("/velodyne_cloud_registered", 100);
934 | pubLaserCloudFullResLocal = nh.advertise("/velodyne_cloud_registered_local", 100);
935 |
936 | pubOdomAftMapped = nh.advertise("/aft_mapped_to_init", 100);
937 |
938 | pubOdomAftMappedHighFrec = nh.advertise("/aft_mapped_to_init_high_frec", 100);
939 |
940 | pubLaserAfterMappedPath = nh.advertise("/aft_mapped_path", 100);
941 |
942 | for (int i = 0; i < laserCloudNum; i++)
943 | {
944 | laserCloudCornerArray[i].reset(new pcl::PointCloud());
945 | laserCloudSurfArray[i].reset(new pcl::PointCloud());
946 | }
947 |
948 | std::thread mapping_process{process};
949 |
950 | ros::spin();
951 |
952 | return 0;
953 | }
--------------------------------------------------------------------------------
/src/laserOdometry.cpp:
--------------------------------------------------------------------------------
1 | // This is an advanced implementation of the algorithm described in the following paper:
2 | // J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
3 | // Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
4 |
5 | // Modifier: Tong Qin qintonguav@gmail.com
6 | // Shaozu Cao saozu.cao@connect.ust.hk
7 |
8 |
9 | // Copyright 2013, Ji Zhang, Carnegie Mellon University
10 | // Further contributions copyright (c) 2016, Southwest Research Institute
11 | // All rights reserved.
12 | //
13 | // Redistribution and use in source and binary forms, with or without
14 | // modification, are permitted provided that the following conditions are met:
15 | //
16 | // 1. Redistributions of source code must retain the above copyright notice,
17 | // this list of conditions and the following disclaimer.
18 | // 2. Redistributions in binary form must reproduce the above copyright notice,
19 | // this list of conditions and the following disclaimer in the documentation
20 | // and/or other materials provided with the distribution.
21 | // 3. Neither the name of the copyright holder nor the names of its
22 | // contributors may be used to endorse or promote products derived from this
23 | // software without specific prior written permission.
24 | //
25 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
26 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
27 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
28 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
29 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
30 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
31 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
32 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
33 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
34 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 | // POSSIBILITY OF SUCH DAMAGE.
36 |
37 | #include
38 | #include
39 | #include
40 | #include
41 | #include
42 | #include
43 | #include
44 | #include
45 | #include
46 | #include
47 | #include
48 | #include
49 | #include
50 | #include
51 | #include
52 | #include
53 | #include
54 |
55 | #include "aloam_velodyne/common.h"
56 | #include "aloam_velodyne/tic_toc.h"
57 | #include "lidarFactor.hpp"
58 |
59 | #define DISTORTION 0
60 |
61 |
62 | int corner_correspondence = 0, plane_correspondence = 0;
63 |
64 | constexpr double SCAN_PERIOD = 0.1;
65 | constexpr double DISTANCE_SQ_THRESHOLD = 25;
66 | constexpr double NEARBY_SCAN = 2.5;
67 |
68 | int skipFrameNum = 5;
69 | bool systemInited = false;
70 |
71 | double timeCornerPointsSharp = 0;
72 | double timeCornerPointsLessSharp = 0;
73 | double timeSurfPointsFlat = 0;
74 | double timeSurfPointsLessFlat = 0;
75 | double timeLaserCloudFullRes = 0;
76 |
77 | pcl::KdTreeFLANN::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN());
78 | pcl::KdTreeFLANN::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN());
79 |
80 | pcl::PointCloud::Ptr cornerPointsSharp(new pcl::PointCloud());
81 | pcl::PointCloud::Ptr cornerPointsLessSharp(new pcl::PointCloud());
82 | pcl::PointCloud::Ptr surfPointsFlat(new pcl::PointCloud());
83 | pcl::PointCloud::Ptr surfPointsLessFlat(new pcl::PointCloud());
84 |
85 | pcl::PointCloud::Ptr laserCloudCornerLast(new pcl::PointCloud());
86 | pcl::PointCloud::Ptr laserCloudSurfLast(new pcl::PointCloud());
87 | pcl::PointCloud::Ptr laserCloudFullRes(new pcl::PointCloud());
88 |
89 | int laserCloudCornerLastNum = 0;
90 | int laserCloudSurfLastNum = 0;
91 |
92 | // Transformation from current frame to world frame
93 | Eigen::Quaterniond q_w_curr(1, 0, 0, 0);
94 | Eigen::Vector3d t_w_curr(0, 0, 0);
95 |
96 | // q_curr_last(x, y, z, w), t_curr_last
97 | double para_q[4] = {0, 0, 0, 1};
98 | double para_t[3] = {0, 0, 0};
99 |
100 | Eigen::Map q_last_curr(para_q);
101 | Eigen::Map t_last_curr(para_t);
102 |
103 | std::queue cornerSharpBuf;
104 | std::queue cornerLessSharpBuf;
105 | std::queue surfFlatBuf;
106 | std::queue surfLessFlatBuf;
107 | std::queue fullPointsBuf;
108 | std::mutex mBuf;
109 |
110 | // undistort lidar point
111 | void TransformToStart(PointType const *const pi, PointType *const po)
112 | {
113 | //interpolation ratio
114 | double s;
115 | if (DISTORTION)
116 | s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;
117 | else
118 | s = 1.0;
119 | //s = 1;
120 | Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);
121 | Eigen::Vector3d t_point_last = s * t_last_curr;
122 | Eigen::Vector3d point(pi->x, pi->y, pi->z);
123 | Eigen::Vector3d un_point = q_point_last * point + t_point_last;
124 |
125 | po->x = un_point.x();
126 | po->y = un_point.y();
127 | po->z = un_point.z();
128 | po->intensity = pi->intensity;
129 | }
130 |
131 | // transform all lidar points to the start of the next frame
132 |
133 | void TransformToEnd(PointType const *const pi, PointType *const po)
134 | {
135 | // undistort point first
136 | pcl::PointXYZI un_point_tmp;
137 | TransformToStart(pi, &un_point_tmp);
138 |
139 | Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z);
140 | Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr);
141 |
142 | po->x = point_end.x();
143 | po->y = point_end.y();
144 | po->z = point_end.z();
145 |
146 | //Remove distortion time info
147 | po->intensity = int(pi->intensity);
148 | }
149 |
150 | void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsSharp2)
151 | {
152 | mBuf.lock();
153 | cornerSharpBuf.push(cornerPointsSharp2);
154 | mBuf.unlock();
155 | }
156 |
157 | void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsLessSharp2)
158 | {
159 | mBuf.lock();
160 | cornerLessSharpBuf.push(cornerPointsLessSharp2);
161 | mBuf.unlock();
162 | }
163 |
164 | void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsFlat2)
165 | {
166 | mBuf.lock();
167 | surfFlatBuf.push(surfPointsFlat2);
168 | mBuf.unlock();
169 | }
170 |
171 | void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsLessFlat2)
172 | {
173 | mBuf.lock();
174 | surfLessFlatBuf.push(surfPointsLessFlat2);
175 | mBuf.unlock();
176 | }
177 |
178 | //receive all point cloud
179 | void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2)
180 | {
181 | mBuf.lock();
182 | fullPointsBuf.push(laserCloudFullRes2);
183 | mBuf.unlock();
184 | }
185 |
186 | int main(int argc, char **argv)
187 | {
188 | ros::init(argc, argv, "laserOdometry");
189 | ros::NodeHandle nh;
190 |
191 | nh.param("mapping_skip_frame", skipFrameNum, 2);
192 |
193 | //printf("Mapping %d Hz \n", 10 / skipFrameNum);
194 |
195 | ros::Subscriber subCornerPointsSharp = nh.subscribe("/laser_cloud_sharp", 100, laserCloudSharpHandler);
196 |
197 | ros::Subscriber subCornerPointsLessSharp = nh.subscribe("/laser_cloud_less_sharp", 100, laserCloudLessSharpHandler);
198 |
199 | ros::Subscriber subSurfPointsFlat = nh.subscribe("/laser_cloud_flat", 100, laserCloudFlatHandler);
200 |
201 | ros::Subscriber subSurfPointsLessFlat = nh.subscribe("/laser_cloud_less_flat", 100, laserCloudLessFlatHandler);
202 |
203 | ros::Subscriber subLaserCloudFullRes = nh.subscribe("/velodyne_cloud_2", 100, laserCloudFullResHandler);
204 |
205 | ros::Publisher pubLaserCloudCornerLast = nh.advertise("/laser_cloud_corner_last", 100);
206 |
207 | ros::Publisher pubLaserCloudSurfLast = nh.advertise("/laser_cloud_surf_last", 100);
208 |
209 | ros::Publisher pubLaserCloudFullRes = nh.advertise("/velodyne_cloud_3", 100);
210 |
211 | ros::Publisher pubLaserOdometry = nh.advertise("/laser_odom_to_init", 100);
212 |
213 | ros::Publisher pubLaserPath = nh.advertise