├── SC-A-LOAM
    ├── utils
    │   └── python
    │   │   ├── bone_table.npy
    │   │   ├── jet_table.npy
    │   │   ├── __pycache__
    │   │       └── pypcdMyUtils.cpython-36.pyc
    │   │   ├── pypcdMyUtils.py
    │   │   └── makeMergedMap.py
    ├── launch
    │   ├── kitti_helper.launch
    │   ├── aloam_velodyne_HDL_64.launch
    │   ├── aloam_velodyne_VLP_16.launch
    │   ├── aloam_mulran.launch
    │   ├── floam_velodyne_32.launch
    │   └── floam_velodyne_64.launch
    ├── package.xml
    ├── include
    │   ├── aloam_velodyne
    │   │   ├── tic_toc.h
    │   │   └── common.h
    │   └── scancontext
    │   │   ├── Scancontext.h
    │   │   ├── KDTreeVectorOfVectorsAdaptor.h
    │   │   └── Scancontext.cpp
    ├── CMakeLists.txt
    ├── README.md
    ├── src
    │   ├── lidarFactor.hpp
    │   ├── kittiHelper.cpp
    │   └── scanRegistration.cpp
    └── rviz_cfg
    │   └── aloam_velodyne.rviz
├── floam
    ├── src
    │   ├── lidar.cpp
    │   ├── laserMappingNode.cpp
    │   ├── laserProcessingNode.cpp
    │   ├── lidarOptimization.cpp
    │   ├── odomEstimationNode.cpp
    │   ├── laserMappingClass.cpp
    │   ├── laserProcessingClass.cpp
    │   └── odomEstimationClass.cpp
    ├── include
    │   ├── lidar.h
    │   ├── laserProcessingClass.h
    │   ├── laserMappingClass.h
    │   ├── lidarOptimization.h
    │   └── odomEstimationClass.h
    ├── package.xml
    ├── launch
    │   ├── floam_velodyne.launch
    │   ├── floam.launch
    │   └── floam_mapping.launch
    ├── CMakeLists.txt
    ├── README.md
    └── rviz
    │   ├── floam_velodyne.rviz
    │   ├── floam.rviz
    │   └── floam_mapping.rviz
├── LICENSE
└── README.md


/SC-A-LOAM/utils/python/bone_table.npy:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/cuge1995/SC-FLOAM/HEAD/SC-A-LOAM/utils/python/bone_table.npy


--------------------------------------------------------------------------------
/SC-A-LOAM/utils/python/jet_table.npy:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/cuge1995/SC-FLOAM/HEAD/SC-A-LOAM/utils/python/jet_table.npy


--------------------------------------------------------------------------------
/SC-A-LOAM/utils/python/__pycache__/pypcdMyUtils.cpython-36.pyc:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/cuge1995/SC-FLOAM/HEAD/SC-A-LOAM/utils/python/__pycache__/pypcdMyUtils.cpython-36.pyc


--------------------------------------------------------------------------------
/SC-A-LOAM/launch/kitti_helper.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |     
 3 | 
 4 |     <node name="kittiHelper" pkg="aloam_velodyne" type="kittiHelper" output="screen"> 
 5 |         <param name="dataset_folder" type="string" value="/data/KITTI/odometry/" />
 6 |         <param name="sequence_number" type="string" value="00" />
 7 |         <param name="to_bag" type="bool" value="false" />
 8 |         <param name="output_bag_file" type="string" value="/tmp/kitti.bag" /> <!-- replace with your output folder -->
 9 |         <param name="publish_delay" type="int" value="1" />
10 |     </node>
11 | </launch>


--------------------------------------------------------------------------------
/floam/src/lidar.cpp:
--------------------------------------------------------------------------------
 1 | // Author of FLOAM: Wang Han 
 2 | // Email wh200720041@gmail.com
 3 | // Homepage https://wanghan.pro
 4 | 
 5 | #include "lidar.h"
 6 | 
 7 | 
 8 | lidar::Lidar::Lidar(){
 9 |  
10 | }
11 | 
12 | 
13 | void lidar::Lidar::setLines(double num_lines_in){
14 |     num_lines=num_lines_in;
15 | }
16 | 
17 | 
18 | void lidar::Lidar::setVerticalAngle(double vertical_angle_in){
19 |     vertical_angle = vertical_angle_in;
20 | }
21 | 
22 | 
23 | void lidar::Lidar::setVerticalResolution(double vertical_angle_resolution_in){
24 |     vertical_angle_resolution = vertical_angle_resolution_in;
25 | }
26 | 
27 | 
28 | void lidar::Lidar::setScanPeriod(double scan_period_in){
29 |     scan_period = scan_period_in;
30 | }
31 | 
32 | 
33 | void lidar::Lidar::setMaxDistance(double max_distance_in){
34 | 	max_distance = max_distance_in;
35 | }
36 | 
37 | void lidar::Lidar::setMinDistance(double min_distance_in){
38 | 	min_distance = min_distance_in;
39 | }


--------------------------------------------------------------------------------
/floam/include/lidar.h:
--------------------------------------------------------------------------------
 1 | // Author of FLOAM: Wang Han 
 2 | // Email wh200720041@gmail.com
 3 | // Homepage https://wanghan.pro
 4 | #ifndef _LIDAR_H_
 5 | #define _LIDAR_H_
 6 | 
 7 | //define lidar parameter
 8 | 
 9 | namespace lidar{
10 | 
11 | class Lidar
12 | {
13 |     public:
14 |         Lidar();
15 | 
16 |         void setScanPeriod(double scan_period_in);
17 |         void setLines(double num_lines_in);
18 |         void setVerticalAngle(double vertical_angle_in);
19 |         void setVerticalResolution(double vertical_angle_resolution_in);
20 |         //by default is 100. pls do not change
21 |         void setMaxDistance(double max_distance_in);
22 |         void setMinDistance(double min_distance_in);
23 | 
24 |     	double max_distance;
25 |         double min_distance;
26 |         int num_lines;
27 |         double scan_period;
28 |         int points_per_line;
29 |         double horizontal_angle_resolution;
30 |         double horizontal_angle;
31 |         double vertical_angle_resolution;
32 |         double vertical_angle;
33 | };
34 | 
35 | 
36 | }
37 | 
38 | 
39 | #endif // _LIDAR_H_
40 | 
41 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2021 Jinlai Zhang
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/floam/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>floam</name>
 4 |   <version>0.0.0</version>
 5 | 
 6 |   <description>
 7 |     This work is created based on some optimization on ALOAM by HKUST and LOAM by CMU
 8 |   </description>
 9 | 
10 |   <maintainer email="wh200720041@gmail.com">Han Wang</maintainer>
11 | 
12 |   <license>BSD</license>
13 |   <url>https://wanghan.pro</url>
14 |   <author email="wh200720041@gmail.com">Han Wang</author>
15 |   
16 |   <buildtool_depend>catkin</buildtool_depend>
17 |   <build_depend>geometry_msgs</build_depend>
18 |   <build_depend>nav_msgs</build_depend>
19 |   <build_depend>roscpp</build_depend>
20 |   <build_depend>rospy</build_depend>
21 |   <build_depend>std_msgs</build_depend>  
22 |   <build_depend>rosbag</build_depend>
23 |   <build_depend>sensor_msgs</build_depend>
24 |   <build_depend>tf</build_depend>
25 |   <build_depend>eigen_conversions</build_depend>
26 |   <run_depend>geometry_msgs</run_depend>
27 |   <run_depend>nav_msgs</run_depend>
28 |   <run_depend>sensor_msgs</run_depend>
29 |   <run_depend>roscpp</run_depend>
30 |   <run_depend>rospy</run_depend>
31 |   <run_depend>std_msgs</run_depend>
32 |   <run_depend>rosbag</run_depend>
33 |   <run_depend>tf</run_depend>
34 |   <run_depend>eigen_conversions</run_depend>
35 | 
36 |   <export>
37 |   </export>
38 | </package>
39 | 


--------------------------------------------------------------------------------
/SC-A-LOAM/utils/python/pypcdMyUtils.py:
--------------------------------------------------------------------------------
 1 | import numpy as np
 2 | import pypcd # for the install, use this command: python3.x (use your python ver) -m pip install --user git+https://github.com/DanielPollithy/pypcd.git
 3 | from pypcd import pypcd
 4 | 
 5 | def make_xyzi_point_cloud(xyzl, label_type='f'):
 6 |     """ Make XYZL point cloud from numpy array.
 7 |     TODO i labels?
 8 |     """
 9 |     md = {'version': .7,
10 |           'fields': ['x', 'y', 'z', 'intensity'],
11 |           'count': [1, 1, 1, 1],
12 |           'width': len(xyzl),
13 |           'height': 1,
14 |           'viewpoint': [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0],
15 |           'points': len(xyzl),
16 |           'data': 'ASCII'}
17 |     if label_type.lower() == 'f':
18 |         md['size'] = [4, 4, 4, 4]
19 |         md['type'] = ['F', 'F', 'F', 'F']
20 |     elif label_type.lower() == 'u':
21 |         md['size'] = [4, 4, 4, 1]
22 |         md['type'] = ['F', 'F', 'F', 'U']
23 |     else:
24 |         raise ValueError('label type must be F or U')
25 |     # TODO use .view()
26 |     xyzl = xyzl.astype(np.float32)
27 |     dt = np.dtype([('x', np.float32), ('y', np.float32), ('z', np.float32),
28 |                    ('intensity', np.float32)])
29 |     pc_data = np.rec.fromarrays([xyzl[:, 0], xyzl[:, 1], xyzl[:, 2],
30 |                                  xyzl[:, 3]], dtype=dt)
31 |     pc = pypcd.PointCloud(md, pc_data)
32 |     return pc
33 | 


--------------------------------------------------------------------------------
/SC-A-LOAM/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>aloam_velodyne</name>
 4 |   <version>0.0.0</version>
 5 | 
 6 |   <description>
 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 |   </description>
12 | 
13 |   <maintainer email="qintonguav@gmail.com">qintong</maintainer>
14 | 
15 |   <license>BSD</license>
16 | 
17 |   <author email="zhangji@cmu.edu">Ji Zhang</author>
18 |   
19 |   <buildtool_depend>catkin</buildtool_depend>
20 |   <build_depend>geometry_msgs</build_depend>
21 |   <build_depend>nav_msgs</build_depend>
22 |   <build_depend>roscpp</build_depend>
23 |   <build_depend>rospy</build_depend>
24 |   <build_depend>std_msgs</build_depend>  
25 |   <build_depend>rosbag</build_depend>
26 |   <build_depend>sensor_msgs</build_depend>
27 |   <build_depend>tf</build_depend>
28 |   <build_depend>image_transport</build_depend>
29 |   
30 |   <run_depend>geometry_msgs</run_depend>
31 |   <run_depend>nav_msgs</run_depend>
32 |   <run_depend>sensor_msgs</run_depend>
33 |   <run_depend>roscpp</run_depend>
34 |   <run_depend>rospy</run_depend>
35 |   <run_depend>std_msgs</run_depend>
36 |   <run_depend>rosbag</run_depend>
37 |   <run_depend>tf</run_depend>
38 |   <run_depend>image_transport</run_depend>
39 | 
40 |   <export>
41 |   </export>
42 | </package>
43 | 


--------------------------------------------------------------------------------
/floam/include/laserProcessingClass.h:
--------------------------------------------------------------------------------
 1 | // Author of FLOAM: Wang Han 
 2 | // Email wh200720041@gmail.com
 3 | // Homepage https://wanghan.pro
 4 | #ifndef _LASER_PROCESSING_CLASS_H_
 5 | #define _LASER_PROCESSING_CLASS_H_
 6 | 
 7 | #include <pcl/point_cloud.h>
 8 | #include <pcl/point_types.h>
 9 | 
10 | #include <pcl/filters/filter.h>
11 | #include <pcl/filters/voxel_grid.h>
12 | #include <pcl/filters/passthrough.h>
13 | #include <pcl/filters/extract_indices.h>
14 | #include <pcl/filters/crop_box.h>
15 | 
16 | #include "lidar.h"
17 | 
18 | //points covariance class
19 | class Double2d{
20 | public:
21 | 	int id;
22 | 	double value;
23 | 	Double2d(int id_in, double value_in);
24 | };
25 | //points info class
26 | class PointsInfo{
27 | public:
28 | 	int layer;
29 | 	double time;
30 | 	PointsInfo(int layer_in, double time_in);
31 | };
32 | 
33 | 
34 | class LaserProcessingClass 
35 | {
36 |     public:
37 |     	LaserProcessingClass();
38 | 		void init(lidar::Lidar lidar_param_in);
39 | 		void featureExtraction(const pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_in, pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_out_edge, pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_out_surf);
40 | 		void featureExtractionFromSector(const pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_in, std::vector<Double2d>& cloudCurvature, pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_out_edge, pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_out_surf);	
41 | 	private:
42 |      	lidar::Lidar lidar_param;
43 | };
44 | 
45 | 
46 | 
47 | #endif // _LASER_PROCESSING_CLASS_H_
48 | 
49 | 


--------------------------------------------------------------------------------
/SC-A-LOAM/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 <ctime>
 7 | #include <cstdlib>
 8 | #include <chrono>
 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<double> elapsed_seconds = end - start;
27 |         return elapsed_seconds.count() * 1000;
28 |     }
29 | 
30 |   private:
31 |     std::chrono::time_point<std::chrono::system_clock> 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<double> 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<std::chrono::system_clock> start, end;
68 |     bool disp_ = false;
69 | };


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # SC-FLOAM
 2 | 
 3 | ## What is SC-FLOAM? 
 4 | - A real-time LiDAR SLAM package that integrates FLOAM and ScanContext. 
 5 |     - **FLOAM** for odometry (i.e., consecutive motion estimation)
 6 |     - **ScanContext** for coarse global localization that can deal with big drifts (i.e., place recognition as kidnapped robot problem without initial pose)
 7 |     - and iSAM2 of GTSAM is used for pose-graph optimization. 
 8 | 
 9 | ## Dependencies
10 | - We mainly depend on ROS, Ceres and PCL(for FLOAM), and GTSAM (for pose-graph optimization). 
11 |     - For the details to install the prerequisites, please follow the FLOAM and LIO-SAM repositiory. 
12 | - The below examples are done under ROS melodic (ubuntu 18) and GTSAM version 4.x. 
13 | 
14 | ## How to use? 
15 | - First, install the abovementioned dependencies, and follow below lines. 
16 | ```
17 |     mkdir -p ~/catkin_scfloam_ws/src
18 |     cd ~/catkin_scfloam_ws/src
19 |     git clone https://github.com/cuge1995/SC-FLOAM.git
20 |     cd ../
21 |     catkin_make
22 |     source ~/catkin_scfloam_ws/devel/setup.bash
23 | ```
24 | 
25 | ## Example Results 
26 | 
27 | 
28 | ### KITTI 
29 | - For KITTI (HDL-64 sensor), run using the command (Download [KITTI sequence 05](https://drive.google.com/file/d/1eyO0Io3lX2z-yYsfGHawMKZa5Z0uYJ0W/view?usp=sharing) or [KITTI sequence 07](https://drive.google.com/file/d/1_qUfwUw88rEKitUpt1kjswv7Cv4GPs0b/view?usp=sharing) first)
30 |     ```
31 |     ##In different terminal
32 |     roslaunch floam floam_velodyne.launch
33 |     roslaunch aloam_velodyne floam_velodyne_64.launch
34 |     rosbag play 2011_09_30_0018.bag
35 |     ```
36 | 
37 | 
38 | ## Acknowledgements
39 | - Thanks to LOAM, FLOAM, SC-ALOAM and LIO-SAM code authors. The major codes in this repository are borrowed from their efforts.
40 | 
41 | 


--------------------------------------------------------------------------------
/SC-A-LOAM/launch/aloam_velodyne_HDL_64.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |     
 3 |     <param name="scan_line" type="int" value="64" />
 4 | 
 5 |     <!-- if 1, do mapping 10 Hz, if 2, do mapping 5 Hz. Suggest to use 1, it will adjust frequence automaticlly -->
 6 |     <param name="mapping_skip_frame" type="int" value="1" />
 7 | 
 8 |     <!-- remove too closed points -->
 9 |     <param name="minimum_range" type="double" value="5"/>
10 | 
11 |     <param name="mapping_line_resolution" type="double" value="0.4"/> <!-- A-LOAM -->
12 |     <param name="mapping_plane_resolution" type="double" value="0.8"/> <!-- A-LOAM -->
13 | 
14 |     <!-- SC-A-LOAM -->
15 |     <param name="keyframe_meter_gap" type="double" value="1.0"/> 
16 | 
17 |     <!-- Scan Context -->
18 |     <param name="sc_dist_thres" type="double" value="0.2"/> <!-- SC-A-LOAM, if want no outliers, use 0.1-0.15 -->
19 |     <!-- <param name="sc_max_radius" type="double" value="20.0"/> 20 or 40 for indoor -->
20 |     <param name="sc_max_radius" type="double" value="80.0"/> <!-- for outdoor -->
21 | 
22 |     <!--  -->
23 |     <param name="lidar_type" type="string" value="HDL64"/> 
24 |     <remap from="/velodyne_points" to="/velodyne_points"/>
25 | 
26 |     <node pkg="aloam_velodyne" type="ascanRegistration" name="ascanRegistration" output="screen" />
27 |     <node pkg="aloam_velodyne" type="alaserOdometry" name="alaserOdometry" output="screen" />
28 |     <node pkg="aloam_velodyne" type="alaserMapping" name="alaserMapping" output="screen" />
29 |     <node pkg="aloam_velodyne" type="alaserPGO" name="alaserPGO" output="screen" /> <!-- SC-A-LOAM -->
30 | 
31 |     <arg name="rviz" default="true" />
32 |     <group if="$(arg rviz)">
33 |         <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find aloam_velodyne)/rviz_cfg/aloam_velodyne.rviz" />
34 |     </group>
35 | 
36 | </launch>
37 | 


--------------------------------------------------------------------------------
/floam/include/laserMappingClass.h:
--------------------------------------------------------------------------------
 1 | // Author of FLOAM: Wang Han 
 2 | // Email wh200720041@gmail.com
 3 | // Homepage https://wanghan.pro
 4 | 
 5 | #ifndef _LASER_MAPPING_H_
 6 | #define _LASER_MAPPING_H_
 7 | 
 8 | //PCL lib
 9 | #include <pcl/point_cloud.h>
10 | #include <pcl/point_types.h>
11 | #include <pcl/filters/filter.h>
12 | #include <pcl/filters/voxel_grid.h>
13 | #include <pcl/filters/passthrough.h>
14 | #include <pcl_ros/impl/transforms.hpp>
15 | 
16 | //eigen  lib
17 | #include <Eigen/Dense>
18 | #include <Eigen/Geometry>
19 | 
20 | //c++ lib
21 | #include <string>
22 | #include <math.h>
23 | #include <vector>
24 | 
25 | 
26 | #define LASER_CELL_WIDTH 50.0
27 | #define LASER_CELL_HEIGHT 50.0
28 | #define LASER_CELL_DEPTH 50.0
29 | 
30 | //separate map as many sub point clouds
31 | 
32 | #define LASER_CELL_RANGE_HORIZONTAL 2
33 | #define LASER_CELL_RANGE_VERTICAL 2
34 | 
35 | 
36 | class LaserMappingClass 
37 | {
38 | 
39 |     public:
40 |     	LaserMappingClass();
41 | 		void init(double map_resolution);
42 | 		void updateCurrentPointsToMap(const pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_in, const Eigen::Isometry3d& pose_current);
43 | 		pcl::PointCloud<pcl::PointXYZI>::Ptr getMap(void);
44 | 
45 | 	private:
46 | 		int origin_in_map_x;
47 | 		int origin_in_map_y;
48 | 		int origin_in_map_z;
49 | 		int map_width;
50 | 		int map_height;
51 | 		int map_depth;
52 | 		std::vector<std::vector<std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>>> map;
53 | 		pcl::VoxelGrid<pcl::PointXYZI> downSizeFilter;
54 | 		
55 | 		void addWidthCellNegative(void);
56 | 		void addWidthCellPositive(void);
57 | 		void addHeightCellNegative(void);
58 | 		void addHeightCellPositive(void);
59 | 		void addDepthCellNegative(void);
60 | 		void addDepthCellPositive(void);
61 | 		void checkPoints(int& x, int& y, int& z);
62 | 
63 | };
64 | 
65 | 
66 | #endif // _LASER_MAPPING_H_
67 | 
68 | 


--------------------------------------------------------------------------------
/floam/launch/floam_velodyne.launch:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <launch>
 3 | 
 4 |     <!-- launch your velodyne here -->
 5 |     <include file="$(find velodyne_pointcloud)/launch/VLP16_points.launch"/>
 6 |     
 7 |     <!-- For Velodyne VLP-16 -->
 8 |     <!-- <param name="scan_line" value="16" /> -->
 9 |     
10 | 
11 |     <!-- For Velodyne HDL-32  -->
12 |     <!-- <param name="scan_line" value="32" /> -->
13 |    
14 | 
15 |     <!-- For Velodyne HDL-64 -->
16 |     <param name="scan_line" value="64" />
17 | 
18 |     <!--- Sim Time -->
19 |     <param name="scan_period" value="0.1" />
20 | 
21 |     <param name="vertical_angle" type="double" value="2.0" />
22 |     <param name="map_resolution" type="double" value="0.2" />
23 |     <param name="max_dis" type="double" value="90.0" />
24 |     <param name="min_dis" type="double" value="0.5" />
25 |     <!--- -->
26 |     <node pkg="floam" type="floam_odom_estimation_node" name="floam_odom_estimation_node" output="screen"/>
27 |     <node pkg="floam" type="floam_laser_processing_node" name="floam_laser_processing_node" output="screen"/>
28 |     <node pkg="floam" type="floam_laser_mapping_node" name="floam_laser_mapping_node" output="screen"/>
29 | 
30 |     <node pkg="tf" type="static_transform_publisher" name="word2map_tf"  args="0 0 0 0 0 0 /world /map 10" />
31 |     
32 |     <!-- <arg name="rviz" default="true" />
33 |     <group if="$(arg rviz)">
34 |         <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find floam)/rviz/floam_velodyne.rviz" />
35 |     </group> -->
36 | 
37 |     <node pkg="hector_trajectory_server" type="hector_trajectory_server" name="trajectory_server_loam" ns="base_link" >
38 |         <param name="/target_frame_name" value="world" />
39 |         <param name="/source_frame_name" value="base_link" />
40 |         <param name="/trajectory_update_rate" value="10.0" />
41 |         <param name="/trajectory_publish_rate" value="10.0" />
42 |     </node>
43 | 
44 | </launch>
45 | 


--------------------------------------------------------------------------------
/SC-A-LOAM/launch/aloam_velodyne_VLP_16.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |     
 3 |     <param name="scan_line" type="int" value="16" />
 4 | 
 5 |     <!-- if 1, do mapping 10 Hz, if 2, do mapping 5 Hz. Suggest to use 1, it will adjust frequence automaticlly -->
 6 |     <param name="mapping_skip_frame" type="int" value="1" />
 7 | 
 8 |     <!-- remove too closed points -->
 9 |     <param name="minimum_range" type="double" value="0.1"/>
10 | 
11 |     <param name="mapping_line_resolution" type="double" value="0.2"/>
12 |     <param name="mapping_plane_resolution" type="double" value="0.4"/>
13 | 
14 |     <param name="mapviz_filter_size" type="double" value="0.1"/>
15 | 
16 |     <!-- SC-A-LOAM -->
17 |     <param name="keyframe_meter_gap" type="double" value="1.0"/> 
18 |     <param name="keyframe_deg_gap" type="double" value="10.0"/> 
19 | 
20 |     <!-- Scan Context -->
21 |     <param name="sc_dist_thres" type="double" value="0.2"/> <!-- SC-A-LOAM, if want no outliers, use 0.1-0.15 -->
22 |     <param name="sc_max_radius" type="double" value="40.0"/> <!-- 20 or 40 for indoor -->
23 |     <!-- <param name="sc_max_radius" type="double" value="80.0"/> for outdoor -->
24 | 
25 |     <!--  -->
26 |     <param name="lidar_type" type="string" value="VLP16"/> 
27 |     <remap from="/velodyne_points" to="/velodyne_points"/>
28 |     
29 |     <!-- nodes -->
30 |     <node pkg="aloam_velodyne" type="ascanRegistration" name="ascanRegistration" output="screen" />
31 |     <node pkg="aloam_velodyne" type="alaserOdometry" name="alaserOdometry" output="screen" />
32 |     <node pkg="aloam_velodyne" type="alaserMapping" name="alaserMapping" output="screen" />
33 |     <node pkg="aloam_velodyne" type="alaserPGO" name="alaserPGO" output="screen" /> <!-- SC-A-LOAM -->
34 | 
35 |     <!-- visulaization -->
36 |     <arg name="rviz" default="true" />
37 |     <group if="$(arg rviz)">
38 |         <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find aloam_velodyne)/rviz_cfg/aloam_velodyne.rviz" />
39 |     </group>
40 | 
41 | </launch>
42 | 


--------------------------------------------------------------------------------
/SC-A-LOAM/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 | 


--------------------------------------------------------------------------------
/floam/include/lidarOptimization.h:
--------------------------------------------------------------------------------
 1 | // Author of FLOAM: Wang Han 
 2 | // Email wh200720041@gmail.com
 3 | // Homepage https://wanghan.pro
 4 | #ifndef _LIDAR_OPTIMIZATION_ANALYTIC_H_
 5 | #define _LIDAR_OPTIMIZATION_ANALYTIC_H_
 6 | 
 7 | #include <ceres/ceres.h>
 8 | #include <ceres/rotation.h>
 9 | #include <Eigen/Dense>
10 | #include <Eigen/Geometry>
11 | 
12 | void getTransformFromSe3(const Eigen::Matrix<double,6,1>& se3, Eigen::Quaterniond& q, Eigen::Vector3d& t);
13 | 
14 | Eigen::Matrix3d skew(Eigen::Vector3d& mat_in);
15 | 
16 | class EdgeAnalyticCostFunction : public ceres::SizedCostFunction<1, 7> {
17 | 	public:
18 | 
19 | 		EdgeAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, Eigen::Vector3d last_point_b_);
20 | 		virtual ~EdgeAnalyticCostFunction() {}
21 | 		virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;
22 | 
23 | 		Eigen::Vector3d curr_point;
24 | 		Eigen::Vector3d last_point_a;
25 | 		Eigen::Vector3d last_point_b;
26 | };
27 | 
28 | class SurfNormAnalyticCostFunction : public ceres::SizedCostFunction<1, 7> {
29 | 	public:
30 | 		SurfNormAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, double negative_OA_dot_norm_);
31 | 		virtual ~SurfNormAnalyticCostFunction() {}
32 | 		virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;
33 | 
34 | 		Eigen::Vector3d curr_point;
35 | 		Eigen::Vector3d plane_unit_norm;
36 | 		double negative_OA_dot_norm;
37 | };
38 | 
39 | class PoseSE3Parameterization : public ceres::LocalParameterization {
40 | public:
41 | 	
42 |     PoseSE3Parameterization() {}
43 |     virtual ~PoseSE3Parameterization() {}
44 |     virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const;
45 |     virtual bool ComputeJacobian(const double* x, double* jacobian) const;
46 |     virtual int GlobalSize() const { return 7; }
47 |     virtual int LocalSize() const { return 6; }
48 | };
49 | 
50 | 
51 | 
52 | #endif // _LIDAR_OPTIMIZATION_ANALYTIC_H_
53 | 
54 | 


--------------------------------------------------------------------------------
/SC-A-LOAM/launch/aloam_mulran.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |     
 3 |     <param name="scan_line" type="int" value="64" />
 4 | 
 5 |     <!-- if 1, do mapping 10 Hz, if 2, do mapping 5 Hz. Suggest to use 1, it will adjust frequence automaticlly -->
 6 |     <param name="mapping_skip_frame" type="int" value="1" />
 7 | 
 8 |     <!-- remove too closed points -->
 9 |     <param name="minimum_range" type="double" value="0.5"/>
10 | 
11 |     <param name="mapping_line_resolution" type="double" value="0.4"/> <!-- A-LOAM -->
12 |     <param name="mapping_plane_resolution" type="double" value="0.8"/> <!-- A-LOAM -->
13 | 
14 |     <!-- SC-A-LOAM -->
15 |     <param name="keyframe_meter_gap" type="double" value="1.0"/> 
16 | 
17 |     <!-- Scan Context -->
18 |     <param name="sc_dist_thres" type="double" value="0.2"/> <!-- SC-A-LOAM, if want no outliers, use 0.1-0.15 -->
19 |     <!-- <param name="sc_max_radius" type="double" value="40.0"/> 20 or 40 for indoor -->
20 |     <param name="sc_max_radius" type="double" value="80.0"/> <!-- for outdoor -->
21 | 
22 |     <!-- for MulRan -->
23 |     <param name="lidar_type" type="string" value="OS1-64"/> 
24 |     <remap from="/velodyne_points" to="/os1_points"/>
25 | 
26 |     <!-- utils -->
27 |     <param name="save_directory" type="string" value="/home/user/Documents/catkin2021/catkin_scaloam_util/data/"/>  <!-- CHANGE THIS and end with / -->
28 | 
29 |     <!-- nodes -->
30 |     <node pkg="aloam_velodyne" type="ascanRegistration" name="ascanRegistration" output="screen" /> <!-- A-LOAM -->
31 |     <node pkg="aloam_velodyne" type="alaserOdometry" name="alaserOdometry" output="screen" /> <!-- A-LOAM -->
32 |     <node pkg="aloam_velodyne" type="alaserMapping" name="alaserMapping" output="screen" /> <!-- A-LOAM -->
33 |     <node pkg="aloam_velodyne" type="alaserPGO" name="alaserPGO" output="screen" /> <!-- SC-A-LOAM -->
34 | 
35 |     <!-- visulaization -->
36 |     <arg name="rviz" default="true" />
37 |     <group if="$(arg rviz)">
38 |         <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find aloam_velodyne)/rviz_cfg/aloam_velodyne.rviz" />
39 |     </group>
40 | 
41 | </launch>
42 | 


--------------------------------------------------------------------------------
/floam/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(floam)
 3 | 
 4 | set(CMAKE_BUILD_TYPE "Release")
 5 | set(CMAKE_CXX_FLAGS "-std=c++11")
 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
 7 | 
 8 | find_package(catkin REQUIRED COMPONENTS
 9 |   geometry_msgs
10 |   nav_msgs
11 |   sensor_msgs
12 |   roscpp
13 |   rospy
14 |   rosbag
15 |   std_msgs
16 |   tf
17 |   eigen_conversions
18 | )
19 | 
20 | find_package(Eigen3)
21 | if(NOT EIGEN3_FOUND)
22 |   # Fallback to cmake_modules
23 |   find_package(cmake_modules REQUIRED)
24 |   find_package(Eigen REQUIRED)
25 |   set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
26 |   set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES})  # Not strictly necessary as Eigen is head only
27 |   # Possibly map additional variables to the EIGEN3_ prefix.
28 | else()
29 |   set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
30 | endif()
31 | find_package(PCL REQUIRED)
32 | find_package(Ceres REQUIRED)
33 | 
34 | include_directories(
35 |   include
36 |   ${catkin_INCLUDE_DIRS} 
37 |   ${PCL_INCLUDE_DIRS}
38 |   ${CERES_INCLUDE_DIRS}
39 |   ${EIGEN3_INCLUDE_DIRS}
40 | )
41 | 
42 | link_directories(
43 |   include
44 |   ${PCL_LIBRARY_DIRS}
45 |   ${CERES_LIBRARY_DIRS}
46 | )
47 | 
48 | 
49 | catkin_package(
50 |   CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs
51 |   DEPENDS EIGEN3 PCL Ceres 
52 |   INCLUDE_DIRS include
53 | )
54 | 
55 | 
56 | 
57 | add_executable(floam_laser_processing_node src/laserProcessingNode.cpp src/laserProcessingClass.cpp src/lidar.cpp)
58 | target_link_libraries(floam_laser_processing_node ${EIGEN3_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES})
59 | 
60 | add_executable(floam_odom_estimation_node src/odomEstimationNode.cpp src/lidarOptimization.cpp src/lidar.cpp src/odomEstimationClass.cpp)
61 | target_link_libraries(floam_odom_estimation_node ${EIGEN3_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES})
62 | 
63 | add_executable(floam_laser_mapping_node src/laserMappingNode.cpp src/laserMappingClass.cpp src/lidar.cpp)
64 | target_link_libraries(floam_laser_mapping_node ${EIGEN3_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES})
65 | 
66 | 


--------------------------------------------------------------------------------
/SC-A-LOAM/launch/floam_velodyne_32.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |     
 3 |     <param name="scan_line" type="int" value="32" />
 4 | 
 5 |     <!-- if 1, do mapping 10 Hz, if 2, do mapping 5 Hz. Suggest to use 1, it will adjust frequence automaticlly -->
 6 |     <param name="mapping_skip_frame" type="int" value="1" />
 7 | 
 8 |     <!-- remove too closed points -->
 9 |     <param name="minimum_range" type="double" value="0.3"/>
10 | 
11 |     <param name="mapping_line_resolution" type="double" value="0.2"/>
12 |     <param name="mapping_plane_resolution" type="double" value="0.4"/>
13 | 
14 |     <!-- SC-A-LOAM -->
15 |     <param name="keyframe_meter_gap" type="double" value="1.0"/> 
16 | 
17 |     <!-- Scan Context -->
18 |     <param name="sc_dist_thres" type="double" value="0.2"/> <!-- SC-A-LOAM, if want no outliers, use 0.1-0.15 -->
19 |     <!-- <param name="sc_max_radius" type="double" value="20.0"/> 20 or 40 for indoor -->
20 |     <param name="sc_max_radius" type="double" value="80.0"/> <!-- for outdoor -->
21 | 
22 |     <!--  -->
23 |     <param name="lidar_type" type="string" value="HDL32"/>
24 |     <remap from="/velodyne_points" to="/velodyne_points"/>
25 | 
26 |     <!-- input from FLOAM -->
27 |     <remap from="/aft_mapped_to_init" to="/odom"/>
28 |     <remap from="/velodyne_cloud_registered_local" to="/velodyne_points_filtered"/>
29 |     <remap from="/cloud_for_scancontext" to="/velodyne_points_filtered"/>   <!-- because ScanContext requires lidar-ego-centric coordinate for the better performance -->
30 | 
31 | 
32 |     <!-- <node pkg="aloam_velodyne" type="ascanRegistration" name="ascanRegistration" output="screen" />
33 |     <node pkg="aloam_velodyne" type="alaserOdometry" name="alaserOdometry" output="screen" />
34 |     <node pkg="aloam_velodyne" type="alaserMapping" name="alaserMapping" output="screen" /> -->
35 |     <node pkg="aloam_velodyne" type="alaserPGO" name="alaserPGO" output="screen" /> <!-- SC-A-LOAM -->
36 | 
37 |     <arg name="rviz" default="true" />
38 |     <group if="$(arg rviz)">
39 |         <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find aloam_velodyne)/rviz_cfg/aloam_velodyne.rviz" />
40 |     </group>
41 | 
42 | </launch>
43 | 


--------------------------------------------------------------------------------
/SC-A-LOAM/launch/floam_velodyne_64.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |     
 3 |     <param name="scan_line" type="int" value="64" />
 4 | 
 5 |     <!-- if 1, do mapping 10 Hz, if 2, do mapping 5 Hz. Suggest to use 1, it will adjust frequence automaticlly -->
 6 |     <param name="mapping_skip_frame" type="int" value="1" />
 7 | 
 8 |     <!-- remove too closed points -->
 9 |     <param name="minimum_range" type="double" value="0.3"/>
10 | 
11 |     <param name="mapping_line_resolution" type="double" value="0.2"/>
12 |     <param name="mapping_plane_resolution" type="double" value="0.4"/>
13 | 
14 |     <!-- SC-A-LOAM -->
15 |     <param name="keyframe_meter_gap" type="double" value="1.0"/> 
16 | 
17 |     <!-- Scan Context -->
18 |     <param name="sc_dist_thres" type="double" value="0.2"/> <!-- SC-A-LOAM, if want no outliers, use 0.1-0.15 -->
19 |     <!-- <param name="sc_max_radius" type="double" value="20.0"/> 20 or 40 for indoor -->
20 |     <param name="sc_max_radius" type="double" value="80.0"/> <!-- for outdoor -->
21 | 
22 |     <!--  -->
23 |     <param name="lidar_type" type="string" value="HDL64"/>
24 |     <remap from="/velodyne_points" to="/velodyne_points"/>
25 | 
26 |     <!-- input from FLOAM -->
27 |     <remap from="/aft_mapped_to_init" to="/odom"/>
28 |     <remap from="/velodyne_cloud_registered_local" to="/velodyne_points_filtered"/>
29 |     <remap from="/cloud_for_scancontext" to="/velodyne_points_filtered"/>   <!-- because ScanContext requires lidar-ego-centric coordinate for the better performance -->
30 | 
31 | 
32 |     <!-- <node pkg="aloam_velodyne" type="ascanRegistration" name="ascanRegistration" output="screen" />
33 |     <node pkg="aloam_velodyne" type="alaserOdometry" name="alaserOdometry" output="screen" />
34 |     <node pkg="aloam_velodyne" type="alaserMapping" name="alaserMapping" output="screen" /> -->
35 |     <node pkg="aloam_velodyne" type="alaserPGO" name="alaserPGO" output="screen" /> <!-- SC-A-LOAM -->
36 | 
37 |     <arg name="rviz" default="true" />
38 |     <group if="$(arg rviz)">
39 |         <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find aloam_velodyne)/rviz_cfg/aloam_velodyne.rviz" />
40 |     </group>
41 | 
42 | </launch>
43 | 


--------------------------------------------------------------------------------
/floam/launch/floam.launch:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <launch>
 3 | 
 4 |     
 5 |     <node pkg="rosbag" type="play" name="rosbag_play" args="--clock $(env HOME)/Downloads/2011_09_30_0018.bag"/> 
 6 | 
 7 |     <!-- For Velodyne VLP-16 
 8 |     <param name="scan_line" value="16" />
 9 |     -->
10 | 
11 |     <!-- For Velodyne HDL-32 
12 |     <param name="scan_line" value="32" />
13 |     -->
14 | 
15 |     <!-- For Velodyne HDL-64 -->
16 |     <param name="scan_line" value="64" />
17 | 
18 | 
19 |     <!--- Sim Time -->
20 |     <param name="/use_sim_time" value="true" />
21 |     <param name="scan_period" value="0.1" /> 
22 |     <param name="vertical_angle" type="double" value="2.0" />
23 |     <param name="max_dis" type="double" value="90.0" />
24 |     <param name="min_dis" type="double" value="3.0" />
25 | 
26 |     <!--- -->
27 |     <node pkg="floam" type="floam_odom_estimation_node" name="floam_odom_estimation_node" output="screen"/>
28 |     <node pkg="floam" type="floam_laser_processing_node" name="floam_laser_processing_node" output="screen"/>
29 |     
30 |     <node pkg="tf" type="static_transform_publisher" name="word2map_tf"  args="0 0 0 0 0 0 /world /map 10" />
31 |     <arg name="rviz" default="true" />
32 |     <group if="$(arg rviz)">
33 |         <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find floam)/rviz/floam.rviz" />
34 |     </group>
35 | 
36 |     
37 |   	<node pkg="hector_trajectory_server" type="hector_trajectory_server" name="trajectory_server_loam" ns="gt" >
38 |         <param name="/target_frame_name" value="world" />
39 |         <param name="/source_frame_name" value="velodyne" />
40 |         <param name="/trajectory_update_rate" value="10.0" />
41 |         <param name="/trajectory_publish_rate" value="10.0" />
42 |     </node>
43 |     <node pkg="hector_trajectory_server" type="hector_trajectory_server" name="trajectory_server_loam" ns="base_link" >
44 |         <param name="/target_frame_name" value="world" />
45 |         <param name="/source_frame_name" value="base_link" />
46 |         <param name="/trajectory_update_rate" value="10.0" />
47 |         <param name="/trajectory_publish_rate" value="10.0" />
48 |     </node>
49 | 
50 | </launch>
51 | 


--------------------------------------------------------------------------------
/floam/launch/floam_mapping.launch:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <launch>
 3 | 
 4 |     
 5 |     <node pkg="rosbag" type="play" name="rosbag_play" args="--clock -r 0.5 $(env HOME)/Downloads/2011_09_30_0018.bag"/> 
 6 |     
 7 |     <!-- For Velodyne VLP-16 
 8 |     <param name="scan_line" value="16" />
 9 |     -->
10 | 
11 |     <!-- For Velodyne HDL-32 
12 |     <param name="scan_line" value="32" />
13 |     -->
14 | 
15 |     <!-- For Velodyne HDL-64 -->
16 |     <param name="scan_line" value="64" />
17 |     
18 |     <!--- Sim Time -->
19 |     <param name="/use_sim_time" value="true" />
20 |     <param name="scan_period" value="0.1" />
21 | 
22 |     <param name="vertical_angle" type="double" value="2.0" />
23 |     <param name="max_dis" type="double" value="90.0" />
24 |     <param name="map_resolution" type="double" value="0.4" />
25 |     <param name="min_dis" type="double" value="3.0" />
26 |     <!--- -->
27 |     <node pkg="floam" type="floam_odom_estimation_node" name="floam_odom_estimation_node" output="screen"/>
28 |     <node pkg="floam" type="floam_laser_processing_node" name="floam_laser_processing_node" output="screen"/>
29 |     <node pkg="floam" type="floam_laser_mapping_node" name="floam_laser_mapping_node" output="screen"/>
30 | 
31 |     <node pkg="tf" type="static_transform_publisher" name="word2map_tf"  args="0 0 0 0 0 0 /world /map 10" />
32 |     
33 |     <arg name="rviz" default="true" />
34 |     <group if="$(arg rviz)">
35 |         <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find floam)/rviz/floam_mapping.rviz" />
36 |     </group>
37 | 
38 |     
39 |   	<node pkg="hector_trajectory_server" type="hector_trajectory_server" name="trajectory_server_loam" ns="gt" >
40 |         <param name="/target_frame_name" value="world" />
41 |         <param name="/source_frame_name" value="velodyne" />
42 |         <param name="/trajectory_update_rate" value="10.0" />
43 |         <param name="/trajectory_publish_rate" value="10.0" />
44 |     </node>
45 |     <node pkg="hector_trajectory_server" type="hector_trajectory_server" name="trajectory_server_loam" ns="base_link" >
46 |         <param name="/target_frame_name" value="world" />
47 |         <param name="/source_frame_name" value="base_link" />
48 |         <param name="/trajectory_update_rate" value="10.0" />
49 |         <param name="/trajectory_publish_rate" value="10.0" />
50 |     </node>
51 | 
52 | </launch>
53 | 


--------------------------------------------------------------------------------
/SC-A-LOAM/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 <cmath>
40 | 
41 | #include <pcl/point_types.h>
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 | };


--------------------------------------------------------------------------------
/floam/include/odomEstimationClass.h:
--------------------------------------------------------------------------------
 1 | // Author of FLOAM: Wang Han 
 2 | // Email wh200720041@gmail.com
 3 | // Homepage https://wanghan.pro
 4 | #ifndef _ODOM_ESTIMATION_CLASS_H_
 5 | #define _ODOM_ESTIMATION_CLASS_H_
 6 | 
 7 | //std lib
 8 | #include <string>
 9 | #include <math.h>
10 | #include <vector>
11 | 
12 | //PCL
13 | #include <pcl/point_cloud.h>
14 | #include <pcl/point_types.h>
15 | #include <pcl/filters/filter.h>
16 | #include <pcl/filters/voxel_grid.h>
17 | #include <pcl/filters/passthrough.h>
18 | #include <pcl/kdtree/kdtree_flann.h>
19 | #include <pcl/filters/statistical_outlier_removal.h>
20 | #include <pcl/filters/extract_indices.h>
21 | #include <pcl/filters/crop_box.h>
22 | 
23 | //ceres
24 | #include <ceres/ceres.h>
25 | #include <ceres/rotation.h>
26 | 
27 | //eigen
28 | #include <Eigen/Dense>
29 | #include <Eigen/Geometry>
30 | 
31 | //LOCAL LIB
32 | #include "lidar.h"
33 | #include "lidarOptimization.h"
34 | #include <ros/ros.h>
35 | 
36 | class OdomEstimationClass 
37 | {
38 | 
39 |     public:
40 |     	OdomEstimationClass();
41 |     	
42 | 		void init(lidar::Lidar lidar_param, double map_resolution);	
43 | 		void initMapWithPoints(const pcl::PointCloud<pcl::PointXYZI>::Ptr& edge_in, const pcl::PointCloud<pcl::PointXYZI>::Ptr& surf_in);
44 | 		void updatePointsToMap(const pcl::PointCloud<pcl::PointXYZI>::Ptr& edge_in, const pcl::PointCloud<pcl::PointXYZI>::Ptr& surf_in);
45 | 		void getMap(pcl::PointCloud<pcl::PointXYZI>::Ptr& laserCloudMap);
46 | 
47 | 		Eigen::Isometry3d odom;
48 | 		pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCornerMap;
49 | 		pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudSurfMap;
50 | 	private:
51 | 		//optimization variable
52 | 		double parameters[7] = {0, 0, 0, 1, 0, 0, 0};
53 | 		Eigen::Map<Eigen::Quaterniond> q_w_curr = Eigen::Map<Eigen::Quaterniond>(parameters);
54 | 		Eigen::Map<Eigen::Vector3d> t_w_curr = Eigen::Map<Eigen::Vector3d>(parameters + 4);
55 | 
56 | 		Eigen::Isometry3d last_odom;
57 | 
58 | 		//kd-tree
59 | 		pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeEdgeMap;
60 | 		pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeSurfMap;
61 | 
62 | 		//points downsampling before add to map
63 | 		pcl::VoxelGrid<pcl::PointXYZI> downSizeFilterEdge;
64 | 		pcl::VoxelGrid<pcl::PointXYZI> downSizeFilterSurf;
65 | 
66 | 		//local map
67 | 		pcl::CropBox<pcl::PointXYZI> cropBoxFilter;
68 | 
69 | 		//optimization count 
70 | 		int optimization_count;
71 | 
72 | 		//function
73 | 		void addEdgeCostFactor(const pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_in, const pcl::PointCloud<pcl::PointXYZI>::Ptr& map_in, ceres::Problem& problem, ceres::LossFunction *loss_function);
74 | 		void addSurfCostFactor(const pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_in, const pcl::PointCloud<pcl::PointXYZI>::Ptr& map_in, ceres::Problem& problem, ceres::LossFunction *loss_function);
75 | 		void addPointsToMap(const pcl::PointCloud<pcl::PointXYZI>::Ptr& downsampledEdgeCloud, const pcl::PointCloud<pcl::PointXYZI>::Ptr& downsampledSurfCloud);
76 | 		void pointAssociateToMap(pcl::PointXYZI const *const pi, pcl::PointXYZI *const po);
77 | 		void downSamplingToMap(const pcl::PointCloud<pcl::PointXYZI>::Ptr& edge_pc_in, pcl::PointCloud<pcl::PointXYZI>::Ptr& edge_pc_out, const pcl::PointCloud<pcl::PointXYZI>::Ptr& surf_pc_in, pcl::PointCloud<pcl::PointXYZI>::Ptr& surf_pc_out);
78 | };
79 | 
80 | #endif // _ODOM_ESTIMATION_CLASS_H_
81 | 
82 | 


--------------------------------------------------------------------------------
/floam/src/laserMappingNode.cpp:
--------------------------------------------------------------------------------
  1 | // Author of FLOAM: Wang Han 
  2 | // Email wh200720041@gmail.com
  3 | // Homepage https://wanghan.pro
  4 | 
  5 | //c++ lib
  6 | #include <cmath>
  7 | #include <vector>
  8 | #include <mutex>
  9 | #include <queue>
 10 | #include <thread>
 11 | #include <chrono>
 12 | 
 13 | //ros lib
 14 | #include <ros/ros.h>
 15 | #include <sensor_msgs/PointCloud2.h>
 16 | #include <nav_msgs/Odometry.h>
 17 | #include <tf/transform_datatypes.h>
 18 | #include <tf/transform_broadcaster.h>
 19 | 
 20 | //pcl lib
 21 | #include <pcl_conversions/pcl_conversions.h>
 22 | #include <pcl/point_cloud.h>
 23 | #include <pcl/point_types.h>
 24 | 
 25 | //local lib
 26 | #include "laserMappingClass.h"
 27 | #include "lidar.h"
 28 | 
 29 | 
 30 | LaserMappingClass laserMapping;
 31 | lidar::Lidar lidar_param;
 32 | std::mutex mutex_lock;
 33 | std::queue<nav_msgs::OdometryConstPtr> odometryBuf;
 34 | std::queue<sensor_msgs::PointCloud2ConstPtr> pointCloudBuf;
 35 | 
 36 | ros::Publisher map_pub;
 37 | void odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
 38 | {
 39 |     mutex_lock.lock();
 40 |     odometryBuf.push(msg);
 41 |     mutex_lock.unlock();
 42 | }
 43 | 
 44 | void velodyneHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
 45 | {
 46 |     mutex_lock.lock();
 47 |     pointCloudBuf.push(laserCloudMsg);
 48 |     mutex_lock.unlock();
 49 | }
 50 | 
 51 | 
 52 | void laser_mapping(){
 53 |     while(1){
 54 |         if(!odometryBuf.empty() && !pointCloudBuf.empty()){
 55 | 
 56 |             //read data
 57 |             mutex_lock.lock();
 58 |             if(!pointCloudBuf.empty() && pointCloudBuf.front()->header.stamp.toSec()<odometryBuf.front()->header.stamp.toSec()-0.5*lidar_param.scan_period){
 59 |                 ROS_WARN("time stamp unaligned error and pointcloud discarded, pls check your data --> laser mapping node"); 
 60 |                 pointCloudBuf.pop();
 61 |                 mutex_lock.unlock();
 62 |                 continue;              
 63 |             }
 64 | 
 65 |             if(!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < pointCloudBuf.front()->header.stamp.toSec()-0.5*lidar_param.scan_period){
 66 |                 odometryBuf.pop();
 67 |                 ROS_INFO("time stamp unaligned with path final, pls check your data --> laser mapping node");
 68 |                 mutex_lock.unlock();
 69 |                 continue;  
 70 |             }
 71 | 
 72 |             //if time aligned 
 73 |             pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud_in(new pcl::PointCloud<pcl::PointXYZI>());
 74 |             pcl::fromROSMsg(*pointCloudBuf.front(), *pointcloud_in);
 75 |             ros::Time pointcloud_time = (pointCloudBuf.front())->header.stamp;
 76 | 
 77 |             Eigen::Isometry3d current_pose = Eigen::Isometry3d::Identity();
 78 |             current_pose.rotate(Eigen::Quaterniond(odometryBuf.front()->pose.pose.orientation.w,odometryBuf.front()->pose.pose.orientation.x,odometryBuf.front()->pose.pose.orientation.y,odometryBuf.front()->pose.pose.orientation.z));  
 79 |             current_pose.pretranslate(Eigen::Vector3d(odometryBuf.front()->pose.pose.position.x,odometryBuf.front()->pose.pose.position.y,odometryBuf.front()->pose.pose.position.z));
 80 |             pointCloudBuf.pop();
 81 |             odometryBuf.pop();
 82 |             mutex_lock.unlock();
 83 |             
 84 | 
 85 |             laserMapping.updateCurrentPointsToMap(pointcloud_in,current_pose);
 86 | 
 87 |             pcl::PointCloud<pcl::PointXYZI>::Ptr pc_map = laserMapping.getMap();
 88 |             sensor_msgs::PointCloud2 PointsMsg;
 89 |             pcl::toROSMsg(*pc_map, PointsMsg);
 90 |             PointsMsg.header.stamp = pointcloud_time;
 91 |             PointsMsg.header.frame_id = "map";
 92 |             map_pub.publish(PointsMsg); 
 93 |             
 94 | 
 95 | 
 96 |         }
 97 |         //sleep 2 ms every time
 98 |         std::chrono::milliseconds dura(2);
 99 |         std::this_thread::sleep_for(dura);
100 |     }
101 | }
102 | 
103 | int main(int argc, char **argv)
104 | {
105 |     ros::init(argc, argv, "main");
106 |     ros::NodeHandle nh;
107 | 
108 |     int scan_line = 64;
109 |     double vertical_angle = 2.0;
110 |     double scan_period= 0.1;
111 |     double max_dis = 60.0;
112 |     double min_dis = 2.0;
113 |     double map_resolution = 0.4;
114 |     nh.getParam("/scan_period", scan_period); 
115 |     nh.getParam("/vertical_angle", vertical_angle); 
116 |     nh.getParam("/max_dis", max_dis);
117 |     nh.getParam("/min_dis", min_dis);
118 |     nh.getParam("/scan_line", scan_line);
119 |     nh.getParam("/map_resolution", map_resolution);
120 | 
121 |     lidar_param.setScanPeriod(scan_period);
122 |     lidar_param.setVerticalAngle(vertical_angle);
123 |     lidar_param.setLines(scan_line);
124 |     lidar_param.setMaxDistance(max_dis);
125 |     lidar_param.setMinDistance(min_dis);
126 | 
127 |     laserMapping.init(map_resolution);
128 |     ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points_filtered", 100, velodyneHandler);
129 |     ros::Subscriber subOdometry = nh.subscribe<nav_msgs::Odometry>("/odom", 100, odomCallback);
130 | 
131 |     map_pub = nh.advertise<sensor_msgs::PointCloud2>("/map", 100);
132 |     std::thread laser_mapping_process{laser_mapping};
133 | 
134 |     ros::spin();
135 | 
136 |     return 0;
137 | }
138 | 


--------------------------------------------------------------------------------
/floam/src/laserProcessingNode.cpp:
--------------------------------------------------------------------------------
  1 | // Author of FLOAM: Wang Han 
  2 | // Email wh200720041@gmail.com
  3 | // Homepage https://wanghan.pro
  4 | 
  5 | //c++ lib
  6 | #include <cmath>
  7 | #include <vector>
  8 | #include <mutex>
  9 | #include <queue>
 10 | #include <thread>
 11 | #include <chrono>
 12 | 
 13 | //ros lib
 14 | #include <ros/ros.h>
 15 | #include <sensor_msgs/Imu.h>
 16 | #include <sensor_msgs/PointCloud2.h>
 17 | #include <nav_msgs/Odometry.h>
 18 | #include <tf/transform_datatypes.h>
 19 | #include <tf/transform_broadcaster.h>
 20 | 
 21 | //pcl lib
 22 | #include <pcl_conversions/pcl_conversions.h>
 23 | #include <pcl/point_cloud.h>
 24 | #include <pcl/point_types.h>
 25 | 
 26 | //local lib
 27 | #include "lidar.h"
 28 | #include "laserProcessingClass.h"
 29 | 
 30 | 
 31 | LaserProcessingClass laserProcessing;
 32 | std::mutex mutex_lock;
 33 | std::queue<sensor_msgs::PointCloud2ConstPtr> pointCloudBuf;
 34 | lidar::Lidar lidar_param;
 35 | 
 36 | ros::Publisher pubEdgePoints;
 37 | ros::Publisher pubSurfPoints;
 38 | ros::Publisher pubLaserCloudFiltered;
 39 | 
 40 | void velodyneHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
 41 | {
 42 |     mutex_lock.lock();
 43 |     pointCloudBuf.push(laserCloudMsg);
 44 |     mutex_lock.unlock();
 45 |    
 46 | }
 47 | 
 48 | double total_time =0;
 49 | int total_frame=0;
 50 | 
 51 | void laser_processing(){
 52 |     while(1){
 53 |         if(!pointCloudBuf.empty()){
 54 |             //read data
 55 |             mutex_lock.lock();
 56 |             pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud_in(new pcl::PointCloud<pcl::PointXYZI>());
 57 |             pcl::fromROSMsg(*pointCloudBuf.front(), *pointcloud_in);
 58 |             ros::Time pointcloud_time = (pointCloudBuf.front())->header.stamp;
 59 |             pointCloudBuf.pop();
 60 |             mutex_lock.unlock();
 61 | 
 62 |             pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud_edge(new pcl::PointCloud<pcl::PointXYZI>());          
 63 |             pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud_surf(new pcl::PointCloud<pcl::PointXYZI>());
 64 | 
 65 |             std::chrono::time_point<std::chrono::system_clock> start, end;
 66 |             start = std::chrono::system_clock::now();
 67 |             laserProcessing.featureExtraction(pointcloud_in,pointcloud_edge,pointcloud_surf);
 68 |             end = std::chrono::system_clock::now();
 69 |             std::chrono::duration<float> elapsed_seconds = end - start;
 70 |             total_frame++;
 71 |             float time_temp = elapsed_seconds.count() * 1000;
 72 |             total_time+=time_temp;
 73 |             //ROS_INFO("average laser processing time %f ms \n \n", total_time/total_frame);
 74 | 
 75 |             sensor_msgs::PointCloud2 laserCloudFilteredMsg;
 76 |             pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud_filtered(new pcl::PointCloud<pcl::PointXYZI>());  
 77 |             *pointcloud_filtered+=*pointcloud_edge;
 78 |             *pointcloud_filtered+=*pointcloud_surf;
 79 |             pcl::toROSMsg(*pointcloud_filtered, laserCloudFilteredMsg);
 80 |             laserCloudFilteredMsg.header.stamp = pointcloud_time;
 81 |             laserCloudFilteredMsg.header.frame_id = "base_link";
 82 |             pubLaserCloudFiltered.publish(laserCloudFilteredMsg);
 83 | 
 84 |             sensor_msgs::PointCloud2 edgePointsMsg;
 85 |             pcl::toROSMsg(*pointcloud_edge, edgePointsMsg);
 86 |             edgePointsMsg.header.stamp = pointcloud_time;
 87 |             edgePointsMsg.header.frame_id = "base_link";
 88 |             pubEdgePoints.publish(edgePointsMsg);
 89 | 
 90 | 
 91 |             sensor_msgs::PointCloud2 surfPointsMsg;
 92 |             pcl::toROSMsg(*pointcloud_surf, surfPointsMsg);
 93 |             surfPointsMsg.header.stamp = pointcloud_time;
 94 |             surfPointsMsg.header.frame_id = "base_link";
 95 |             pubSurfPoints.publish(surfPointsMsg);
 96 | 
 97 |         }
 98 |         //sleep 2 ms every time
 99 |         std::chrono::milliseconds dura(2);
100 |         std::this_thread::sleep_for(dura);
101 |     }
102 | }
103 | 
104 | int main(int argc, char **argv)
105 | {
106 |     ros::init(argc, argv, "main");
107 |     ros::NodeHandle nh;
108 | 
109 |     int scan_line = 64;
110 |     double vertical_angle = 2.0;
111 |     double scan_period= 0.1;
112 |     double max_dis = 60.0;
113 |     double min_dis = 2.0;
114 | 
115 |     nh.getParam("/scan_period", scan_period); 
116 |     nh.getParam("/vertical_angle", vertical_angle); 
117 |     nh.getParam("/max_dis", max_dis);
118 |     nh.getParam("/min_dis", min_dis);
119 |     nh.getParam("/scan_line", scan_line);
120 | 
121 |     lidar_param.setScanPeriod(scan_period);
122 |     lidar_param.setVerticalAngle(vertical_angle);
123 |     lidar_param.setLines(scan_line);
124 |     lidar_param.setMaxDistance(max_dis);
125 |     lidar_param.setMinDistance(min_dis);
126 | 
127 |     laserProcessing.init(lidar_param);
128 | 
129 |     ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, velodyneHandler);
130 | 
131 |     pubLaserCloudFiltered = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_points_filtered", 100);
132 | 
133 |     pubEdgePoints = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_edge", 100);
134 | 
135 |     pubSurfPoints = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf", 100); 
136 | 
137 |     std::thread laser_processing_process{laser_processing};
138 | 
139 |     ros::spin();
140 | 
141 |     return 0;
142 | }
143 | 
144 | 


--------------------------------------------------------------------------------
/floam/README.md:
--------------------------------------------------------------------------------
  1 | # FLOAM 
  2 | ## Fast LOAM (Lidar Odometry And Mapping)
  3 | 
  4 | This work is an optimized version of A-LOAM and LOAM with the computational cost reduced by up to 3 times.
  5 | This code is modified from [LOAM](https://github.com/laboshinl/loam_velodyne) and [A-LOAM](https://github.com/HKUST-Aerial-Robotics/A-LOAM) .
  6 | 
  7 | **Modifier:** [Wang Han](http://wanghan.pro), Nanyang Technological University, Singapore
  8 | 
  9 | ## 1. Demo Highlights
 10 | Watch our demo at [Video Link](https://youtu.be/PzZly1SQtng)
 11 | <p align='center'>
 12 | <a href="https://youtu.be/PzZly1SQtng">
 13 | <img width="65%" src="/img/floam_kitti.gif"/>
 14 | </a>
 15 | </p>
 16 | 
 17 | ## 2. Evaluation
 18 | ### 2.1. Computational efficiency evaluation
 19 | Computational efficiency evaluation (based on KITTI dataset):
 20 | Platform: Intel® Core™ i7-8700 CPU @ 3.20GHz 
 21 | | Dataset                                      | ALOAM                      | FLOAM                  |
 22 | |----------------------------------------------|----------------------------|------------------------|
 23 | | `KITTI`                                      | 151ms                      | 59ms                   |
 24 | 
 25 | Localization error:
 26 | | Dataset                                      | ALOAM                      | FLOAM                  |
 27 | |----------------------------------------------|----------------------------|------------------------|
 28 | | `KITTI sequence 00`                          | 0.55%                      | 0.51%                  |
 29 | | `KITTI sequence 02`                          | 3.93%                      | 1.25%                  |
 30 | | `KITTI sequence 05`                          | 1.28%                      | 0.93%                  |
 31 | 
 32 | ### 2.2. localization result
 33 | <p align='center'>
 34 | <img width="65%" src="/img/kitti_example.gif"/>
 35 | </p>
 36 | 
 37 | ### 2.3. mapping result
 38 | <p align='center'>
 39 | <a href="https://youtu.be/w_R0JAymOSs">
 40 | <img width="65%" src="/img/floam_mapping.gif"/>
 41 | </a>
 42 | </p>
 43 | 
 44 | ## 3. Prerequisites
 45 | ### 3.1 **Ubuntu** and **ROS**
 46 | Ubuntu 64-bit 18.04.
 47 | 
 48 | ROS Melodic. [ROS Installation](http://wiki.ros.org/ROS/Installation)
 49 | 
 50 | ### 3.2. **Ceres Solver**
 51 | Follow [Ceres Installation](http://ceres-solver.org/installation.html).
 52 | 
 53 | ### 3.3. **PCL**
 54 | Follow [PCL Installation](http://www.pointclouds.org/downloads/linux.html).
 55 | 
 56 | ### 3.4. **Trajectory visualization**
 57 | For visualization purpose, this package uses hector trajectory sever, you may install the package by 
 58 | ```
 59 | sudo apt-get install ros-melodic-hector-trajectory-server
 60 | ```
 61 | Alternatively, you may remove the hector trajectory server node if trajectory visualization is not needed
 62 | 
 63 | ## 4. Build 
 64 | ### 4.1 Clone repository:
 65 | ```
 66 |     cd ~/catkin_ws/src
 67 |     git clone https://github.com/wh200720041/floam.git
 68 |     cd ..
 69 |     catkin_make
 70 |     source ~/catkin_ws/devel/setup.bash
 71 | ```
 72 | ### 4.2 Download test rosbag
 73 | Download [KITTI sequence 05](https://drive.google.com/file/d/1eyO0Io3lX2z-yYsfGHawMKZa5Z0uYJ0W/view?usp=sharing) or [KITTI sequence 07](https://drive.google.com/file/d/1_qUfwUw88rEKitUpt1kjswv7Cv4GPs0b/view?usp=sharing)
 74 | 
 75 | Unzip compressed file 2011_09_30_0018.zip. If your system does not have unzip. please install unzip by 
 76 | ```
 77 | sudo apt-get install unzip 
 78 | ```
 79 | 
 80 | And this may take a few minutes to unzip the file
 81 | ```
 82 | 	cd ~/Downloads
 83 | 	unzip ~/Downloads/2011_09_30_0018.zip
 84 | ```
 85 | 
 86 | ### 4.3 Launch ROS
 87 | ```
 88 |     roslaunch floam floam.launch
 89 | ```
 90 | if you would like to create the map at the same time, you can run (more cpu cost)
 91 | ```
 92 |     roslaunch floam floam_mapping.launch
 93 | ```
 94 | If the mapping process is slow, you may wish to change the rosbag speed by replacing "--clock -r 0.5" with "--clock -r 0.2" in your launch file, or you can change the map publish frequency manually (default is 10 Hz)
 95 | 
 96 | 
 97 | ## 5. Test on other sequence
 98 | To generate rosbag file of kitti dataset, you may use the tools provided by 
 99 | [kitti_to_rosbag](https://github.com/ethz-asl/kitti_to_rosbag) or [kitti2bag](https://github.com/tomas789/kitti2bag) 
100 | 
101 | ## 6. Test on Velodyne VLP-16 or HDL-32
102 | You may wish to test FLOAM on your own platform and sensor such as VLP-16
103 | You can install the velodyne sensor driver by 
104 | ```
105 | sudo apt-get install ros-melodic-velodyne-pointcloud
106 | ```
107 | launch floam for your own velodyne sensor
108 | ```
109 |     roslaunch floam floam_velodyne.launch
110 | ```
111 | If you are using HDL-32 or other sensor, please change the scan_line in the launch file 
112 | 
113 | 
114 | ## 7.Acknowledgements
115 | Thanks for [A-LOAM](https://github.com/HKUST-Aerial-Robotics/A-LOAM) and LOAM(J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time) and [LOAM_NOTED](https://github.com/cuitaixiang/LOAM_NOTED).
116 | 
117 | 
118 | ## 8. Citation
119 | If you use this work for your research, you may want to cite
120 | ```
121 | @inproceedings{wang2021,
122 |   author={H. {Wang} and C. {Wang} and C. {Chen} and L. {Xie}},
123 |   booktitle={2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 
124 |   title={F-LOAM : Fast LiDAR Odometry and Mapping}, 
125 |   year={2020},
126 |   volume={},
127 |   number={}
128 | }
129 | ```
130 | 


--------------------------------------------------------------------------------
/floam/src/lidarOptimization.cpp:
--------------------------------------------------------------------------------
  1 | // Author of FLOAM: Wang Han 
  2 | // Email wh200720041@gmail.com
  3 | // Homepage https://wanghan.pro
  4 | 
  5 | #include "lidarOptimization.h"
  6 | 
  7 | EdgeAnalyticCostFunction::EdgeAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, Eigen::Vector3d last_point_b_)
  8 |         : curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_){
  9 | 
 10 | }
 11 | 
 12 | bool EdgeAnalyticCostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
 13 | {
 14 |     
 15 |     Eigen::Map<const Eigen::Quaterniond> q_last_curr(parameters[0]);
 16 |     Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[0] + 4);
 17 |     Eigen::Vector3d lp;
 18 |     lp = q_last_curr * curr_point + t_last_curr; 
 19 | 
 20 |     Eigen::Vector3d nu = (lp - last_point_a).cross(lp - last_point_b);
 21 |     Eigen::Vector3d de = last_point_a - last_point_b;
 22 |     double de_norm = de.norm();
 23 |     residuals[0] = nu.norm()/de_norm;
 24 |     
 25 |     if(jacobians != NULL)
 26 |     {
 27 |         if(jacobians[0] != NULL)
 28 |         {
 29 |             Eigen::Matrix3d skew_lp = skew(lp);
 30 |             Eigen::Matrix<double, 3, 6> dp_by_se3;
 31 |             dp_by_se3.block<3,3>(0,0) = -skew_lp;
 32 |             (dp_by_se3.block<3,3>(0, 3)).setIdentity();
 33 |             Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> > J_se3(jacobians[0]);
 34 |             J_se3.setZero();
 35 |             Eigen::Matrix3d skew_de = skew(de);
 36 |             J_se3.block<1,6>(0,0) = - nu.transpose() / nu.norm() * skew_de * dp_by_se3/de_norm;
 37 |       
 38 |         }
 39 |     }  
 40 | 
 41 |     return true;
 42 |  
 43 | }   
 44 | 
 45 | 
 46 | SurfNormAnalyticCostFunction::SurfNormAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, double negative_OA_dot_norm_) 
 47 |                                                         : curr_point(curr_point_), plane_unit_norm(plane_unit_norm_), negative_OA_dot_norm(negative_OA_dot_norm_){
 48 | 
 49 | }
 50 | 
 51 | bool SurfNormAnalyticCostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
 52 | {
 53 |     Eigen::Map<const Eigen::Quaterniond> q_w_curr(parameters[0]);
 54 |     Eigen::Map<const Eigen::Vector3d> t_w_curr(parameters[0] + 4);
 55 |     Eigen::Vector3d point_w = q_w_curr * curr_point + t_w_curr;
 56 |     residuals[0] = plane_unit_norm.dot(point_w) + negative_OA_dot_norm;
 57 | 
 58 |     if(jacobians != NULL)
 59 |     {
 60 |         if(jacobians[0] != NULL)
 61 |         {
 62 |             Eigen::Matrix3d skew_point_w = skew(point_w);
 63 |             Eigen::Matrix<double, 3, 6> dp_by_se3;
 64 |             dp_by_se3.block<3,3>(0,0) = -skew_point_w;
 65 |             (dp_by_se3.block<3,3>(0, 3)).setIdentity();
 66 |             Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> > J_se3(jacobians[0]);
 67 |             J_se3.setZero();
 68 |             J_se3.block<1,6>(0,0) = plane_unit_norm.transpose() * dp_by_se3;
 69 |    
 70 |         }
 71 |     }
 72 |     return true;
 73 | 
 74 | }   
 75 | 
 76 | 
 77 | bool PoseSE3Parameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const
 78 | {
 79 |     Eigen::Map<const Eigen::Vector3d> trans(x + 4);
 80 | 
 81 |     Eigen::Quaterniond delta_q;
 82 |     Eigen::Vector3d delta_t;
 83 |     getTransformFromSe3(Eigen::Map<const Eigen::Matrix<double,6,1>>(delta), delta_q, delta_t);
 84 |     Eigen::Map<const Eigen::Quaterniond> quater(x);
 85 |     Eigen::Map<Eigen::Quaterniond> quater_plus(x_plus_delta);
 86 |     Eigen::Map<Eigen::Vector3d> trans_plus(x_plus_delta + 4);
 87 | 
 88 |     quater_plus = delta_q * quater;
 89 |     trans_plus = delta_q * trans + delta_t;
 90 | 
 91 |     return true;
 92 | }
 93 | 
 94 | bool PoseSE3Parameterization::ComputeJacobian(const double *x, double *jacobian) const
 95 | {
 96 |     Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
 97 |     (j.topRows(6)).setIdentity();
 98 |     (j.bottomRows(1)).setZero();
 99 | 
100 |     return true;
101 | }
102 | 
103 | void getTransformFromSe3(const Eigen::Matrix<double,6,1>& se3, Eigen::Quaterniond& q, Eigen::Vector3d& t){
104 |     Eigen::Vector3d omega(se3.data());
105 |     Eigen::Vector3d upsilon(se3.data()+3);
106 |     Eigen::Matrix3d Omega = skew(omega);
107 | 
108 |     double theta = omega.norm();
109 |     double half_theta = 0.5*theta;
110 | 
111 |     double imag_factor;
112 |     double real_factor = cos(half_theta);
113 |     if(theta<1e-10)
114 |     {
115 |         double theta_sq = theta*theta;
116 |         double theta_po4 = theta_sq*theta_sq;
117 |         imag_factor = 0.5-0.0208333*theta_sq+0.000260417*theta_po4;
118 |     }
119 |     else
120 |     {
121 |         double sin_half_theta = sin(half_theta);
122 |         imag_factor = sin_half_theta/theta;
123 |     }
124 | 
125 |     q = Eigen::Quaterniond(real_factor, imag_factor*omega.x(), imag_factor*omega.y(), imag_factor*omega.z());
126 | 
127 | 
128 |     Eigen::Matrix3d J;
129 |     if (theta<1e-10)
130 |     {
131 |         J = q.matrix();
132 |     }
133 |     else
134 |     {
135 |         Eigen::Matrix3d Omega2 = Omega*Omega;
136 |         J = (Eigen::Matrix3d::Identity() + (1-cos(theta))/(theta*theta)*Omega + (theta-sin(theta))/(pow(theta,3))*Omega2);
137 |     }
138 | 
139 |     t = J*upsilon;
140 | }
141 | 
142 | Eigen::Matrix<double,3,3> skew(Eigen::Matrix<double,3,1>& mat_in){
143 |     Eigen::Matrix<double,3,3> skew_mat;
144 |     skew_mat.setZero();
145 |     skew_mat(0,1) = -mat_in(2);
146 |     skew_mat(0,2) =  mat_in(1);
147 |     skew_mat(1,2) = -mat_in(0);
148 |     skew_mat(1,0) =  mat_in(2);
149 |     skew_mat(2,0) = -mat_in(1);
150 |     skew_mat(2,1) =  mat_in(0);
151 |     return skew_mat;
152 | }


--------------------------------------------------------------------------------
/SC-A-LOAM/include/scancontext/Scancontext.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #include <ctime>
  4 | #include <cassert>
  5 | #include <cmath>
  6 | #include <utility>
  7 | #include <vector>
  8 | #include <algorithm> 
  9 | #include <cstdlib>
 10 | #include <memory>
 11 | #include <iostream>
 12 | 
 13 | #include <Eigen/Dense>
 14 | 
 15 | #include <opencv2/opencv.hpp>
 16 | #include <opencv2/core/eigen.hpp>
 17 | #include <opencv2/highgui/highgui.hpp>
 18 | #include <cv_bridge/cv_bridge.h>
 19 | 
 20 | #include <pcl/point_cloud.h>
 21 | #include <pcl/point_types.h>
 22 | #include <pcl/filters/voxel_grid.h>
 23 | #include <pcl_conversions/pcl_conversions.h>
 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<std::vector<float> >;
 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<float> 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<SCPointType> & _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<double, int> distanceBtnScanContext ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "D" (eq 6) in the original paper (IROS 18)
 69 | 
 70 |     // User-side API
 71 |     void makeAndSaveScancontextAndKeys( pcl::PointCloud<SCPointType> & _scan_down );
 72 |     std::pair<int, float> 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<int, float> detectLoopClosureIDBetweenSession ( std::vector<float>& 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<double> polarcontexts_timestamp_; // optional.
112 |     std::vector<Eigen::MatrixXd> polarcontexts_;
113 |     std::vector<Eigen::MatrixXd> polarcontext_invkeys_;
114 |     std::vector<Eigen::MatrixXd> polarcontext_vkeys_;
115 | 
116 |     KeyMat polarcontext_invkeys_mat_;
117 |     KeyMat polarcontext_invkeys_to_search_;
118 |     std::unique_ptr<InvKeyTree> polarcontext_tree_;
119 | 
120 |     bool is_tree_batch_made = false;
121 |     std::unique_ptr<InvKeyTree> polarcontext_tree_batch_;
122 | 
123 | }; // SCManager
124 | 
125 | // } // namespace SC2
126 | 


--------------------------------------------------------------------------------
/SC-A-LOAM/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 <vector>
 34 | 
 35 | // ===== This example shows how to use nanoflann with these types of containers: =======
 36 | //typedef std::vector<std::vector<double> > my_vector_of_vectors_t;
 37 | //typedef std::vector<Eigen::VectorXd> my_vector_of_vectors_t;   // This requires #include <Eigen/Dense>
 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 <class VectorOfVectorsType, typename num_t = double, int DIM = -1, class Distance = nanoflann::metric_L2, typename IndexType = size_t>
 50 | struct KDTreeVectorOfVectorsAdaptor
 51 | {
 52 | 	typedef KDTreeVectorOfVectorsAdaptor<VectorOfVectorsType,num_t,DIM,Distance> self_t;
 53 | 	typedef typename Distance::template traits<num_t,self_t>::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<int>(dims) != DIM)
 64 | 			throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument");
 65 | 		index = new index_t( static_cast<int>(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<num_t,IndexType> 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 <class BBOX>
111 | 	bool kdtree_get_bbox(BBOX & /*bb*/) const {
112 | 		return false;
113 | 	}
114 | 
115 | 	/** @} */
116 | 
117 | }; // end of KDTreeVectorOfVectorsAdaptor
118 | 


--------------------------------------------------------------------------------
/SC-A-LOAM/utils/python/makeMergedMap.py:
--------------------------------------------------------------------------------
  1 | import os 
  2 | import sys
  3 | import time 
  4 | import copy 
  5 | from io import StringIO
  6 | 
  7 | import pypcd # for the install, use this command: python3.x (use your python ver) -m pip install --user git+https://github.com/DanielPollithy/pypcd.git
  8 | from pypcd import pypcd
  9 | 
 10 | import numpy as np
 11 | from numpy import linalg as LA
 12 | 
 13 | import open3d as o3d
 14 | 
 15 | from pypcdMyUtils import * 
 16 | 
 17 | jet_table = np.load('jet_table.npy')
 18 | bone_table = np.load('bone_table.npy')
 19 | 
 20 | color_table = bone_table
 21 | color_table_len = color_table.shape[0]
 22 | 
 23 | 
 24 | ##########################
 25 | # User only consider this block
 26 | ##########################
 27 | 
 28 | data_dir = "../sample_data/Seosan01/" # should end with / 
 29 | scan_idx_range_to_stack = [0, 20] # if you want a whole map, use [0, len(scan_files)]
 30 | node_skip = 1
 31 | 
 32 | num_points_in_a_scan = 150000 # for reservation (save faster) // e.g., use 150000 for 128 ray lidars, 100000 for 64 ray lidars, 30000 for 16 ray lidars, if error occured, use the larger value.
 33 | 
 34 | is_live_vis = False # recommend to use false 
 35 | is_o3d_vis = True
 36 | intensity_color_max = 200
 37 | 
 38 | is_near_removal = True
 39 | thres_near_removal = 2 # meter (to remove platform-myself structure ghost points)
 40 | 
 41 | ##########################
 42 | 
 43 | 
 44 | #
 45 | scan_dir = data_dir + "Scans"
 46 | scan_files = os.listdir(scan_dir) 
 47 | scan_files.sort()
 48 | 
 49 | poses = []
 50 | f = open(data_dir+"optimized_poses.txt", 'r')
 51 | while True:
 52 |     line = f.readline()
 53 |     if not line: break
 54 |     pose_SE3 = np.asarray([float(i) for i in line.split()])
 55 |     pose_SE3 = np.vstack( (np.reshape(pose_SE3, (3, 4)), np.asarray([0,0,0,1])) )
 56 |     poses.append(pose_SE3)
 57 | f.close()
 58 | 
 59 | 
 60 | #
 61 | assert (scan_idx_range_to_stack[1] > scan_idx_range_to_stack[0])
 62 | print("Merging scans from", scan_idx_range_to_stack[0], "to", scan_idx_range_to_stack[1])
 63 | 
 64 | 
 65 | #
 66 | if(is_live_vis):
 67 |     vis = o3d.visualization.Visualizer() 
 68 |     vis.create_window('Map', visible = True) 
 69 | 
 70 | nodes_count = 0
 71 | pcd_combined_for_vis = o3d.geometry.PointCloud()
 72 | pcd_combined_for_save = None
 73 | 
 74 | # The scans from 000000.pcd should be prepared if it is not used (because below code indexing is designed in a naive way)
 75 | 
 76 | # manually reserve memory for fast write  
 77 | num_all_points_expected = int(num_points_in_a_scan * np.round((scan_idx_range_to_stack[1] - scan_idx_range_to_stack[0])/node_skip))
 78 | 
 79 | np_xyz_all = np.empty([num_all_points_expected, 3])
 80 | np_intensity_all = np.empty([num_all_points_expected, 1])
 81 | curr_count = 0
 82 | 
 83 | for node_idx in range(len(scan_files)):
 84 |     if(node_idx < scan_idx_range_to_stack[0] or node_idx >= scan_idx_range_to_stack[1]):
 85 |         continue
 86 | 
 87 |     nodes_count = nodes_count + 1
 88 |     if( nodes_count % node_skip is not 0): 
 89 |         if(node_idx is not scan_idx_range_to_stack[0]): # to ensure the vis init 
 90 |             continue
 91 | 
 92 |     print("read keyframe scan idx", node_idx)
 93 | 
 94 |     scan_pose = poses[node_idx]
 95 | 
 96 |     scan_path = os.path.join(scan_dir, scan_files[node_idx])
 97 |     scan_pcd = o3d.io.read_point_cloud(scan_path)
 98 |     scan_xyz_local = copy.deepcopy(np.asarray(scan_pcd.points))
 99 | 
100 |     scan_pypcd_with_intensity = pypcd.PointCloud.from_path(scan_path)
101 |     scan_intensity = scan_pypcd_with_intensity.pc_data['intensity']
102 |     scan_intensity_colors_idx = np.round( (color_table_len-1) * np.minimum( 1, np.maximum(0, scan_intensity / intensity_color_max) ) )
103 |     scan_intensity_colors = color_table[scan_intensity_colors_idx.astype(int)]
104 | 
105 |     scan_pcd_global = scan_pcd.transform(scan_pose) # global coord, note that this is not deepcopy
106 |     scan_pcd_global.colors = o3d.utility.Vector3dVector(scan_intensity_colors)
107 |     scan_xyz = np.asarray(scan_pcd_global.points)
108 | 
109 |     scan_intensity = np.expand_dims(scan_intensity, axis=1) 
110 |     scan_ranges = LA.norm(scan_xyz_local, axis=1)
111 | 
112 |     if(is_near_removal):
113 |         eff_idxes = np.where (scan_ranges > thres_near_removal)
114 |         scan_xyz = scan_xyz[eff_idxes[0], :]
115 |         scan_intensity = scan_intensity[eff_idxes[0], :]
116 | 
117 |         scan_pcd_global = scan_pcd_global.select_by_index(eff_idxes[0])
118 | 
119 |     if(is_o3d_vis):
120 |         pcd_combined_for_vis += scan_pcd_global # open3d pointcloud class append is fast 
121 | 
122 |     if is_live_vis:
123 |         if(node_idx is scan_idx_range_to_stack[0]): # to ensure the vis init 
124 |             vis.add_geometry(pcd_combined_for_vis) 
125 | 
126 |         vis.update_geometry(pcd_combined_for_vis)
127 |         vis.poll_events()
128 |         vis.update_renderer()
129 | 
130 |     # save 
131 |     np_xyz_all[curr_count:curr_count + scan_xyz.shape[0], :] = scan_xyz
132 |     np_intensity_all[curr_count:curr_count + scan_xyz.shape[0], :] = scan_intensity
133 | 
134 |     curr_count = curr_count + scan_xyz.shape[0]
135 |     print(curr_count)
136 |  
137 | #
138 | if(is_o3d_vis):
139 |     print("draw the merged map.")
140 |     o3d.visualization.draw_geometries([pcd_combined_for_vis])
141 | 
142 | 
143 | # save ply having intensity
144 | np_xyz_all = np_xyz_all[0:curr_count, :]
145 | np_intensity_all = np_intensity_all[0:curr_count, :]
146 | 
147 | np_xyzi_all = np.hstack( (np_xyz_all, np_intensity_all) )
148 | xyzi = make_xyzi_point_cloud(np_xyzi_all)
149 | 
150 | map_name = data_dir + "map_" + str(scan_idx_range_to_stack[0]) + "_to_" + str(scan_idx_range_to_stack[1]) + "_with_intensity.pcd"
151 | xyzi.save_pcd(map_name, compression='binary_compressed')
152 | print("intensity map is save (path:", map_name, ")")
153 | 
154 | # save rgb colored points 
155 | # map_name = data_dir + "map_" + str(scan_idx_range_to_stack[0]) + "_to_" + str(scan_idx_range_to_stack[1]) + ".pcd"
156 | # o3d.io.write_point_cloud(map_name, pcd_combined_for_vis)
157 | # print("the map is save (path:", map_name, ")")
158 | 
159 | 
160 | 


--------------------------------------------------------------------------------
/SC-A-LOAM/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 |     - <p align="center"><img src="picture/anypipe.png" width=800></p>
 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 | <p align="center"><img src="picture/riverside01.png" width=800></p>
 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 | <p align="center"><img src="picture/kitti05.png" width=800></p>
 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 | <p align="center"><img src="picture/indoor.png" width=800></p>
 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 |     <p align="center"><img src="picture/scfastlio.png" width=600></p>
 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 |     <p align="center"><img src="picture/kaist-03-merged.png" width=800></p>
 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 | 


--------------------------------------------------------------------------------
/SC-A-LOAM/src/lidarFactor.hpp:
--------------------------------------------------------------------------------
  1 | // Author:   Tong Qin               qintonguav@gmail.com
  2 | // 	         Shaozu Cao 		    saozu.cao@connect.ust.hk
  3 | 
  4 | #include <ceres/ceres.h>
  5 | #include <ceres/rotation.h>
  6 | #include <eigen3/Eigen/Dense>
  7 | #include <pcl/point_cloud.h>
  8 | #include <pcl/point_types.h>
  9 | #include <pcl/kdtree/kdtree_flann.h>
 10 | #include <pcl_conversions/pcl_conversions.h>
 11 | 
 12 | struct LidarEdgeFactor
 13 | {
 14 | 	LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,
 15 | 					Eigen::Vector3d last_point_b_, double s_)
 16 | 		: curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {}
 17 | 
 18 | 	template <typename T>
 19 | 	bool operator()(const T *q, const T *t, T *residual) const
 20 | 	{
 21 | 
 22 | 		Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
 23 | 		Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};
 24 | 		Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};
 25 | 
 26 | 		//Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
 27 | 		Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};
 28 | 		Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
 29 | 		q_last_curr = q_identity.slerp(T(s), q_last_curr);
 30 | 		Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};
 31 | 
 32 | 		Eigen::Matrix<T, 3, 1> lp;
 33 | 		lp = q_last_curr * cp + t_last_curr;
 34 | 
 35 | 		Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);
 36 | 		Eigen::Matrix<T, 3, 1> de = lpa - lpb;
 37 | 
 38 | 		residual[0] = nu.x() / de.norm();
 39 | 		residual[1] = nu.y() / de.norm();
 40 | 		residual[2] = nu.z() / de.norm();
 41 | 
 42 | 		return true;
 43 | 	}
 44 | 
 45 | 	static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,
 46 | 									   const Eigen::Vector3d last_point_b_, const double s_)
 47 | 	{
 48 | 		return (new ceres::AutoDiffCostFunction<
 49 | 				LidarEdgeFactor, 3, 4, 3>(
 50 | 			new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));
 51 | 	}
 52 | 
 53 | 	Eigen::Vector3d curr_point, last_point_a, last_point_b;
 54 | 	double s;
 55 | };
 56 | 
 57 | struct LidarPlaneFactor
 58 | {
 59 | 	LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,
 60 | 					 Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_)
 61 | 		: curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_),
 62 | 		  last_point_m(last_point_m_), s(s_)
 63 | 	{
 64 | 		ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);
 65 | 		ljm_norm.normalize();
 66 | 	}
 67 | 
 68 | 	template <typename T>
 69 | 	bool operator()(const T *q, const T *t, T *residual) const
 70 | 	{
 71 | 
 72 | 		Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
 73 | 		Eigen::Matrix<T, 3, 1> lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())};
 74 | 		//Eigen::Matrix<T, 3, 1> lpl{T(last_point_l.x()), T(last_point_l.y()), T(last_point_l.z())};
 75 | 		//Eigen::Matrix<T, 3, 1> lpm{T(last_point_m.x()), T(last_point_m.y()), T(last_point_m.z())};
 76 | 		Eigen::Matrix<T, 3, 1> ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())};
 77 | 
 78 | 		//Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
 79 | 		Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};
 80 | 		Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
 81 | 		q_last_curr = q_identity.slerp(T(s), q_last_curr);
 82 | 		Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};
 83 | 
 84 | 		Eigen::Matrix<T, 3, 1> lp;
 85 | 		lp = q_last_curr * cp + t_last_curr;
 86 | 
 87 | 		residual[0] = (lp - lpj).dot(ljm);
 88 | 
 89 | 		return true;
 90 | 	}
 91 | 
 92 | 	static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_,
 93 | 									   const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_,
 94 | 									   const double s_)
 95 | 	{
 96 | 		return (new ceres::AutoDiffCostFunction<
 97 | 				LidarPlaneFactor, 1, 4, 3>(
 98 | 			new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_)));
 99 | 	}
100 | 
101 | 	Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;
102 | 	Eigen::Vector3d ljm_norm;
103 | 	double s;
104 | };
105 | 
106 | struct LidarPlaneNormFactor
107 | {
108 | 
109 | 	LidarPlaneNormFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_,
110 | 						 double negative_OA_dot_norm_) : curr_point(curr_point_), plane_unit_norm(plane_unit_norm_),
111 | 														 negative_OA_dot_norm(negative_OA_dot_norm_) {}
112 | 
113 | 	template <typename T>
114 | 	bool operator()(const T *q, const T *t, T *residual) const
115 | 	{
116 | 		Eigen::Quaternion<T> q_w_curr{q[3], q[0], q[1], q[2]};
117 | 		Eigen::Matrix<T, 3, 1> t_w_curr{t[0], t[1], t[2]};
118 | 		Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
119 | 		Eigen::Matrix<T, 3, 1> point_w;
120 | 		point_w = q_w_curr * cp + t_w_curr;
121 | 
122 | 		Eigen::Matrix<T, 3, 1> norm(T(plane_unit_norm.x()), T(plane_unit_norm.y()), T(plane_unit_norm.z()));
123 | 		residual[0] = norm.dot(point_w) + T(negative_OA_dot_norm);
124 | 		return true;
125 | 	}
126 | 
127 | 	static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d plane_unit_norm_,
128 | 									   const double negative_OA_dot_norm_)
129 | 	{
130 | 		return (new ceres::AutoDiffCostFunction<
131 | 				LidarPlaneNormFactor, 1, 4, 3>(
132 | 			new LidarPlaneNormFactor(curr_point_, plane_unit_norm_, negative_OA_dot_norm_)));
133 | 	}
134 | 
135 | 	Eigen::Vector3d curr_point;
136 | 	Eigen::Vector3d plane_unit_norm;
137 | 	double negative_OA_dot_norm;
138 | };
139 | 
140 | 
141 | struct LidarDistanceFactor
142 | {
143 | 
144 | 	LidarDistanceFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d closed_point_) 
145 | 						: curr_point(curr_point_), closed_point(closed_point_){}
146 | 
147 | 	template <typename T>
148 | 	bool operator()(const T *q, const T *t, T *residual) const
149 | 	{
150 | 		Eigen::Quaternion<T> q_w_curr{q[3], q[0], q[1], q[2]};
151 | 		Eigen::Matrix<T, 3, 1> t_w_curr{t[0], t[1], t[2]};
152 | 		Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
153 | 		Eigen::Matrix<T, 3, 1> point_w;
154 | 		point_w = q_w_curr * cp + t_w_curr;
155 | 
156 | 
157 | 		residual[0] = point_w.x() - T(closed_point.x());
158 | 		residual[1] = point_w.y() - T(closed_point.y());
159 | 		residual[2] = point_w.z() - T(closed_point.z());
160 | 		return true;
161 | 	}
162 | 
163 | 	static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d closed_point_)
164 | 	{
165 | 		return (new ceres::AutoDiffCostFunction<
166 | 				LidarDistanceFactor, 3, 4, 3>(
167 | 			new LidarDistanceFactor(curr_point_, closed_point_)));
168 | 	}
169 | 
170 | 	Eigen::Vector3d curr_point;
171 | 	Eigen::Vector3d closed_point;
172 | };


--------------------------------------------------------------------------------
/floam/src/odomEstimationNode.cpp:
--------------------------------------------------------------------------------
  1 | // Author of FLOAM: Wang Han 
  2 | // Email wh200720041@gmail.com
  3 | // Homepage https://wanghan.pro
  4 | 
  5 | //c++ lib
  6 | #include <cmath>
  7 | #include <vector>
  8 | #include <mutex>
  9 | #include <queue>
 10 | #include <thread>
 11 | #include <chrono>
 12 | 
 13 | //ros lib
 14 | #include <ros/ros.h>
 15 | #include <sensor_msgs/PointCloud2.h>
 16 | #include <nav_msgs/Odometry.h>
 17 | #include <tf/transform_datatypes.h>
 18 | #include <tf/transform_broadcaster.h>
 19 | 
 20 | //pcl lib
 21 | #include <pcl_conversions/pcl_conversions.h>
 22 | #include <pcl/point_cloud.h>
 23 | #include <pcl/point_types.h>
 24 | 
 25 | //local lib
 26 | #include "lidar.h"
 27 | #include "odomEstimationClass.h"
 28 | 
 29 | OdomEstimationClass odomEstimation;
 30 | std::mutex mutex_lock;
 31 | std::queue<sensor_msgs::PointCloud2ConstPtr> pointCloudEdgeBuf;
 32 | std::queue<sensor_msgs::PointCloud2ConstPtr> pointCloudSurfBuf;
 33 | lidar::Lidar lidar_param;
 34 | 
 35 | ros::Publisher pubLaserOdometry;
 36 | void velodyneSurfHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
 37 | {
 38 |     mutex_lock.lock();
 39 |     pointCloudSurfBuf.push(laserCloudMsg);
 40 |     mutex_lock.unlock();
 41 | }
 42 | void velodyneEdgeHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
 43 | {
 44 |     mutex_lock.lock();
 45 |     pointCloudEdgeBuf.push(laserCloudMsg);
 46 |     mutex_lock.unlock();
 47 | }
 48 | 
 49 | bool is_odom_inited = false;
 50 | double total_time =0;
 51 | int total_frame=0;
 52 | void odom_estimation(){
 53 |     while(1){
 54 |         if(!pointCloudEdgeBuf.empty() && !pointCloudSurfBuf.empty()){
 55 | 
 56 |             //read data
 57 |             mutex_lock.lock();
 58 |             if(!pointCloudSurfBuf.empty() && (pointCloudSurfBuf.front()->header.stamp.toSec()<pointCloudEdgeBuf.front()->header.stamp.toSec()-0.5*lidar_param.scan_period)){
 59 |                 pointCloudSurfBuf.pop();
 60 |                 ROS_WARN_ONCE("time stamp unaligned with extra point cloud, pls check your data --> odom correction");
 61 |                 mutex_lock.unlock();
 62 |                 continue;  
 63 |             }
 64 | 
 65 |             if(!pointCloudEdgeBuf.empty() && (pointCloudEdgeBuf.front()->header.stamp.toSec()<pointCloudSurfBuf.front()->header.stamp.toSec()-0.5*lidar_param.scan_period)){
 66 |                 pointCloudEdgeBuf.pop();
 67 |                 ROS_WARN_ONCE("time stamp unaligned with extra point cloud, pls check your data --> odom correction");
 68 |                 mutex_lock.unlock();
 69 |                 continue;  
 70 |             }
 71 |             //if time aligned 
 72 | 
 73 |             pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud_surf_in(new pcl::PointCloud<pcl::PointXYZI>());
 74 |             pcl::PointCloud<pcl::PointXYZI>::Ptr pointcloud_edge_in(new pcl::PointCloud<pcl::PointXYZI>());
 75 |             pcl::fromROSMsg(*pointCloudEdgeBuf.front(), *pointcloud_edge_in);
 76 |             pcl::fromROSMsg(*pointCloudSurfBuf.front(), *pointcloud_surf_in);
 77 |             ros::Time pointcloud_time = (pointCloudSurfBuf.front())->header.stamp;
 78 |             pointCloudEdgeBuf.pop();
 79 |             pointCloudSurfBuf.pop();
 80 |             mutex_lock.unlock();
 81 | 
 82 |             if(is_odom_inited == false){
 83 |                 odomEstimation.initMapWithPoints(pointcloud_edge_in, pointcloud_surf_in);
 84 |                 is_odom_inited = true;
 85 |                 ROS_INFO("odom inited");
 86 |             }else{
 87 |                 std::chrono::time_point<std::chrono::system_clock> start, end;
 88 |                 start = std::chrono::system_clock::now();
 89 |                 odomEstimation.updatePointsToMap(pointcloud_edge_in, pointcloud_surf_in);
 90 |                 end = std::chrono::system_clock::now();
 91 |                 std::chrono::duration<float> elapsed_seconds = end - start;
 92 |                 total_frame++;
 93 |                 float time_temp = elapsed_seconds.count() * 1000;
 94 |                 total_time+=time_temp;
 95 |                 ROS_INFO("average odom estimation time %f ms \n \n", total_time/total_frame);
 96 |             }
 97 | 
 98 | 
 99 | 
100 |             Eigen::Quaterniond q_current(odomEstimation.odom.rotation());
101 |             //q_current.normalize();
102 |             Eigen::Vector3d t_current = odomEstimation.odom.translation();
103 | 
104 |             static tf::TransformBroadcaster br;
105 |             tf::Transform transform;
106 |             transform.setOrigin( tf::Vector3(t_current.x(), t_current.y(), t_current.z()) );
107 |             tf::Quaternion q(q_current.x(),q_current.y(),q_current.z(),q_current.w());
108 |             transform.setRotation(q);
109 |             br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "base_link"));
110 | 
111 |             // publish odometry
112 |             nav_msgs::Odometry laserOdometry;
113 |             laserOdometry.header.frame_id = "map";
114 |             laserOdometry.child_frame_id = "base_link";
115 |             laserOdometry.header.stamp = pointcloud_time;
116 |             laserOdometry.pose.pose.orientation.x = q_current.x();
117 |             laserOdometry.pose.pose.orientation.y = q_current.y();
118 |             laserOdometry.pose.pose.orientation.z = q_current.z();
119 |             laserOdometry.pose.pose.orientation.w = q_current.w();
120 |             laserOdometry.pose.pose.position.x = t_current.x();
121 |             laserOdometry.pose.pose.position.y = t_current.y();
122 |             laserOdometry.pose.pose.position.z = t_current.z();
123 |             pubLaserOdometry.publish(laserOdometry);
124 | 
125 |         }
126 |         //sleep 2 ms every time
127 |         std::chrono::milliseconds dura(2);
128 |         std::this_thread::sleep_for(dura);
129 |     }
130 | }
131 | 
132 | int main(int argc, char **argv)
133 | {
134 |     ros::init(argc, argv, "main");
135 |     ros::NodeHandle nh;
136 | 
137 |     int scan_line = 64;
138 |     double vertical_angle = 2.0;
139 |     double scan_period= 0.1;
140 |     double max_dis = 60.0;
141 |     double min_dis = 2.0;
142 |     double map_resolution = 0.4;
143 |     nh.getParam("/scan_period", scan_period); 
144 |     nh.getParam("/vertical_angle", vertical_angle); 
145 |     nh.getParam("/max_dis", max_dis);
146 |     nh.getParam("/min_dis", min_dis);
147 |     nh.getParam("/scan_line", scan_line);
148 |     nh.getParam("/map_resolution", map_resolution);
149 | 
150 |     lidar_param.setScanPeriod(scan_period);
151 |     lidar_param.setVerticalAngle(vertical_angle);
152 |     lidar_param.setLines(scan_line);
153 |     lidar_param.setMaxDistance(max_dis);
154 |     lidar_param.setMinDistance(min_dis);
155 | 
156 |     odomEstimation.init(lidar_param, map_resolution);
157 |     ros::Subscriber subEdgeLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_edge", 100, velodyneEdgeHandler);
158 |     ros::Subscriber subSurfLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf", 100, velodyneSurfHandler);
159 | 
160 |     pubLaserOdometry = nh.advertise<nav_msgs::Odometry>("/odom", 100);
161 |     std::thread odom_estimation_process{odom_estimation};
162 | 
163 |     ros::spin();
164 | 
165 |     return 0;
166 | }
167 | 
168 | 


--------------------------------------------------------------------------------
/floam/src/laserMappingClass.cpp:
--------------------------------------------------------------------------------
  1 | // Author of FLOAM: Wang Han 
  2 | // Email wh200720041@gmail.com
  3 | // Homepage https://wanghan.pro
  4 | 
  5 | #include "laserMappingClass.h"
  6 | 
  7 | void LaserMappingClass::init(double map_resolution){
  8 | 	//init map
  9 | 	//init can have real object, but future added block does not need
 10 | 	for(int i=0;i<LASER_CELL_RANGE_HORIZONTAL*2+1;i++){
 11 | 		std::vector<std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>> map_height_temp;
 12 | 		for(int j=0;j<LASER_CELL_RANGE_HORIZONTAL*2+1;j++){
 13 | 			std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> map_depth_temp;
 14 | 			for(int k=0;k<LASER_CELL_RANGE_VERTICAL*2+1;k++){
 15 | 				pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_temp(new pcl::PointCloud<pcl::PointXYZI>);
 16 | 				map_depth_temp.push_back(point_cloud_temp);	
 17 | 			}
 18 | 			map_height_temp.push_back(map_depth_temp);
 19 | 		}
 20 | 		map.push_back(map_height_temp);
 21 | 	}
 22 | 
 23 | 	origin_in_map_x = LASER_CELL_RANGE_HORIZONTAL;
 24 | 	origin_in_map_y = LASER_CELL_RANGE_HORIZONTAL;
 25 | 	origin_in_map_z = LASER_CELL_RANGE_VERTICAL;
 26 | 	map_width = LASER_CELL_RANGE_HORIZONTAL*2+1;
 27 | 	map_height = LASER_CELL_RANGE_HORIZONTAL*2+1;
 28 | 	map_depth = LASER_CELL_RANGE_HORIZONTAL*2+1;
 29 | 
 30 | 	//downsampling size
 31 | 	downSizeFilter.setLeafSize(map_resolution, map_resolution, map_resolution);
 32 | }
 33 | 
 34 | void LaserMappingClass::addWidthCellNegative(void){
 35 | 	std::vector<std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>> map_height_temp;
 36 | 	for(int j=0; j < map_height;j++){
 37 | 		std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> map_depth_temp;
 38 | 		for(int k=0;k< map_depth;k++){
 39 | 			pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_temp;
 40 | 			map_depth_temp.push_back(point_cloud_temp);
 41 | 		}
 42 | 		map_height_temp.push_back(map_depth_temp);
 43 | 	}
 44 | 	map.insert(map.begin(), map_height_temp);
 45 | 
 46 | 	origin_in_map_x++;
 47 | 	map_width++;
 48 | }
 49 | void LaserMappingClass::addWidthCellPositive(void){
 50 | 	std::vector<std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>> map_height_temp;
 51 | 	for(int j=0; j < map_height;j++){
 52 | 		std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> map_depth_temp;
 53 | 		for(int k=0;k< map_depth;k++){
 54 | 			pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_temp;
 55 | 			map_depth_temp.push_back(point_cloud_temp);
 56 | 		}
 57 | 		map_height_temp.push_back(map_depth_temp);
 58 | 	}
 59 | 	map.push_back(map_height_temp);
 60 | 	map_width++;
 61 | }
 62 | void LaserMappingClass::addHeightCellNegative(void){
 63 | 	for(int i=0; i < map_width;i++){
 64 | 		std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> map_depth_temp;
 65 | 		for(int k=0;k<map_depth;k++){
 66 | 			pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_temp;
 67 | 			map_depth_temp.push_back(point_cloud_temp);
 68 | 		}
 69 | 		map[i].insert(map[i].begin(), map_depth_temp);
 70 | 	}
 71 | 	origin_in_map_y++;
 72 | 	map_height++;
 73 | }
 74 | void LaserMappingClass::addHeightCellPositive(void){
 75 | 	for(int i=0; i < map_width;i++){
 76 | 		std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> map_depth_temp;
 77 | 		for(int k=0;k<map_depth;k++){
 78 | 			pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_temp;
 79 | 			map_depth_temp.push_back(point_cloud_temp);
 80 | 		}
 81 | 		map[i].push_back(map_depth_temp);
 82 | 	}
 83 | 	map_height++;
 84 | }
 85 | void LaserMappingClass::addDepthCellNegative(void){
 86 | 	for(int i=0; i < map_width;i++){
 87 | 		for(int j=0;j< map_height;j++){
 88 | 			pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_temp;
 89 | 			map[i][j].insert(map[i][j].begin(), point_cloud_temp);
 90 | 		}
 91 | 	}
 92 | 	origin_in_map_z++;
 93 | 	map_depth++;
 94 | }
 95 | void LaserMappingClass::addDepthCellPositive(void){
 96 | 	for(int i=0; i < map_width;i++){
 97 | 		for(int j=0;j< map_height;j++){
 98 | 			pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_temp;
 99 | 			map[i][j].push_back(point_cloud_temp);
100 | 		}
101 | 	}
102 | 	map_depth++;
103 | }
104 | 
105 | //extend map is points exceed size
106 | void LaserMappingClass::checkPoints(int& x, int& y, int& z){
107 | 
108 | 	while(x + LASER_CELL_RANGE_HORIZONTAL> map_width-1){
109 | 		addWidthCellPositive();
110 | 	}
111 | 	while(x-LASER_CELL_RANGE_HORIZONTAL<0){
112 | 		addWidthCellNegative();
113 | 		x++;
114 | 	}
115 | 	while(y + LASER_CELL_RANGE_HORIZONTAL> map_height-1){
116 | 		addHeightCellPositive();
117 | 	}
118 | 	while(y-LASER_CELL_RANGE_HORIZONTAL<0){
119 | 		addHeightCellNegative();
120 | 		y++;
121 | 	}
122 | 	while(z + LASER_CELL_RANGE_VERTICAL> map_depth-1){
123 | 		addDepthCellPositive();
124 | 	}
125 | 	while(z-LASER_CELL_RANGE_VERTICAL<0){
126 | 		addDepthCellNegative();
127 | 		z++;
128 | 	}
129 | 
130 | 	//initialize 
131 | 	//create object if area is null
132 | 	for(int i=x-LASER_CELL_RANGE_HORIZONTAL;i<x+LASER_CELL_RANGE_HORIZONTAL+1;i++){
133 | 		for(int j=y-LASER_CELL_RANGE_HORIZONTAL;j<y+LASER_CELL_RANGE_HORIZONTAL+1;j++){
134 | 			for(int k=z-LASER_CELL_RANGE_VERTICAL;k<z+LASER_CELL_RANGE_VERTICAL+1;k++){
135 | 				if(map[i][j][k]==NULL){
136 | 					pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_temp(new pcl::PointCloud<pcl::PointXYZI>());
137 | 					map[i][j][k] = point_cloud_temp;
138 | 				}
139 | 
140 | 			}
141 | 				
142 | 		}
143 | 
144 | 	}
145 | }
146 | 
147 | //update points to map 
148 | void LaserMappingClass::updateCurrentPointsToMap(const pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_in, const Eigen::Isometry3d& pose_current){
149 | 	
150 | 	int currentPosIdX = int(std::floor(pose_current.translation().x() / LASER_CELL_WIDTH + 0.5)) + origin_in_map_x;
151 | 	int currentPosIdY = int(std::floor(pose_current.translation().y() / LASER_CELL_HEIGHT + 0.5)) + origin_in_map_y;
152 | 	int currentPosIdZ = int(std::floor(pose_current.translation().z() / LASER_CELL_DEPTH + 0.5)) + origin_in_map_z;
153 | 
154 | 	//check is submap is null
155 | 	checkPoints(currentPosIdX,currentPosIdY,currentPosIdZ);
156 | 
157 | 	pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_pc(new pcl::PointCloud<pcl::PointXYZI>());
158 | 	pcl::transformPointCloud(*pc_in, *transformed_pc, pose_current.cast<float>());
159 | 	
160 | 	//save points
161 | 	for (int i = 0; i < (int)transformed_pc->points.size(); i++)
162 | 	{
163 | 		pcl::PointXYZI point_temp = transformed_pc->points[i];
164 | 		//for visualization only
165 | 		point_temp.intensity = std::min(1.0 , std::max(pc_in->points[i].z+2.0, 0.0) / 5);
166 | 		int currentPointIdX = int(std::floor(point_temp.x / LASER_CELL_WIDTH + 0.5)) + origin_in_map_x;
167 | 		int currentPointIdY = int(std::floor(point_temp.y / LASER_CELL_HEIGHT + 0.5)) + origin_in_map_y;
168 | 		int currentPointIdZ = int(std::floor(point_temp.z / LASER_CELL_DEPTH + 0.5)) + origin_in_map_z;
169 | 
170 | 		map[currentPointIdX][currentPointIdY][currentPointIdZ]->push_back(point_temp);
171 | 		
172 | 	}
173 | 	
174 | 	//filtering points 
175 | 	for(int i=currentPosIdX-LASER_CELL_RANGE_HORIZONTAL;i<currentPosIdX+LASER_CELL_RANGE_HORIZONTAL+1;i++){
176 | 		for(int j=currentPosIdY-LASER_CELL_RANGE_HORIZONTAL;j<currentPosIdY+LASER_CELL_RANGE_HORIZONTAL+1;j++){
177 | 			for(int k=currentPosIdZ-LASER_CELL_RANGE_VERTICAL;k<currentPosIdZ+LASER_CELL_RANGE_VERTICAL+1;k++){
178 | 				downSizeFilter.setInputCloud(map[i][j][k]);
179 | 				downSizeFilter.filter(*(map[i][j][k]));
180 | 			}
181 | 				
182 | 		}
183 | 
184 | 	}
185 | 
186 | }
187 | 
188 | pcl::PointCloud<pcl::PointXYZI>::Ptr LaserMappingClass::getMap(void){
189 | 	pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudMap = pcl::PointCloud<pcl::PointXYZI>::Ptr(new  pcl::PointCloud<pcl::PointXYZI>());
190 | 	for (int i = 0; i < map_width; i++){
191 | 		for (int j = 0; j < map_height; j++){
192 | 			for (int k = 0; k < map_depth; k++){
193 | 				if(map[i][j][k]!=NULL){
194 | 					*laserCloudMap += *(map[i][j][k]);
195 | 				}
196 | 			}
197 | 		}
198 | 	}
199 | 	return laserCloudMap;
200 | }
201 | 
202 | LaserMappingClass::LaserMappingClass(){
203 | 
204 | }
205 | 
206 | 


--------------------------------------------------------------------------------
/SC-A-LOAM/src/kittiHelper.cpp:
--------------------------------------------------------------------------------
  1 | // Author:   Tong Qin               qintonguav@gmail.com
  2 | // 	         Shaozu Cao 		    saozu.cao@connect.ust.hk
  3 | 
  4 | #include <iostream>
  5 | #include <fstream>
  6 | #include <iterator>
  7 | #include <string>
  8 | #include <vector>
  9 | #include <opencv2/opencv.hpp>
 10 | #include <image_transport/image_transport.h>
 11 | #include <opencv2/highgui/highgui.hpp>
 12 | #include <nav_msgs/Odometry.h>
 13 | #include <nav_msgs/Path.h>
 14 | #include <ros/ros.h>
 15 | #include <rosbag/bag.h>
 16 | #include <geometry_msgs/PoseStamped.h>
 17 | #include <cv_bridge/cv_bridge.h>
 18 | #include <sensor_msgs/image_encodings.h>
 19 | #include <eigen3/Eigen/Dense>
 20 | #include <pcl/point_cloud.h>
 21 | #include <pcl/point_types.h>
 22 | #include <pcl_conversions/pcl_conversions.h>
 23 | #include <sensor_msgs/PointCloud2.h>
 24 | 
 25 | std::vector<float> 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<float> lidar_data_buffer(num_elements);
 33 |     lidar_data_file.read(reinterpret_cast<char*>(&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<sensor_msgs::PointCloud2>("/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<nav_msgs::Odometry> ("/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<nav_msgs::Path> ("/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::IMREAD_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::IMREAD_GRAYSCALE);
 94 | 
 95 |         std::getline(ground_truth_file, line);
 96 |         std::stringstream pose_stream(line);
 97 |         std::string s;
 98 |         Eigen::Matrix<double, 3, 4> 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<float> 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<Eigen::Vector3d> lidar_points;
138 |         std::vector<float> lidar_intensities;
139 |         pcl::PointCloud<pcl::PointXYZI> 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 | }


--------------------------------------------------------------------------------
/floam/rviz/floam_velodyne.rviz:
--------------------------------------------------------------------------------
  1 | Panels:
  2 |   - Class: rviz/Displays
  3 |     Help Height: 78
  4 |     Name: Displays
  5 |     Property Tree Widget:
  6 |       Expanded:
  7 |         - /Grid1
  8 |         - /TF1
  9 |         - /TF1/Frames1
 10 |         - /map1
 11 |         - /map1/Autocompute Value Bounds1
 12 |       Splitter Ratio: 0.5
 13 |     Tree Height: 728
 14 |   - Class: rviz/Selection
 15 |     Name: Selection
 16 |   - Class: rviz/Tool Properties
 17 |     Expanded:
 18 |       - /2D Pose Estimate1
 19 |       - /2D Nav Goal1
 20 |       - /Publish Point1
 21 |     Name: Tool Properties
 22 |     Splitter Ratio: 0.5886790156364441
 23 |   - Class: rviz/Views
 24 |     Expanded:
 25 |       - /Current View1
 26 |     Name: Views
 27 |     Splitter Ratio: 0.5
 28 |   - Class: rviz/Time
 29 |     Experimental: false
 30 |     Name: Time
 31 |     SyncMode: 0
 32 |     SyncSource: map
 33 | Preferences:
 34 |   PromptSaveOnExit: true
 35 | Toolbars:
 36 |   toolButtonStyle: 2
 37 | Visualization Manager:
 38 |   Class: ""
 39 |   Displays:
 40 |     - Alpha: 0.5
 41 |       Cell Size: 5
 42 |       Class: rviz/Grid
 43 |       Color: 160; 160; 164
 44 |       Enabled: true
 45 |       Line Style:
 46 |         Line Width: 0.029999999329447746
 47 |         Value: Lines
 48 |       Name: Grid
 49 |       Normal Cell Count: 0
 50 |       Offset:
 51 |         X: 0
 52 |         Y: 0
 53 |         Z: 0
 54 |       Plane: XY
 55 |       Plane Cell Count: 15
 56 |       Reference Frame: <Fixed Frame>
 57 |       Value: true
 58 |     - Class: rviz/TF
 59 |       Enabled: true
 60 |       Frame Timeout: 15
 61 |       Frames:
 62 |         All Enabled: false
 63 |         aft_mapped:
 64 |           Value: false
 65 |         base_link:
 66 |           Value: true
 67 |         map:
 68 |           Value: true
 69 |         world:
 70 |           Value: false
 71 |       Marker Scale: 3
 72 |       Name: TF
 73 |       Show Arrows: false
 74 |       Show Axes: true
 75 |       Show Names: true
 76 |       Tree:
 77 |         world:
 78 |           map:
 79 |             aft_mapped:
 80 |               {}
 81 |             base_link:
 82 |               {}
 83 |       Update Interval: 0
 84 |       Value: true
 85 |     - Alpha: 1
 86 |       Autocompute Intensity Bounds: true
 87 |       Autocompute Value Bounds:
 88 |         Max Value: 0.21922564506530762
 89 |         Min Value: -11.99945068359375
 90 |         Value: true
 91 |       Axis: Z
 92 |       Channel Name: intensity
 93 |       Class: rviz/PointCloud2
 94 |       Color: 255; 255; 255
 95 |       Color Transformer: Intensity
 96 |       Decay Time: 0
 97 |       Enabled: false
 98 |       Invert Rainbow: false
 99 |       Max Color: 255; 255; 255
100 |       Max Intensity: 255
101 |       Min Color: 0; 0; 0
102 |       Min Intensity: 0
103 |       Name: raw_data
104 |       Position Transformer: XYZ
105 |       Queue Size: 10
106 |       Selectable: true
107 |       Size (Pixels): 1
108 |       Size (m): 0.009999999776482582
109 |       Style: Points
110 |       Topic: /velodyne_points_filtered
111 |       Unreliable: false
112 |       Use Fixed Frame: true
113 |       Use rainbow: true
114 |       Value: false
115 |     - Alpha: 0.699999988079071
116 |       Autocompute Intensity Bounds: true
117 |       Autocompute Value Bounds:
118 |         Max Value: 2.685699939727783
119 |         Min Value: -1.4920799732208252
120 |         Value: false
121 |       Axis: Z
122 |       Channel Name: intensity
123 |       Class: rviz/PointCloud2
124 |       Color: 255; 255; 255
125 |       Color Transformer: AxisColor
126 |       Decay Time: 0
127 |       Enabled: true
128 |       Invert Rainbow: false
129 |       Max Color: 255; 255; 255
130 |       Max Intensity: 0.9565645456314087
131 |       Min Color: 0; 0; 0
132 |       Min Intensity: 0
133 |       Name: map
134 |       Position Transformer: XYZ
135 |       Queue Size: 10
136 |       Selectable: true
137 |       Size (Pixels): 2
138 |       Size (m): 0.10000000149011612
139 |       Style: Flat Squares
140 |       Topic: /map
141 |       Unreliable: false
142 |       Use Fixed Frame: true
143 |       Use rainbow: true
144 |       Value: true
145 |     - Alpha: 1
146 |       Buffer Length: 1
147 |       Class: rviz/Path
148 |       Color: 25; 255; 0
149 |       Enabled: true
150 |       Head Diameter: 0.30000001192092896
151 |       Head Length: 0.20000000298023224
152 |       Length: 0.30000001192092896
153 |       Line Style: Lines
154 |       Line Width: 0.029999999329447746
155 |       Name: floam_result
156 |       Offset:
157 |         X: 0
158 |         Y: 0
159 |         Z: 0
160 |       Pose Color: 255; 85; 255
161 |       Pose Style: None
162 |       Radius: 0.029999999329447746
163 |       Shaft Diameter: 0.10000000149011612
164 |       Shaft Length: 0.10000000149011612
165 |       Topic: /base_link/trajectory
166 |       Unreliable: false
167 |       Value: true
168 |   Enabled: true
169 |   Global Options:
170 |     Background Color: 0; 0; 0
171 |     Default Light: true
172 |     Fixed Frame: map
173 |     Frame Rate: 30
174 |   Name: root
175 |   Tools:
176 |     - Class: rviz/Interact
177 |       Hide Inactive Objects: true
178 |     - Class: rviz/MoveCamera
179 |     - Class: rviz/Select
180 |     - Class: rviz/FocusCamera
181 |     - Class: rviz/Measure
182 |     - Class: rviz/SetInitialPose
183 |       Theta std deviation: 0.2617993950843811
184 |       Topic: /initialpose
185 |       X std deviation: 0.5
186 |       Y std deviation: 0.5
187 |     - Class: rviz/SetGoal
188 |       Topic: /move_base_simple/goal
189 |     - Class: rviz/PublishPoint
190 |       Single click: true
191 |       Topic: /clicked_point
192 |   Value: true
193 |   Views:
194 |     Current:
195 |       Class: rviz/ThirdPersonFollower
196 |       Distance: 16.892133712768555
197 |       Enable Stereo Rendering:
198 |         Stereo Eye Separation: 0.05999999865889549
199 |         Stereo Focal Distance: 1
200 |         Swap Stereo Eyes: false
201 |         Value: false
202 |       Focal Point:
203 |         X: -1.854732632637024
204 |         Y: -1.3892109394073486
205 |         Z: 6.691127782687545e-5
206 |       Focal Shape Fixed Size: true
207 |       Focal Shape Size: 0.05000000074505806
208 |       Invert Z Axis: false
209 |       Name: Current View
210 |       Near Clip Distance: 0.009999999776482582
211 |       Pitch: 0.924797773361206
212 |       Target Frame: body
213 |       Value: ThirdPersonFollower (rviz)
214 |       Yaw: 1.1854360103607178
215 |     Saved: ~
216 | Window Geometry:
217 |   Displays:
218 |     collapsed: false
219 |   Height: 1025
220 |   Hide Left Dock: false
221 |   Hide Right Dock: false
222 |   QMainWindow State: 000000ff00000000fd00000004000000000000016a00000363fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002e0000000c00000000000000000fb0000000600490053004300000002d9000000c70000000000000000000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004b80000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
223 |   Selection:
224 |     collapsed: false
225 |   Time:
226 |     collapsed: false
227 |   Tool Properties:
228 |     collapsed: false
229 |   Views:
230 |     collapsed: false
231 |   Width: 1853
232 |   X: 795
233 |   Y: 27
234 | 


--------------------------------------------------------------------------------
/floam/rviz/floam.rviz:
--------------------------------------------------------------------------------
  1 | Panels:
  2 |   - Class: rviz/Displays
  3 |     Help Height: 78
  4 |     Name: Displays
  5 |     Property Tree Widget:
  6 |       Expanded:
  7 |         - /Status1
  8 |         - /Grid1
  9 |         - /TF1
 10 |         - /TF1/Frames1
 11 |       Splitter Ratio: 0.5
 12 |     Tree Height: 530
 13 |   - Class: rviz/Selection
 14 |     Name: Selection
 15 |   - Class: rviz/Tool Properties
 16 |     Expanded:
 17 |       - /2D Pose Estimate1
 18 |       - /2D Nav Goal1
 19 |       - /Publish Point1
 20 |     Name: Tool Properties
 21 |     Splitter Ratio: 0.5886790156364441
 22 |   - Class: rviz/Views
 23 |     Expanded:
 24 |       - /Current View1
 25 |     Name: Views
 26 |     Splitter Ratio: 0.5
 27 |   - Class: rviz/Time
 28 |     Experimental: false
 29 |     Name: Time
 30 |     SyncMode: 0
 31 |     SyncSource: Image
 32 | Preferences:
 33 |   PromptSaveOnExit: true
 34 | Toolbars:
 35 |   toolButtonStyle: 2
 36 | Visualization Manager:
 37 |   Class: ""
 38 |   Displays:
 39 |     - Alpha: 0.5
 40 |       Cell Size: 50
 41 |       Class: rviz/Grid
 42 |       Color: 160; 160; 164
 43 |       Enabled: true
 44 |       Line Style:
 45 |         Line Width: 0.029999999329447746
 46 |         Value: Lines
 47 |       Name: Grid
 48 |       Normal Cell Count: 0
 49 |       Offset:
 50 |         X: 0
 51 |         Y: 0
 52 |         Z: 0
 53 |       Plane: XY
 54 |       Plane Cell Count: 15
 55 |       Reference Frame: <Fixed Frame>
 56 |       Value: true
 57 |     - Class: rviz/Image
 58 |       Enabled: true
 59 |       Image Topic: /cam03/image_raw
 60 |       Max Value: 1
 61 |       Median window: 5
 62 |       Min Value: 0
 63 |       Name: Image
 64 |       Normalize Range: true
 65 |       Queue Size: 2
 66 |       Transport Hint: raw
 67 |       Unreliable: false
 68 |       Value: true
 69 |     - Class: rviz/TF
 70 |       Enabled: true
 71 |       Frame Timeout: 15
 72 |       Frames:
 73 |         All Enabled: false
 74 |         base_link:
 75 |           Value: true
 76 |         cam00:
 77 |           Value: false
 78 |         cam01:
 79 |           Value: false
 80 |         cam02:
 81 |           Value: false
 82 |         cam03:
 83 |           Value: false
 84 |         imu:
 85 |           Value: false
 86 |         map:
 87 |           Value: false
 88 |         velodyne:
 89 |           Value: true
 90 |         world:
 91 |           Value: true
 92 |       Marker Scale: 100
 93 |       Name: TF
 94 |       Show Arrows: false
 95 |       Show Axes: true
 96 |       Show Names: true
 97 |       Tree:
 98 |         world:
 99 |           imu:
100 |             cam00:
101 |               {}
102 |             cam01:
103 |               {}
104 |             cam02:
105 |               {}
106 |             cam03:
107 |               {}
108 |             velodyne:
109 |               {}
110 |           map:
111 |             base_link:
112 |               {}
113 |       Update Interval: 0
114 |       Value: true
115 |     - Alpha: 1
116 |       Autocompute Intensity Bounds: true
117 |       Autocompute Value Bounds:
118 |         Max Value: 4.832673072814941
119 |         Min Value: -8.602354049682617
120 |         Value: true
121 |       Axis: Z
122 |       Channel Name: intensity
123 |       Class: rviz/PointCloud2
124 |       Color: 255; 255; 255
125 |       Color Transformer: Intensity
126 |       Decay Time: 0
127 |       Enabled: true
128 |       Invert Rainbow: false
129 |       Max Color: 255; 255; 255
130 |       Max Intensity: 0.9900000095367432
131 |       Min Color: 0; 0; 0
132 |       Min Intensity: 0
133 |       Name: raw_data
134 |       Position Transformer: XYZ
135 |       Queue Size: 10
136 |       Selectable: true
137 |       Size (Pixels): 1
138 |       Size (m): 0.009999999776482582
139 |       Style: Points
140 |       Topic: /velodyne_points
141 |       Unreliable: false
142 |       Use Fixed Frame: true
143 |       Use rainbow: true
144 |       Value: true
145 |     - Alpha: 1
146 |       Buffer Length: 1
147 |       Class: rviz/Path
148 |       Color: 239; 41; 41
149 |       Enabled: true
150 |       Head Diameter: 0.30000001192092896
151 |       Head Length: 0.20000000298023224
152 |       Length: 0.30000001192092896
153 |       Line Style: Lines
154 |       Line Width: 0.029999999329447746
155 |       Name: ground_truth
156 |       Offset:
157 |         X: 0
158 |         Y: 0
159 |         Z: 0
160 |       Pose Color: 255; 85; 255
161 |       Pose Style: None
162 |       Radius: 0.029999999329447746
163 |       Shaft Diameter: 0.10000000149011612
164 |       Shaft Length: 0.10000000149011612
165 |       Topic: /gt/trajectory
166 |       Unreliable: false
167 |       Value: true
168 |     - Alpha: 1
169 |       Buffer Length: 1
170 |       Class: rviz/Path
171 |       Color: 0; 255; 0
172 |       Enabled: true
173 |       Head Diameter: 0.30000001192092896
174 |       Head Length: 0.20000000298023224
175 |       Length: 0.30000001192092896
176 |       Line Style: Lines
177 |       Line Width: 0.029999999329447746
178 |       Name: floam_result
179 |       Offset:
180 |         X: 0
181 |         Y: 0
182 |         Z: 0
183 |       Pose Color: 255; 85; 255
184 |       Pose Style: None
185 |       Radius: 0.029999999329447746
186 |       Shaft Diameter: 0.10000000149011612
187 |       Shaft Length: 0.10000000149011612
188 |       Topic: /base_link/trajectory
189 |       Unreliable: false
190 |       Value: true
191 |   Enabled: true
192 |   Global Options:
193 |     Background Color: 0; 0; 0
194 |     Default Light: true
195 |     Fixed Frame: world
196 |     Frame Rate: 30
197 |   Name: root
198 |   Tools:
199 |     - Class: rviz/Interact
200 |       Hide Inactive Objects: true
201 |     - Class: rviz/MoveCamera
202 |     - Class: rviz/Select
203 |     - Class: rviz/FocusCamera
204 |     - Class: rviz/Measure
205 |     - Class: rviz/SetInitialPose
206 |       Theta std deviation: 0.2617993950843811
207 |       Topic: /initialpose
208 |       X std deviation: 0.5
209 |       Y std deviation: 0.5
210 |     - Class: rviz/SetGoal
211 |       Topic: /move_base_simple/goal
212 |     - Class: rviz/PublishPoint
213 |       Single click: true
214 |       Topic: /clicked_point
215 |   Value: true
216 |   Views:
217 |     Current:
218 |       Angle: -1.5749943256378174
219 |       Class: rviz/TopDownOrtho
220 |       Enable Stereo Rendering:
221 |         Stereo Eye Separation: 0.05999999865889549
222 |         Stereo Focal Distance: 1
223 |         Swap Stereo Eyes: false
224 |         Value: false
225 |       Invert Z Axis: false
226 |       Name: Current View
227 |       Near Clip Distance: 0.009999999776482582
228 |       Scale: 2.7801454067230225
229 |       Target Frame: body
230 |       Value: TopDownOrtho (rviz)
231 |       X: 84.71299743652344
232 |       Y: -50.52915954589844
233 |     Saved: ~
234 | Window Geometry:
235 |   Displays:
236 |     collapsed: false
237 |   Height: 1025
238 |   Hide Left Dock: false
239 |   Hide Right Dock: false
240 |   Image:
241 |     collapsed: false
242 |   QMainWindow State: 000000ff00000000fd00000004000000000000016a00000363fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000029d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002e0000000c00000001600fffffffb0000000600490053004300000002d9000000c70000000000000000000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004b80000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
243 |   Selection:
244 |     collapsed: false
245 |   Time:
246 |     collapsed: false
247 |   Tool Properties:
248 |     collapsed: false
249 |   Views:
250 |     collapsed: false
251 |   Width: 1853
252 |   X: 795
253 |   Y: 27
254 | 


--------------------------------------------------------------------------------
/floam/rviz/floam_mapping.rviz:
--------------------------------------------------------------------------------
  1 | Panels:
  2 |   - Class: rviz/Displays
  3 |     Help Height: 78
  4 |     Name: Displays
  5 |     Property Tree Widget:
  6 |       Expanded:
  7 |         - /Global Options1
  8 |         - /TF1/Frames1
  9 |         - /map1
 10 |       Splitter Ratio: 0.5
 11 |     Tree Height: 530
 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: Image
 31 | Preferences:
 32 |   PromptSaveOnExit: true
 33 | Toolbars:
 34 |   toolButtonStyle: 2
 35 | Visualization Manager:
 36 |   Class: ""
 37 |   Displays:
 38 |     - Alpha: 0.5
 39 |       Cell Size: 50
 40 |       Class: rviz/Grid
 41 |       Color: 160; 160; 164
 42 |       Enabled: false
 43 |       Line Style:
 44 |         Line Width: 0.029999999329447746
 45 |         Value: Lines
 46 |       Name: Grid
 47 |       Normal Cell Count: 0
 48 |       Offset:
 49 |         X: 0
 50 |         Y: 0
 51 |         Z: 0
 52 |       Plane: XY
 53 |       Plane Cell Count: 15
 54 |       Reference Frame: <Fixed Frame>
 55 |       Value: false
 56 |     - Class: rviz/Image
 57 |       Enabled: true
 58 |       Image Topic: /cam03/image_raw
 59 |       Max Value: 1
 60 |       Median window: 5
 61 |       Min Value: 0
 62 |       Name: Image
 63 |       Normalize Range: true
 64 |       Queue Size: 2
 65 |       Transport Hint: raw
 66 |       Unreliable: false
 67 |       Value: true
 68 |     - Class: rviz/TF
 69 |       Enabled: true
 70 |       Frame Timeout: 15
 71 |       Frames:
 72 |         All Enabled: false
 73 |         base_link:
 74 |           Value: true
 75 |         cam00:
 76 |           Value: false
 77 |         cam01:
 78 |           Value: false
 79 |         cam02:
 80 |           Value: false
 81 |         cam03:
 82 |           Value: false
 83 |         imu:
 84 |           Value: false
 85 |         map:
 86 |           Value: false
 87 |         velodyne:
 88 |           Value: false
 89 |         world:
 90 |           Value: true
 91 |       Marker Scale: 50
 92 |       Name: TF
 93 |       Show Arrows: false
 94 |       Show Axes: true
 95 |       Show Names: true
 96 |       Tree:
 97 |         world:
 98 |           imu:
 99 |             cam00:
100 |               {}
101 |             cam01:
102 |               {}
103 |             cam02:
104 |               {}
105 |             cam03:
106 |               {}
107 |             velodyne:
108 |               {}
109 |           map:
110 |             base_link:
111 |               {}
112 |       Update Interval: 0
113 |       Value: true
114 |     - Alpha: 1
115 |       Autocompute Intensity Bounds: true
116 |       Autocompute Value Bounds:
117 |         Max Value: 0.21922564506530762
118 |         Min Value: -11.99945068359375
119 |         Value: true
120 |       Axis: Z
121 |       Channel Name: intensity
122 |       Class: rviz/PointCloud2
123 |       Color: 255; 255; 255
124 |       Color Transformer: Intensity
125 |       Decay Time: 0
126 |       Enabled: false
127 |       Invert Rainbow: false
128 |       Max Color: 255; 255; 255
129 |       Max Intensity: 0.9900000095367432
130 |       Min Color: 0; 0; 0
131 |       Min Intensity: 0
132 |       Name: raw_data
133 |       Position Transformer: XYZ
134 |       Queue Size: 10
135 |       Selectable: true
136 |       Size (Pixels): 2
137 |       Size (m): 0.009999999776482582
138 |       Style: Points
139 |       Topic: /velodyne_points
140 |       Unreliable: false
141 |       Use Fixed Frame: true
142 |       Use rainbow: true
143 |       Value: false
144 |     - Alpha: 0.699999988079071
145 |       Autocompute Intensity Bounds: true
146 |       Autocompute Value Bounds:
147 |         Max Value: 8.244071960449219
148 |         Min Value: -20.63126564025879
149 |         Value: true
150 |       Axis: Z
151 |       Channel Name: intensity
152 |       Class: rviz/PointCloud2
153 |       Color: 255; 255; 255
154 |       Color Transformer: Intensity
155 |       Decay Time: 0
156 |       Enabled: true
157 |       Invert Rainbow: false
158 |       Max Color: 255; 255; 255
159 |       Max Intensity: 0.8811999559402466
160 |       Min Color: 0; 0; 0
161 |       Min Intensity: 0
162 |       Name: map
163 |       Position Transformer: XYZ
164 |       Queue Size: 10
165 |       Selectable: true
166 |       Size (Pixels): 1
167 |       Size (m): 0.3499999940395355
168 |       Style: Squares
169 |       Topic: /map
170 |       Unreliable: false
171 |       Use Fixed Frame: true
172 |       Use rainbow: true
173 |       Value: true
174 |     - Alpha: 1
175 |       Buffer Length: 1
176 |       Class: rviz/Path
177 |       Color: 239; 41; 41
178 |       Enabled: true
179 |       Head Diameter: 0.30000001192092896
180 |       Head Length: 0.20000000298023224
181 |       Length: 0.30000001192092896
182 |       Line Style: Lines
183 |       Line Width: 0.029999999329447746
184 |       Name: ground_truth
185 |       Offset:
186 |         X: 0
187 |         Y: 0
188 |         Z: 0
189 |       Pose Color: 255; 85; 255
190 |       Pose Style: None
191 |       Radius: 0.029999999329447746
192 |       Shaft Diameter: 0.10000000149011612
193 |       Shaft Length: 0.10000000149011612
194 |       Topic: /gt/trajectory
195 |       Unreliable: false
196 |       Value: true
197 |     - Alpha: 1
198 |       Buffer Length: 1
199 |       Class: rviz/Path
200 |       Color: 25; 255; 0
201 |       Enabled: true
202 |       Head Diameter: 0.30000001192092896
203 |       Head Length: 0.20000000298023224
204 |       Length: 0.30000001192092896
205 |       Line Style: Lines
206 |       Line Width: 0.029999999329447746
207 |       Name: floam_result
208 |       Offset:
209 |         X: 0
210 |         Y: 0
211 |         Z: 0
212 |       Pose Color: 255; 85; 255
213 |       Pose Style: None
214 |       Radius: 0.029999999329447746
215 |       Shaft Diameter: 0.10000000149011612
216 |       Shaft Length: 0.10000000149011612
217 |       Topic: /base_link/trajectory
218 |       Unreliable: false
219 |       Value: true
220 |   Enabled: true
221 |   Global Options:
222 |     Background Color: 136; 138; 133
223 |     Default Light: true
224 |     Fixed Frame: base_link
225 |     Frame Rate: 30
226 |   Name: root
227 |   Tools:
228 |     - Class: rviz/Interact
229 |       Hide Inactive Objects: true
230 |     - Class: rviz/MoveCamera
231 |     - Class: rviz/Select
232 |     - Class: rviz/FocusCamera
233 |     - Class: rviz/Measure
234 |     - Class: rviz/SetInitialPose
235 |       Theta std deviation: 0.2617993950843811
236 |       Topic: /initialpose
237 |       X std deviation: 0.5
238 |       Y std deviation: 0.5
239 |     - Class: rviz/SetGoal
240 |       Topic: /move_base_simple/goal
241 |     - Class: rviz/PublishPoint
242 |       Single click: true
243 |       Topic: /clicked_point
244 |   Value: true
245 |   Views:
246 |     Current:
247 |       Class: rviz/ThirdPersonFollower
248 |       Distance: 86.67870330810547
249 |       Enable Stereo Rendering:
250 |         Stereo Eye Separation: 0.05999999865889549
251 |         Stereo Focal Distance: 1
252 |         Swap Stereo Eyes: false
253 |         Value: false
254 |       Focal Point:
255 |         X: -35.922298431396484
256 |         Y: 0.5339305400848389
257 |         Z: 6.388764450093731e-5
258 |       Focal Shape Fixed Size: true
259 |       Focal Shape Size: 0.05000000074505806
260 |       Invert Z Axis: false
261 |       Name: Current View
262 |       Near Clip Distance: 0.009999999776482582
263 |       Pitch: 0.9953982830047607
264 |       Target Frame: body
265 |       Value: ThirdPersonFollower (rviz)
266 |       Yaw: 3.2104086875915527
267 |     Saved: ~
268 | Window Geometry:
269 |   Displays:
270 |     collapsed: false
271 |   Height: 1025
272 |   Hide Left Dock: false
273 |   Hide Right Dock: false
274 |   Image:
275 |     collapsed: false
276 |   QMainWindow State: 000000ff00000000fd00000004000000000000016a00000363fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000029d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002e0000000c00000001600fffffffb0000000600490053004300000002d9000000c70000000000000000000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004b80000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
277 |   Selection:
278 |     collapsed: false
279 |   Time:
280 |     collapsed: false
281 |   Tool Properties:
282 |     collapsed: false
283 |   Views:
284 |     collapsed: false
285 |   Width: 1853
286 |   X: 795
287 |   Y: 27
288 | 


--------------------------------------------------------------------------------
/floam/src/laserProcessingClass.cpp:
--------------------------------------------------------------------------------
  1 | // Author of FLOAM: Wang Han 
  2 | // Email wh200720041@gmail.com
  3 | // Homepage https://wanghan.pro
  4 | #include "laserProcessingClass.h"
  5 | 
  6 | void LaserProcessingClass::init(lidar::Lidar lidar_param_in){
  7 |     
  8 |     lidar_param = lidar_param_in;
  9 | 
 10 | }
 11 | 
 12 | void LaserProcessingClass::featureExtraction(const pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_in, pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_out_edge, pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_out_surf){
 13 | 
 14 |     std::vector<int> indices;
 15 |     pcl::removeNaNFromPointCloud(*pc_in, indices);
 16 | 
 17 | 
 18 |     int N_SCANS = lidar_param.num_lines;
 19 |     std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> laserCloudScans;
 20 |     for(int i=0;i<N_SCANS;i++){
 21 |         laserCloudScans.push_back(pcl::PointCloud<pcl::PointXYZI>::Ptr(new pcl::PointCloud<pcl::PointXYZI>()));
 22 |     }
 23 | 
 24 |     for (int i = 0; i < (int) pc_in->points.size(); i++)
 25 |     {
 26 |         int scanID=0;
 27 |         double distance = sqrt(pc_in->points[i].x * pc_in->points[i].x + pc_in->points[i].y * pc_in->points[i].y);
 28 |         if(distance<lidar_param.min_distance || distance>lidar_param.max_distance)
 29 |             continue;
 30 |         double angle = atan(pc_in->points[i].z / distance) * 180 / M_PI;
 31 |         
 32 |         if (N_SCANS == 16)
 33 |         {
 34 |             scanID = int((angle + 15) / 2 + 0.5);
 35 |             if (scanID > (N_SCANS - 1) || scanID < 0)
 36 |             {
 37 |                 continue;
 38 |             }
 39 |         }
 40 |         else if (N_SCANS == 32)
 41 |         {
 42 |             scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);
 43 |             if (scanID > (N_SCANS - 1) || scanID < 0)
 44 |             {
 45 |                 continue;
 46 |             }
 47 |         }
 48 |         else if (N_SCANS == 64)
 49 |         {   
 50 |             if (angle >= -8.83)
 51 |                 scanID = int((2 - angle) * 3.0 + 0.5);
 52 |             else
 53 |                 scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);
 54 | 
 55 |             if (angle > 2 || angle < -24.33 || scanID > 63 || scanID < 0)
 56 |             {
 57 |                 continue;
 58 |             }
 59 |         }
 60 |         else
 61 |         {
 62 |             printf("wrong scan number\n");
 63 |         }
 64 |         laserCloudScans[scanID]->push_back(pc_in->points[i]); 
 65 | 
 66 |     }
 67 | 
 68 |     for(int i = 0; i < N_SCANS; i++){
 69 |         if(laserCloudScans[i]->points.size()<131){
 70 |             continue;
 71 |         }
 72 |         
 73 |         std::vector<Double2d> cloudCurvature; 
 74 |         int total_points = laserCloudScans[i]->points.size()-10;
 75 |         for(int j = 5; j < (int)laserCloudScans[i]->points.size() - 5; j++){
 76 |             double diffX = laserCloudScans[i]->points[j - 5].x + laserCloudScans[i]->points[j - 4].x + laserCloudScans[i]->points[j - 3].x + laserCloudScans[i]->points[j - 2].x + laserCloudScans[i]->points[j - 1].x - 10 * laserCloudScans[i]->points[j].x + laserCloudScans[i]->points[j + 1].x + laserCloudScans[i]->points[j + 2].x + laserCloudScans[i]->points[j + 3].x + laserCloudScans[i]->points[j + 4].x + laserCloudScans[i]->points[j + 5].x;
 77 |             double diffY = laserCloudScans[i]->points[j - 5].y + laserCloudScans[i]->points[j - 4].y + laserCloudScans[i]->points[j - 3].y + laserCloudScans[i]->points[j - 2].y + laserCloudScans[i]->points[j - 1].y - 10 * laserCloudScans[i]->points[j].y + laserCloudScans[i]->points[j + 1].y + laserCloudScans[i]->points[j + 2].y + laserCloudScans[i]->points[j + 3].y + laserCloudScans[i]->points[j + 4].y + laserCloudScans[i]->points[j + 5].y;
 78 |             double diffZ = laserCloudScans[i]->points[j - 5].z + laserCloudScans[i]->points[j - 4].z + laserCloudScans[i]->points[j - 3].z + laserCloudScans[i]->points[j - 2].z + laserCloudScans[i]->points[j - 1].z - 10 * laserCloudScans[i]->points[j].z + laserCloudScans[i]->points[j + 1].z + laserCloudScans[i]->points[j + 2].z + laserCloudScans[i]->points[j + 3].z + laserCloudScans[i]->points[j + 4].z + laserCloudScans[i]->points[j + 5].z;
 79 |             Double2d distance(j,diffX * diffX + diffY * diffY + diffZ * diffZ);
 80 |             cloudCurvature.push_back(distance);
 81 | 
 82 |         }
 83 |         for(int j=0;j<6;j++){
 84 |             int sector_length = (int)(total_points/6);
 85 |             int sector_start = sector_length *j;
 86 |             int sector_end = sector_length *(j+1)-1;
 87 |             if (j==5){
 88 |                 sector_end = total_points - 1; 
 89 |             }
 90 |             std::vector<Double2d> subCloudCurvature(cloudCurvature.begin()+sector_start,cloudCurvature.begin()+sector_end); 
 91 |             
 92 |             featureExtractionFromSector(laserCloudScans[i],subCloudCurvature, pc_out_edge, pc_out_surf);
 93 |             
 94 |         }
 95 | 
 96 |     }
 97 | 
 98 | }
 99 | 
100 | 
101 | void LaserProcessingClass::featureExtractionFromSector(const pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_in, std::vector<Double2d>& cloudCurvature, pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_out_edge, pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_out_surf){
102 | 
103 |     std::sort(cloudCurvature.begin(), cloudCurvature.end(), [](const Double2d & a, const Double2d & b)
104 |     { 
105 |         return a.value < b.value; 
106 |     });
107 | 
108 | 
109 |     int largestPickedNum = 0;
110 |     std::vector<int> picked_points;
111 |     int point_info_count =0;
112 |     for (int i = cloudCurvature.size()-1; i >= 0; i--)
113 |     {
114 |         int ind = cloudCurvature[i].id; 
115 |         if(std::find(picked_points.begin(), picked_points.end(), ind)==picked_points.end()){
116 |             if(cloudCurvature[i].value <= 0.1){
117 |                 break;
118 |             }
119 |             
120 |             largestPickedNum++;
121 |             picked_points.push_back(ind);
122 |             
123 |             if (largestPickedNum <= 20){
124 |                 pc_out_edge->push_back(pc_in->points[ind]);
125 |                 point_info_count++;
126 |             }else{
127 |                 break;
128 |             }
129 | 
130 |             for(int k=1;k<=5;k++){
131 |                 double diffX = pc_in->points[ind + k].x - pc_in->points[ind + k - 1].x;
132 |                 double diffY = pc_in->points[ind + k].y - pc_in->points[ind + k - 1].y;
133 |                 double diffZ = pc_in->points[ind + k].z - pc_in->points[ind + k - 1].z;
134 |                 if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){
135 |                     break;
136 |                 }
137 |                 picked_points.push_back(ind+k);
138 |             }
139 |             for(int k=-1;k>=-5;k--){
140 |                 double diffX = pc_in->points[ind + k].x - pc_in->points[ind + k + 1].x;
141 |                 double diffY = pc_in->points[ind + k].y - pc_in->points[ind + k + 1].y;
142 |                 double diffZ = pc_in->points[ind + k].z - pc_in->points[ind + k + 1].z;
143 |                 if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){
144 |                     break;
145 |                 }
146 |                 picked_points.push_back(ind+k);
147 |             }
148 | 
149 |         }
150 |     }
151 | 
152 |     //find flat points
153 |     // point_info_count =0;
154 |     // int smallestPickedNum = 0;
155 |     
156 |     // for (int i = 0; i <= (int)cloudCurvature.size()-1; i++)
157 |     // {
158 |     //     int ind = cloudCurvature[i].id; 
159 | 
160 |     //     if( std::find(picked_points.begin(), picked_points.end(), ind)==picked_points.end()){
161 |     //         if(cloudCurvature[i].value > 0.1){
162 |     //             //ROS_WARN("extracted feature not qualified, please check lidar");
163 |     //             break;
164 |     //         }
165 |     //         smallestPickedNum++;
166 |     //         picked_points.push_back(ind);
167 |             
168 |     //         if(smallestPickedNum <= 4){
169 |     //             //find all points
170 |     //             pc_surf_flat->push_back(pc_in->points[ind]);
171 |     //             pc_surf_lessFlat->push_back(pc_in->points[ind]);
172 |     //             point_info_count++;
173 |     //         }
174 |     //         else{
175 |     //             break;
176 |     //         }
177 | 
178 |     //         for(int k=1;k<=5;k++){
179 |     //             double diffX = pc_in->points[ind + k].x - pc_in->points[ind + k - 1].x;
180 |     //             double diffY = pc_in->points[ind + k].y - pc_in->points[ind + k - 1].y;
181 |     //             double diffZ = pc_in->points[ind + k].z - pc_in->points[ind + k - 1].z;
182 |     //             if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){
183 |     //                 break;
184 |     //             }
185 |     //             picked_points.push_back(ind+k);
186 |     //         }
187 |     //         for(int k=-1;k>=-5;k--){
188 |     //             double diffX = pc_in->points[ind + k].x - pc_in->points[ind + k + 1].x;
189 |     //             double diffY = pc_in->points[ind + k].y - pc_in->points[ind + k + 1].y;
190 |     //             double diffZ = pc_in->points[ind + k].z - pc_in->points[ind + k + 1].z;
191 |     //             if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){
192 |     //                 break;
193 |     //             }
194 |     //             picked_points.push_back(ind+k);
195 |     //         }
196 | 
197 |     //     }
198 |     // }
199 |     
200 |     for (int i = 0; i <= (int)cloudCurvature.size()-1; i++)
201 |     {
202 |         int ind = cloudCurvature[i].id; 
203 |         if( std::find(picked_points.begin(), picked_points.end(), ind)==picked_points.end())
204 |         {
205 |             pc_out_surf->push_back(pc_in->points[ind]);
206 |         }
207 |     }
208 |     
209 | 
210 | 
211 | }
212 | LaserProcessingClass::LaserProcessingClass(){
213 |     
214 | }
215 | 
216 | Double2d::Double2d(int id_in, double value_in){
217 |     id = id_in;
218 |     value =value_in;
219 | };
220 | 
221 | PointsInfo::PointsInfo(int layer_in, double time_in){
222 |     layer = layer_in;
223 |     time = time_in;
224 | };
225 | 


--------------------------------------------------------------------------------
/floam/src/odomEstimationClass.cpp:
--------------------------------------------------------------------------------
  1 | // Author of FLOAM: Wang Han 
  2 | // Email wh200720041@gmail.com
  3 | // Homepage https://wanghan.pro
  4 | 
  5 | #include "odomEstimationClass.h"
  6 | 
  7 | void OdomEstimationClass::init(lidar::Lidar lidar_param, double map_resolution){
  8 |     //init local map
  9 |     laserCloudCornerMap = pcl::PointCloud<pcl::PointXYZI>::Ptr(new pcl::PointCloud<pcl::PointXYZI>());
 10 |     laserCloudSurfMap = pcl::PointCloud<pcl::PointXYZI>::Ptr(new pcl::PointCloud<pcl::PointXYZI>());
 11 | 
 12 |     //downsampling size
 13 |     downSizeFilterEdge.setLeafSize(map_resolution, map_resolution, map_resolution);
 14 |     downSizeFilterSurf.setLeafSize(map_resolution * 2, map_resolution * 2, map_resolution * 2);
 15 | 
 16 |     //kd-tree
 17 |     kdtreeEdgeMap = pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr(new pcl::KdTreeFLANN<pcl::PointXYZI>());
 18 |     kdtreeSurfMap = pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr(new pcl::KdTreeFLANN<pcl::PointXYZI>());
 19 | 
 20 |     odom = Eigen::Isometry3d::Identity();
 21 |     last_odom = Eigen::Isometry3d::Identity();
 22 |     optimization_count=2;
 23 | }
 24 | 
 25 | void OdomEstimationClass::initMapWithPoints(const pcl::PointCloud<pcl::PointXYZI>::Ptr& edge_in, const pcl::PointCloud<pcl::PointXYZI>::Ptr& surf_in){
 26 |     *laserCloudCornerMap += *edge_in;
 27 |     *laserCloudSurfMap += *surf_in;
 28 |     optimization_count=12;
 29 | }
 30 | 
 31 | 
 32 | void OdomEstimationClass::updatePointsToMap(const pcl::PointCloud<pcl::PointXYZI>::Ptr& edge_in, const pcl::PointCloud<pcl::PointXYZI>::Ptr& surf_in){
 33 | 
 34 |     if(optimization_count>2)
 35 |         optimization_count--;
 36 | 
 37 |     Eigen::Isometry3d odom_prediction = odom * (last_odom.inverse() * odom);
 38 |     last_odom = odom;
 39 |     odom = odom_prediction;
 40 | 
 41 |     q_w_curr = Eigen::Quaterniond(odom.rotation());
 42 |     t_w_curr = odom.translation();
 43 | 
 44 |     pcl::PointCloud<pcl::PointXYZI>::Ptr downsampledEdgeCloud(new pcl::PointCloud<pcl::PointXYZI>());
 45 |     pcl::PointCloud<pcl::PointXYZI>::Ptr downsampledSurfCloud(new pcl::PointCloud<pcl::PointXYZI>());
 46 |     downSamplingToMap(edge_in,downsampledEdgeCloud,surf_in,downsampledSurfCloud);
 47 |     //ROS_WARN("point nyum%d,%d",(int)downsampledEdgeCloud->points.size(), (int)downsampledSurfCloud->points.size());
 48 |     if(laserCloudCornerMap->points.size()>10 && laserCloudSurfMap->points.size()>50){
 49 |         kdtreeEdgeMap->setInputCloud(laserCloudCornerMap);
 50 |         kdtreeSurfMap->setInputCloud(laserCloudSurfMap);
 51 | 
 52 |         for (int iterCount = 0; iterCount < optimization_count; iterCount++){
 53 |             ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
 54 |             ceres::Problem::Options problem_options;
 55 |             ceres::Problem problem(problem_options);
 56 | 
 57 |             problem.AddParameterBlock(parameters, 7, new PoseSE3Parameterization());
 58 |             
 59 |             addEdgeCostFactor(downsampledEdgeCloud,laserCloudCornerMap,problem,loss_function);
 60 |             addSurfCostFactor(downsampledSurfCloud,laserCloudSurfMap,problem,loss_function);
 61 | 
 62 |             ceres::Solver::Options options;
 63 |             options.linear_solver_type = ceres::DENSE_QR;
 64 |             options.max_num_iterations = 4;
 65 |             options.minimizer_progress_to_stdout = false;
 66 |             options.check_gradients = false;
 67 |             options.gradient_check_relative_precision = 1e-4;
 68 |             ceres::Solver::Summary summary;
 69 | 
 70 |             ceres::Solve(options, &problem, &summary);
 71 | 
 72 |         }
 73 |     }else{
 74 |         printf("not enough points in map to associate, map error");
 75 |     }
 76 |     odom = Eigen::Isometry3d::Identity();
 77 |     odom.linear() = q_w_curr.toRotationMatrix();
 78 |     odom.translation() = t_w_curr;
 79 |     addPointsToMap(downsampledEdgeCloud,downsampledSurfCloud);
 80 | 
 81 | }
 82 | 
 83 | void OdomEstimationClass::pointAssociateToMap(pcl::PointXYZI const *const pi, pcl::PointXYZI *const po)
 84 | {
 85 |     Eigen::Vector3d point_curr(pi->x, pi->y, pi->z);
 86 |     Eigen::Vector3d point_w = q_w_curr * point_curr + t_w_curr;
 87 |     po->x = point_w.x();
 88 |     po->y = point_w.y();
 89 |     po->z = point_w.z();
 90 |     po->intensity = pi->intensity;
 91 |     //po->intensity = 1.0;
 92 | }
 93 | 
 94 | void OdomEstimationClass::downSamplingToMap(const pcl::PointCloud<pcl::PointXYZI>::Ptr& edge_pc_in, pcl::PointCloud<pcl::PointXYZI>::Ptr& edge_pc_out, const pcl::PointCloud<pcl::PointXYZI>::Ptr& surf_pc_in, pcl::PointCloud<pcl::PointXYZI>::Ptr& surf_pc_out){
 95 |     downSizeFilterEdge.setInputCloud(edge_pc_in);
 96 |     downSizeFilterEdge.filter(*edge_pc_out);
 97 |     downSizeFilterSurf.setInputCloud(surf_pc_in);
 98 |     downSizeFilterSurf.filter(*surf_pc_out);    
 99 | }
100 | 
101 | void OdomEstimationClass::addEdgeCostFactor(const pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_in, const pcl::PointCloud<pcl::PointXYZI>::Ptr& map_in, ceres::Problem& problem, ceres::LossFunction *loss_function){
102 |     int corner_num=0;
103 |     for (int i = 0; i < (int)pc_in->points.size(); i++)
104 |     {
105 |         pcl::PointXYZI point_temp;
106 |         pointAssociateToMap(&(pc_in->points[i]), &point_temp);
107 | 
108 |         std::vector<int> pointSearchInd;
109 |         std::vector<float> pointSearchSqDis;
110 |         kdtreeEdgeMap->nearestKSearch(point_temp, 5, pointSearchInd, pointSearchSqDis); 
111 |         if (pointSearchSqDis[4] < 1.0)
112 |         {
113 |             std::vector<Eigen::Vector3d> nearCorners;
114 |             Eigen::Vector3d center(0, 0, 0);
115 |             for (int j = 0; j < 5; j++)
116 |             {
117 |                 Eigen::Vector3d tmp(map_in->points[pointSearchInd[j]].x,
118 |                                     map_in->points[pointSearchInd[j]].y,
119 |                                     map_in->points[pointSearchInd[j]].z);
120 |                 center = center + tmp;
121 |                 nearCorners.push_back(tmp);
122 |             }
123 |             center = center / 5.0;
124 | 
125 |             Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
126 |             for (int j = 0; j < 5; j++)
127 |             {
128 |                 Eigen::Matrix<double, 3, 1> tmpZeroMean = nearCorners[j] - center;
129 |                 covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
130 |             }
131 | 
132 |             Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);
133 | 
134 |             Eigen::Vector3d unit_direction = saes.eigenvectors().col(2);
135 |             Eigen::Vector3d curr_point(pc_in->points[i].x, pc_in->points[i].y, pc_in->points[i].z);
136 |             if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])
137 |             { 
138 |                 Eigen::Vector3d point_on_line = center;
139 |                 Eigen::Vector3d point_a, point_b;
140 |                 point_a = 0.1 * unit_direction + point_on_line;
141 |                 point_b = -0.1 * unit_direction + point_on_line;
142 | 
143 |                 ceres::CostFunction *cost_function = new EdgeAnalyticCostFunction(curr_point, point_a, point_b);  
144 |                 problem.AddResidualBlock(cost_function, loss_function, parameters);
145 |                 corner_num++;   
146 |             }                           
147 |         }
148 |     }
149 |     if(corner_num<20){
150 |         printf("not enough correct points");
151 |     }
152 | 
153 | }
154 | 
155 | void OdomEstimationClass::addSurfCostFactor(const pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_in, const pcl::PointCloud<pcl::PointXYZI>::Ptr& map_in, ceres::Problem& problem, ceres::LossFunction *loss_function){
156 |     int surf_num=0;
157 |     for (int i = 0; i < (int)pc_in->points.size(); i++)
158 |     {
159 |         pcl::PointXYZI point_temp;
160 |         pointAssociateToMap(&(pc_in->points[i]), &point_temp);
161 |         std::vector<int> pointSearchInd;
162 |         std::vector<float> pointSearchSqDis;
163 |         kdtreeSurfMap->nearestKSearch(point_temp, 5, pointSearchInd, pointSearchSqDis);
164 | 
165 |         Eigen::Matrix<double, 5, 3> matA0;
166 |         Eigen::Matrix<double, 5, 1> matB0 = -1 * Eigen::Matrix<double, 5, 1>::Ones();
167 |         if (pointSearchSqDis[4] < 1.0)
168 |         {
169 |             
170 |             for (int j = 0; j < 5; j++)
171 |             {
172 |                 matA0(j, 0) = map_in->points[pointSearchInd[j]].x;
173 |                 matA0(j, 1) = map_in->points[pointSearchInd[j]].y;
174 |                 matA0(j, 2) = map_in->points[pointSearchInd[j]].z;
175 |             }
176 |             // find the norm of plane
177 |             Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0);
178 |             double negative_OA_dot_norm = 1 / norm.norm();
179 |             norm.normalize();
180 | 
181 |             bool planeValid = true;
182 |             for (int j = 0; j < 5; j++)
183 |             {
184 |                 // if OX * n > 0.2, then plane is not fit well
185 |                 if (fabs(norm(0) * map_in->points[pointSearchInd[j]].x +
186 |                          norm(1) * map_in->points[pointSearchInd[j]].y +
187 |                          norm(2) * map_in->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2)
188 |                 {
189 |                     planeValid = false;
190 |                     break;
191 |                 }
192 |             }
193 |             Eigen::Vector3d curr_point(pc_in->points[i].x, pc_in->points[i].y, pc_in->points[i].z);
194 |             if (planeValid)
195 |             {
196 |                 ceres::CostFunction *cost_function = new SurfNormAnalyticCostFunction(curr_point, norm, negative_OA_dot_norm);    
197 |                 problem.AddResidualBlock(cost_function, loss_function, parameters);
198 | 
199 |                 surf_num++;
200 |             }
201 |         }
202 | 
203 |     }
204 |     if(surf_num<20){
205 |         printf("not enough correct points");
206 |     }
207 | 
208 | }
209 | 
210 | void OdomEstimationClass::addPointsToMap(const pcl::PointCloud<pcl::PointXYZI>::Ptr& downsampledEdgeCloud, const pcl::PointCloud<pcl::PointXYZI>::Ptr& downsampledSurfCloud){
211 | 
212 |     for (int i = 0; i < (int)downsampledEdgeCloud->points.size(); i++)
213 |     {
214 |         pcl::PointXYZI point_temp;
215 |         pointAssociateToMap(&downsampledEdgeCloud->points[i], &point_temp);
216 |         laserCloudCornerMap->push_back(point_temp); 
217 |     }
218 |     
219 |     for (int i = 0; i < (int)downsampledSurfCloud->points.size(); i++)
220 |     {
221 |         pcl::PointXYZI point_temp;
222 |         pointAssociateToMap(&downsampledSurfCloud->points[i], &point_temp);
223 |         laserCloudSurfMap->push_back(point_temp);
224 |     }
225 |     
226 |     double x_min = +odom.translation().x()-100;
227 |     double y_min = +odom.translation().y()-100;
228 |     double z_min = +odom.translation().z()-100;
229 |     double x_max = +odom.translation().x()+100;
230 |     double y_max = +odom.translation().y()+100;
231 |     double z_max = +odom.translation().z()+100;
232 |     
233 |     //ROS_INFO("size : %f,%f,%f,%f,%f,%f", x_min, y_min, z_min,x_max, y_max, z_max);
234 |     cropBoxFilter.setMin(Eigen::Vector4f(x_min, y_min, z_min, 1.0));
235 |     cropBoxFilter.setMax(Eigen::Vector4f(x_max, y_max, z_max, 1.0));
236 |     cropBoxFilter.setNegative(false);    
237 | 
238 |     pcl::PointCloud<pcl::PointXYZI>::Ptr tmpCorner(new pcl::PointCloud<pcl::PointXYZI>());
239 |     pcl::PointCloud<pcl::PointXYZI>::Ptr tmpSurf(new pcl::PointCloud<pcl::PointXYZI>());
240 |     cropBoxFilter.setInputCloud(laserCloudSurfMap);
241 |     cropBoxFilter.filter(*tmpSurf);
242 |     cropBoxFilter.setInputCloud(laserCloudCornerMap);
243 |     cropBoxFilter.filter(*tmpCorner);
244 | 
245 |     downSizeFilterSurf.setInputCloud(tmpSurf);
246 |     downSizeFilterSurf.filter(*laserCloudSurfMap);
247 |     downSizeFilterEdge.setInputCloud(tmpCorner);
248 |     downSizeFilterEdge.filter(*laserCloudCornerMap);
249 | 
250 | }
251 | 
252 | void OdomEstimationClass::getMap(pcl::PointCloud<pcl::PointXYZI>::Ptr& laserCloudMap){
253 |     
254 |     *laserCloudMap += *laserCloudSurfMap;
255 |     *laserCloudMap += *laserCloudCornerMap;
256 | }
257 | 
258 | OdomEstimationClass::OdomEstimationClass(){
259 | 
260 | }
261 | 


--------------------------------------------------------------------------------
/SC-A-LOAM/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<float> eig2stdvec( MatrixXd _eigmat )
 63 | {
 64 |     std::vector<float> 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<double, int> 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<int> 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<SCPointType> & _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<float> 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<SCPointType> & _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<float> 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<int, float> SCManager::detectLoopClosureIDBetweenSession (std::vector<float>& _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<InvKeyTree>(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<size_t> candidate_indexes( NUM_CANDIDATES_FROM_TREE ); 
297 |     std::vector<float> out_dists_sqr( NUM_CANDIDATES_FROM_TREE );
298 | 
299 |     nanoflann::KNNResultSet<float> 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<double, int> 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<int, float> result {loop_id, yaw_diff_rad};
330 | 
331 |     return result;
332 | 
333 | } // SCManager::detectLoopClosureIDBetweenSession
334 | 
335 | 
336 | std::pair<int, float> 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<int, float> 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<InvKeyTree>(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<size_t> candidate_indexes( NUM_CANDIDATES_FROM_TREE ); 
373 |     std::vector<float> out_dists_sqr( NUM_CANDIDATES_FROM_TREE );
374 | 
375 |     TicTocV2 t_tree_search;
376 |     nanoflann::KNNResultSet<float> 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<double, int> 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<int, float> result {loop_id, yaw_diff_rad};
424 | 
425 |     return result;
426 | 
427 | } // SCManager::detectLoopClosureID
428 | 
429 | // } // namespace SC2


--------------------------------------------------------------------------------
/SC-A-LOAM/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: <Fixed 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 | 


--------------------------------------------------------------------------------
/SC-A-LOAM/src/scanRegistration.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 | 
 38 | #include <cmath>
 39 | #include <vector>
 40 | #include <string>
 41 | #include "aloam_velodyne/common.h"
 42 | #include "aloam_velodyne/tic_toc.h"
 43 | #include <nav_msgs/Odometry.h>
 44 | #include <opencv/cv.h>
 45 | #include <pcl_conversions/pcl_conversions.h>
 46 | #include <pcl/point_cloud.h>
 47 | #include <pcl/point_types.h>
 48 | #include <pcl/filters/voxel_grid.h>
 49 | #include <pcl/kdtree/kdtree_flann.h>
 50 | #include <ros/ros.h>
 51 | #include <sensor_msgs/Imu.h>
 52 | #include <sensor_msgs/PointCloud2.h>
 53 | #include <tf/transform_datatypes.h>
 54 | #include <tf/transform_broadcaster.h>
 55 | 
 56 | using std::atan2;
 57 | using std::cos;
 58 | using std::sin;
 59 | 
 60 | std::string LIDAR_TYPE;
 61 | 
 62 | const double scanPeriod = 0.1;
 63 | 
 64 | const int systemDelay = 0; 
 65 | int systemInitCount = 0;
 66 | bool systemInited = false;
 67 | int N_SCANS = 0;
 68 | float cloudCurvature[400000];
 69 | int cloudSortInd[400000];
 70 | int cloudNeighborPicked[400000];
 71 | int cloudLabel[400000];
 72 | 
 73 | bool comp (int i,int j) { return (cloudCurvature[i]<cloudCurvature[j]); }
 74 | 
 75 | ros::Publisher pubLaserCloud;
 76 | ros::Publisher pubCornerPointsSharp;
 77 | ros::Publisher pubCornerPointsLessSharp;
 78 | ros::Publisher pubSurfPointsFlat;
 79 | ros::Publisher pubSurfPointsLessFlat;
 80 | ros::Publisher pubRemovePoints;
 81 | std::vector<ros::Publisher> pubEachScan;
 82 | 
 83 | bool PUB_EACH_LINE = false;
 84 | 
 85 | double MINIMUM_RANGE = 0.1; 
 86 | 
 87 | template <typename PointT>
 88 | void removeClosedPointCloud(const pcl::PointCloud<PointT> &cloud_in,
 89 |                               pcl::PointCloud<PointT> &cloud_out, float thres)
 90 | {
 91 |     if (&cloud_in != &cloud_out)
 92 |     {
 93 |         cloud_out.header = cloud_in.header;
 94 |         cloud_out.points.resize(cloud_in.points.size());
 95 |     }
 96 | 
 97 |     size_t j = 0;
 98 | 
 99 |     for (size_t i = 0; i < cloud_in.points.size(); ++i)
100 |     {
101 |         if (cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y + cloud_in.points[i].z * cloud_in.points[i].z < thres * thres)
102 |             continue;
103 |         cloud_out.points[j] = cloud_in.points[i];
104 |         j++;
105 |     }
106 |     if (j != cloud_in.points.size())
107 |     {
108 |         cloud_out.points.resize(j);
109 |     }
110 | 
111 |     cloud_out.height = 1;
112 |     cloud_out.width = static_cast<uint32_t>(j);
113 |     cloud_out.is_dense = true;
114 | }
115 | 
116 | void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
117 | {
118 |     if (!systemInited)
119 |     { 
120 |         systemInitCount++;
121 |         if (systemInitCount >= systemDelay)
122 |         {
123 |             systemInited = true;
124 |         }
125 |         else
126 |             return;
127 |     }
128 | 
129 |     TicToc t_whole;
130 |     TicToc t_prepare;
131 |     std::vector<int> scanStartInd(N_SCANS, 0);
132 |     std::vector<int> scanEndInd(N_SCANS, 0);
133 | 
134 |     pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
135 |     pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
136 |     std::vector<int> indices;
137 | 
138 |     pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
139 |     removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);
140 | 
141 | 
142 |     int cloudSize = laserCloudIn.points.size();
143 |     float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
144 |     float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
145 |                           laserCloudIn.points[cloudSize - 1].x) +
146 |                    2 * M_PI;
147 | 
148 |     if (endOri - startOri > 3 * M_PI)
149 |     {
150 |         endOri -= 2 * M_PI;
151 |     }
152 |     else if (endOri - startOri < M_PI)
153 |     {
154 |         endOri += 2 * M_PI;
155 |     }
156 |     ////printf("end Ori %f\n", endOri);
157 | 
158 |     bool halfPassed = false;
159 |     int count = cloudSize;
160 |     PointType point;
161 |     std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);
162 |     for (int i = 0; i < cloudSize; i++)
163 |     {
164 |         point.x = laserCloudIn.points[i].x;
165 |         point.y = laserCloudIn.points[i].y;
166 |         point.z = laserCloudIn.points[i].z;
167 | 
168 |         float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
169 |         int scanID = 0;
170 | 
171 |         if (LIDAR_TYPE == "VLP16" && N_SCANS == 16)
172 |         {
173 |             scanID = int((angle + 15) / 2 + 0.5);
174 |             if (scanID > (N_SCANS - 1) || scanID < 0)
175 |             {
176 |                 count--;
177 |                 continue;
178 |             }
179 |         }
180 |         else if (LIDAR_TYPE == "HDL32" && N_SCANS == 32)
181 |         {
182 |             scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);
183 |             if (scanID > (N_SCANS - 1) || scanID < 0)
184 |             {
185 |                 count--;
186 |                 continue;
187 |             }
188 |         }
189 |         // HDL64 (e.g., KITTI)
190 |         else if (LIDAR_TYPE == "HDL64" && N_SCANS == 64)
191 |         {   
192 |             if (angle >= -8.83)
193 |                 scanID = int((2 - angle) * 3.0 + 0.5);
194 |             else
195 |                 scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);
196 | 
197 |             // use [0 50]  > 50 remove outlies 
198 |             if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
199 |             {
200 |                 count--;
201 |                 continue;
202 |             }
203 |         }
204 |         // Ouster OS1-64 (e.g., MulRan)
205 |         else if (LIDAR_TYPE == "OS1-64" && N_SCANS == 64)
206 |         {   
207 |             scanID = int((angle + 22.5) / 2 + 0.5); // ouster os1-64 vfov is [-22.5, 22.5] see https://ouster.com/products/os1-lidar-sensor/
208 |             if (scanID > (N_SCANS - 1) || scanID < 0)
209 |             {
210 |                 count--;
211 |                 continue;
212 |             }
213 |         }
214 |         else
215 |         {
216 |             //printf("wrong scan number\n");
217 |             ROS_BREAK();
218 |         }
219 |         ////printf("angle %f scanID %d \n", angle, scanID);
220 | 
221 |         float ori = -atan2(point.y, point.x);
222 |         if (!halfPassed)
223 |         { 
224 |             if (ori < startOri - M_PI / 2)
225 |             {
226 |                 ori += 2 * M_PI;
227 |             }
228 |             else if (ori > startOri + M_PI * 3 / 2)
229 |             {
230 |                 ori -= 2 * M_PI;
231 |             }
232 | 
233 |             if (ori - startOri > M_PI)
234 |             {
235 |                 halfPassed = true;
236 |             }
237 |         }
238 |         else
239 |         {
240 |             ori += 2 * M_PI;
241 |             if (ori < endOri - M_PI * 3 / 2)
242 |             {
243 |                 ori += 2 * M_PI;
244 |             }
245 |             else if (ori > endOri + M_PI / 2)
246 |             {
247 |                 ori -= 2 * M_PI;
248 |             }
249 |         }
250 | 
251 |         float relTime = (ori - startOri) / (endOri - startOri);
252 |         point.intensity = scanID + scanPeriod * relTime;
253 |         laserCloudScans[scanID].push_back(point); 
254 |     }
255 |     
256 |     cloudSize = count;
257 |     //printf("points size %d \n", cloudSize);
258 | 
259 |     pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
260 |     for (int i = 0; i < N_SCANS; i++)
261 |     { 
262 |         scanStartInd[i] = laserCloud->size() + 5;
263 |         *laserCloud += laserCloudScans[i];
264 |         scanEndInd[i] = laserCloud->size() - 6;
265 |     }
266 | 
267 |     //printf("prepare time %f \n", t_prepare.toc());
268 | 
269 |     for (int i = 5; i < cloudSize - 5; i++)
270 |     { 
271 |         float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;
272 |         float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;
273 |         float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;
274 | 
275 |         cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
276 |         cloudSortInd[i] = i;
277 |         cloudNeighborPicked[i] = 0;
278 |         cloudLabel[i] = 0;
279 |     }
280 | 
281 | 
282 |     TicToc t_pts;
283 | 
284 |     pcl::PointCloud<PointType> cornerPointsSharp;
285 |     pcl::PointCloud<PointType> cornerPointsLessSharp;
286 |     pcl::PointCloud<PointType> surfPointsFlat;
287 |     pcl::PointCloud<PointType> surfPointsLessFlat;
288 | 
289 |     float t_q_sort = 0;
290 |     for (int i = 0; i < N_SCANS; i++)
291 |     {
292 |         if( scanEndInd[i] - scanStartInd[i] < 6)
293 |             continue;
294 |         pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
295 |         for (int j = 0; j < 6; j++)
296 |         {
297 |             int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6; 
298 |             int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;
299 | 
300 |             TicToc t_tmp;
301 |             std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);
302 |             t_q_sort += t_tmp.toc();
303 | 
304 |             int largestPickedNum = 0;
305 |             for (int k = ep; k >= sp; k--)
306 |             {
307 |                 int ind = cloudSortInd[k]; 
308 | 
309 |                 if (cloudNeighborPicked[ind] == 0 &&
310 |                     cloudCurvature[ind] > 0.1)
311 |                 {
312 | 
313 |                     largestPickedNum++;
314 |                     if (largestPickedNum <= 2)
315 |                     {                        
316 |                         cloudLabel[ind] = 2;
317 |                         cornerPointsSharp.push_back(laserCloud->points[ind]);
318 |                         cornerPointsLessSharp.push_back(laserCloud->points[ind]);
319 |                     }
320 |                     else if (largestPickedNum <= 20)
321 |                     {                        
322 |                         cloudLabel[ind] = 1; 
323 |                         cornerPointsLessSharp.push_back(laserCloud->points[ind]);
324 |                     }
325 |                     else
326 |                     {
327 |                         break;
328 |                     }
329 | 
330 |                     cloudNeighborPicked[ind] = 1; 
331 | 
332 |                     for (int l = 1; l <= 5; l++)
333 |                     {
334 |                         float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
335 |                         float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
336 |                         float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
337 |                         if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
338 |                         {
339 |                             break;
340 |                         }
341 | 
342 |                         cloudNeighborPicked[ind + l] = 1;
343 |                     }
344 |                     for (int l = -1; l >= -5; l--)
345 |                     {
346 |                         float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
347 |                         float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
348 |                         float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
349 |                         if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
350 |                         {
351 |                             break;
352 |                         }
353 | 
354 |                         cloudNeighborPicked[ind + l] = 1;
355 |                     }
356 |                 }
357 |             }
358 | 
359 |             int smallestPickedNum = 0;
360 |             for (int k = sp; k <= ep; k++)
361 |             {
362 |                 int ind = cloudSortInd[k];
363 | 
364 |                 if (cloudNeighborPicked[ind] == 0 &&
365 |                     cloudCurvature[ind] < 0.1)
366 |                 {
367 | 
368 |                     cloudLabel[ind] = -1; 
369 |                     surfPointsFlat.push_back(laserCloud->points[ind]);
370 | 
371 |                     smallestPickedNum++;
372 |                     if (smallestPickedNum >= 4)
373 |                     { 
374 |                         break;
375 |                     }
376 | 
377 |                     cloudNeighborPicked[ind] = 1;
378 |                     for (int l = 1; l <= 5; l++)
379 |                     { 
380 |                         float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
381 |                         float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
382 |                         float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
383 |                         if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
384 |                         {
385 |                             break;
386 |                         }
387 | 
388 |                         cloudNeighborPicked[ind + l] = 1;
389 |                     }
390 |                     for (int l = -1; l >= -5; l--)
391 |                     {
392 |                         float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
393 |                         float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
394 |                         float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
395 |                         if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
396 |                         {
397 |                             break;
398 |                         }
399 | 
400 |                         cloudNeighborPicked[ind + l] = 1;
401 |                     }
402 |                 }
403 |             }
404 | 
405 |             for (int k = sp; k <= ep; k++)
406 |             {
407 |                 if (cloudLabel[k] <= 0)
408 |                 {
409 |                     surfPointsLessFlatScan->push_back(laserCloud->points[k]);
410 |                 }
411 |             }
412 |         }
413 | 
414 |         pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
415 |         pcl::VoxelGrid<PointType> downSizeFilter;
416 |         downSizeFilter.setInputCloud(surfPointsLessFlatScan);
417 |         downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
418 |         downSizeFilter.filter(surfPointsLessFlatScanDS);
419 | 
420 |         surfPointsLessFlat += surfPointsLessFlatScanDS;
421 |     }
422 |     //printf("sort q time %f \n", t_q_sort);
423 |     //printf("seperate points time %f \n", t_pts.toc());
424 | 
425 | 
426 |     sensor_msgs::PointCloud2 laserCloudOutMsg;
427 |     pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
428 |     laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
429 |     laserCloudOutMsg.header.frame_id = "/camera_init";
430 |     pubLaserCloud.publish(laserCloudOutMsg);
431 | 
432 |     sensor_msgs::PointCloud2 cornerPointsSharpMsg;
433 |     pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
434 |     cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
435 |     cornerPointsSharpMsg.header.frame_id = "/camera_init";
436 |     pubCornerPointsSharp.publish(cornerPointsSharpMsg);
437 | 
438 |     sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
439 |     pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
440 |     cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
441 |     cornerPointsLessSharpMsg.header.frame_id = "/camera_init";
442 |     pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);
443 | 
444 |     sensor_msgs::PointCloud2 surfPointsFlat2;
445 |     pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
446 |     surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
447 |     surfPointsFlat2.header.frame_id = "/camera_init";
448 |     pubSurfPointsFlat.publish(surfPointsFlat2);
449 | 
450 |     sensor_msgs::PointCloud2 surfPointsLessFlat2;
451 |     pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
452 |     surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
453 |     surfPointsLessFlat2.header.frame_id = "/camera_init";
454 |     pubSurfPointsLessFlat.publish(surfPointsLessFlat2);
455 |     // std::cout << "published ... " << std::endl;
456 | 
457 |     // pub each scam
458 |     if(PUB_EACH_LINE)
459 |     {
460 |         for(int i = 0; i< N_SCANS; i++)
461 |         {
462 |             sensor_msgs::PointCloud2 scanMsg;
463 |             pcl::toROSMsg(laserCloudScans[i], scanMsg);
464 |             scanMsg.header.stamp = laserCloudMsg->header.stamp;
465 |             scanMsg.header.frame_id = "/camera_init";
466 |             pubEachScan[i].publish(scanMsg);
467 |         }
468 |     }
469 | 
470 |     //printf("scan registration time %f ms *************\n", t_whole.toc());
471 |     if(t_whole.toc() > 100)
472 |         ROS_WARN("scan registration process over 100ms");
473 | }
474 | 
475 | int main(int argc, char **argv)
476 | {
477 |     ros::init(argc, argv, "scanRegistration");
478 |     ros::NodeHandle nh;
479 | 
480 |     nh.param<int>("scan_line", N_SCANS, 16);
481 |     nh.param<std::string>("lidar_type", LIDAR_TYPE, "KITTI");
482 |     nh.param<double>("minimum_range", MINIMUM_RANGE, 0.1);
483 | 
484 |     //printf("scan line number %d \n", N_SCANS);
485 | 
486 |     if(N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64)
487 |     {
488 |         //printf("only support velodyne with 16, 32 or 64 scan line!");
489 |         return 0;
490 |     }
491 | 
492 |     ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, laserCloudHandler);
493 | 
494 |     pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100);
495 | 
496 |     pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100);
497 | 
498 |     pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100);
499 | 
500 |     pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100);
501 | 
502 |     pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100);
503 | 
504 |     pubRemovePoints = nh.advertise<sensor_msgs::PointCloud2>("/laser_remove_points", 100);
505 | 
506 |     if(PUB_EACH_LINE)
507 |     {
508 |         for(int i = 0; i < N_SCANS; i++)
509 |         {
510 |             ros::Publisher tmp = nh.advertise<sensor_msgs::PointCloud2>("/laser_scanid_" + std::to_string(i), 100);
511 |             pubEachScan.push_back(tmp);
512 |         }
513 |     }
514 |     ros::spin();
515 | 
516 |     return 0;
517 | }
518 | 


--------------------------------------------------------------------------------