├── .gitignore
├── README.md
├── floam
├── CMakeLists.txt
├── LICENSE
├── PCD
│ └── readme.txt
├── README.md
├── img
│ ├── floam_mapping.gif
│ └── kitti_example.gif
├── include
│ ├── laserMappingClass.h
│ ├── laserProcessingClass.h
│ ├── lidar.h
│ ├── lidarOptimization.h
│ └── odomEstimationClass.h
├── launch
│ ├── floam.launch
│ ├── floam_mapping.launch
│ ├── floam_rsbpearl32.launch
│ ├── floam_rslidar32.launch
│ ├── floam_velodyne.launch
│ └── floam_velodyne1.launch
├── package.xml
├── rviz
│ ├── floam.rviz
│ ├── floam_mapping.rviz
│ └── floam_velodyne.rviz
└── src
│ ├── laserMappingClass.cpp
│ ├── laserMappingNode.cpp
│ ├── laserProcessingClass.cpp
│ ├── laserProcessingNode.cpp
│ ├── lidar.cpp
│ ├── lidarOptimization.cpp
│ ├── odomEstimationClass.cpp
│ └── odomEstimationNode.cpp
├── livox_camera_calib
├── CMakeLists.txt
├── LICENSE
├── README.md
├── config
│ ├── camera.yaml
│ ├── camera_rsbpearl32.yaml
│ ├── camera_velodyne16.yaml
│ ├── camera_velodyne16_r.yaml
│ ├── config_indoor.yaml
│ ├── config_indoor_rsbpearl32.yaml
│ ├── config_indoor_velodyne16.yaml
│ ├── config_indoor_velodyne16_r.yaml
│ └── config_outdoor.yaml
├── include
│ ├── CustomMsg.h
│ ├── CustomPoint.h
│ ├── common.h
│ └── lidar_camera_calib.hpp
├── launch
│ ├── calib_indoor_livox.launch
│ ├── calib_indoor_rsbpearl32.launch
│ ├── calib_indoor_velodyne16.launch
│ ├── calib_indoor_velodyne16_r.launch
│ └── calib_outdoor.launch
├── package.xml
├── pics
│ └── color_cloud.png
├── result
│ ├── extrinsic copy.txt
│ └── extrinsic.txt
├── rviz_cfg
│ ├── calib.rviz
│ └── calib_indoor.rviz
└── src
│ └── lidar_camera_calib.cpp
└── pics
├── calib_result.png
├── floam_save_pcd.png
└── map_pcd.png
/.gitignore:
--------------------------------------------------------------------------------
1 | *.pcd
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # extended_lidar_camera_calib
2 |
3 | This work is an extended version of [livox_camera_calib](https://github.com/hku-mars/livox_camera_calib.git), which is suitable for spinning LiDAR。
4 |
5 | In order to apply this algorithm on spinning LIDAR(e.g:VLP16)， I add the preprocess process([FLOAM](https://github.com/wh200720041/floam.git)) to make the point cloud of the spinning LiDAR denser.
6 |
7 | ## Data Prepare
8 |
9 | When you calibrate the spinning lidar and camera, record the data by holding the device(lidar and camera) stationary for a period of time and then slowly and repeatedly move the entire device in this direction to accumulate the point cloud.
10 |
11 | ## Dependency
12 | ```
13 | ceres-slover == 1.14.0
14 | pcl==1.8.0
15 | ```
16 |
17 | ## Build
18 | ```
19 | cd ~/catkin_ws/src
20 | git clone https://github.com/AFEICHINA/extended_lidar_camera_calib.git
21 | cd ..
22 | catkin_make
23 | source ~/catkin_ws/devel/setup.bash
24 | ```
25 |
26 | ## Run
27 | step1: doing slam to accumulate dense pointcloud.
28 | ```
29 | roslaunch floam floam_XXX.launch
30 | ```
31 |
32 |
33 | step2: lidar camera calibration
34 | ```
35 | roslaunch livox_camera_calib calib_XXX.launch
36 | ```
37 |
38 | ## My Result
39 | In my test, I get better results in indoor enviroment.
40 | ```
41 | LIDAR : Robosense RS-Bpearl
42 | Camera: MindVision MV-SUA133GC-T
43 | ```
44 | slam result:
45 |
46 | 
47 |
48 |
49 | map result:
50 |
51 | 
52 |
53 |
54 | calib result:
55 |
56 | 
57 |
58 |
59 | ## Acknowledgements
60 | Thanks for [livox_camera_calib](https://github.com/hku-mars/livox_camera_calib.git) and [FLOAM](https://github.com/wh200720041/floam.git).
61 |
--------------------------------------------------------------------------------
/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})
--------------------------------------------------------------------------------
/floam/LICENSE:
--------------------------------------------------------------------------------
1 | This is an optimized 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 | And this work is modified based on Advanced implementation of LOAM (A-LOAM) by
6 | Tong Qin qintonguav@gmail.com
7 | Shaozu Cao saozu.cao@connect.ust.hk
8 |
9 | Author of FLOAM
10 | Wang Han
11 | Nanyang Technological University, Singapore
12 | Email: wh200720041@gmail.com
13 | Homepage: https://wanghan.pro
14 |
15 | Copyright 2013, Ji Zhang, Carnegie Mellon University
16 | Further contributions copyright (c) 2016, Southwest Research Institute
17 | All rights reserved.
18 |
19 | Redistribution and use in source and binary forms, with or without
20 | modification, are permitted provided that the following conditions are met:
21 |
22 | 1. Redistributions of source code must retain the above copyright notice, this
23 | list of conditions and the following disclaimer.
24 | 2. Redistributions in binary form must reproduce the above copyright notice,
25 | this list of conditions and the following disclaimer in the documentation
26 | and/or other materials provided with the distribution.
27 | 3. Neither the name of the copyright holder nor the names of its contributors
28 | may be used to endorse or promote products derived from this software without
29 | specific prior written permission.
30 |
31 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
32 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
33 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
34 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
35 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
36 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
37 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
38 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,OR
39 | TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
40 | THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
41 |
--------------------------------------------------------------------------------
/floam/PCD/readme.txt:
--------------------------------------------------------------------------------
1 | pcd files are saved here.
--------------------------------------------------------------------------------
/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. Modification Highlights
10 | This includes some optimization on the original implementation
11 | 1. Analytic methods is used instead of auto differentiation. This is performed on se3
12 | 2. Use linear motion prediction model to estimate the initial pose
13 | 3. Laser odometry and laser mapping are merged
14 | 4. A dynamic local map is used instead of global map, in order to save memory cost. Based on massive experiments, this only has slight influence on the performance.
15 |
16 | ## 2. Evaluation
17 | ### 2.1. Computational efficiency evaluation
18 | Computational efficiency evaluation (based on KITTI dataset):
19 | Platform: Intel® Core™ i7-8700 CPU @ 3.20GHz
20 | | Dataset | ALOAM | FLOAM |
21 | |----------------------------------------------|----------------------------|------------------------|
22 | | `KITTI` | 151ms | 59ms |
23 |
24 | Localization error:
25 | | Dataset | ALOAM | FLOAM |
26 | |----------------------------------------------|----------------------------|------------------------|
27 | | `KITTI sequence 00` | 0.55% | 0.51% |
28 | | `KITTI sequence 02` | 3.93% | 1.25% |
29 | | `KITTI sequence 05` | 1.28% | 0.93% |
30 |
31 | ### 2.2. localization result
32 |
33 | 
34 |
35 |
36 | ### 2.3. mapping result
37 |
38 |
39 | 
40 |
41 |
42 |
43 | ## 3. Prerequisites
44 | ### 3.1 **Ubuntu** and **ROS**
45 | Ubuntu 64-bit 18.04.
46 |
47 | ROS Melodic. [ROS Installation](http://wiki.ros.org/ROS/Installation)
48 |
49 | ### 3.2. **Ceres Solver**
50 | Follow [Ceres Installation](http://ceres-solver.org/installation.html).
51 |
52 | ### 3.3. **PCL**
53 | Follow [PCL Installation](http://www.pointclouds.org/downloads/linux.html).
54 |
55 | ### 3.4. **Trajectory visualization**
56 | For visualization purpose, this package uses hector trajectory sever, you may install the package by
57 | ```
58 | sudo apt-get install ros-melodic-hector-trajectory-server
59 | ```
60 | Alternatively, you may remove the hector trajectory server node if trajectory visualization is not needed
61 |
62 | ## 4. Build
63 | ### 4.1 Clone repository:
64 | ```
65 | cd ~/catkin_ws/src
66 | git clone https://github.com/wh200720041/floam.git
67 | cd ..
68 | catkin_make
69 | source ~/catkin_ws/devel/setup.bash
70 | ```
71 | ### 4.2 Download test rosbag
72 | Download [KITTI sequence 05](https://drive.google.com/open?id=18ilF7GZDg2tmT6sD5pd1RjqO0XJLn9Mv) or [KITTI sequence 07](https://drive.google.com/open?id=1VpoKm7f4es4ISQ-psp4CV3iylcA4eu0-)
73 |
74 | Unzip compressed file 2011_09_30_0018.zip. If your system does not have unzip. please install unzip by
75 | ```
76 | sudo apt-get install unzip
77 | ```
78 |
79 | And then copy the file 2011_09_30_0018.bag into ~/catkin_ws/src/floam/dataset/ (this may take a few minutes to unzip the file)
80 | ```
81 | cd ~/catkin_ws/src/floam/dataset/
82 | unzip ~/Downloads/2011_09_30_0018.zip
83 | ```
84 |
85 | ### 4.3 Launch ROS
86 | ```
87 | roslaunch floam floam.launch
88 | ```
89 | if you would like to create the map at the same time, you can run (more cpu cost)
90 | ```
91 | roslaunch floam floam_mapping.launch
92 | ```
93 | 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)
94 |
95 |
96 | ## 5. Test on other sequence
97 | To generate rosbag file of kitti dataset, you may use the tools provided by
98 | [kitti_to_rosbag](https://github.com/ethz-asl/kitti_to_rosbag) or [kitti2bag](https://github.com/tomas789/kitti2bag)
99 |
100 | ## 6. Test on Velodyne VLP-16 or HDL-32
101 | You may wish to test FLOAM on your own platform and sensor such as VLP-16
102 | You can install the velodyne sensor driver by
103 | ```
104 | sudo apt-get install ros-melodic-velodyne-pointcloud
105 | ```
106 | launch floam for your own velodyne sensor
107 | ```
108 | roslaunch floam floam_velodyne.launch
109 | ```
110 | If you are using HDL-32 or other sensor, please change the scan_line in the launch file
111 |
112 |
113 | ## 7.Acknowledgements
114 | 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).
115 |
116 |
--------------------------------------------------------------------------------
/floam/img/floam_mapping.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/AFEICHINA/extended_lidar_camera_calib/dcb4a852be780104f8f52edf6e684fb0d83076dc/floam/img/floam_mapping.gif
--------------------------------------------------------------------------------
/floam/img/kitti_example.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/AFEICHINA/extended_lidar_camera_calib/dcb4a852be780104f8f52edf6e684fb0d83076dc/floam/img/kitti_example.gif
--------------------------------------------------------------------------------
/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
10 | #include
11 | #include
12 | #include
13 | #include
14 | #include
15 |
16 | //eigen lib
17 | #include
18 | #include
19 |
20 | //c++ lib
21 | #include
22 | #include
23 | #include
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::Ptr& pc_in, const Eigen::Isometry3d& pose_current);
43 | pcl::PointCloud::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::Ptr>>> map;
53 | pcl::VoxelGrid 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/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
8 | #include
9 |
10 | #include
11 | #include
12 | #include
13 | #include
14 | #include
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::Ptr& pc_in, pcl::PointCloud::Ptr& pc_out_edge, pcl::PointCloud::Ptr& pc_out_surf);
40 | void rsbp_featureExtraction(const pcl::PointCloud::Ptr& pc_in, pcl::PointCloud::Ptr& pc_out_edge, pcl::PointCloud::Ptr& pc_out_surf);
41 | void featureExtractionFromSector(const pcl::PointCloud::Ptr& pc_in, std::vector& cloudCurvature, pcl::PointCloud::Ptr& pc_out_edge, pcl::PointCloud::Ptr& pc_out_surf);
42 | private:
43 | lidar::Lidar lidar_param;
44 | };
45 |
46 |
47 |
48 | #endif // _LASER_PROCESSING_CLASS_H_
49 |
50 |
--------------------------------------------------------------------------------
/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 |
--------------------------------------------------------------------------------
/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
8 | #include
9 | #include
10 | #include
11 |
12 | void getTransformFromSe3(const Eigen::Matrix& se3, Eigen::Quaterniond& q, Eigen::Vector3d& t);
13 |
14 | Eigen::Matrix3d skew(Eigen::Vector3d& mat_in);
15 |
16 | class EdgeAnalyticCostFunction : public ceres::SizedCostFunction<3, 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 |
--------------------------------------------------------------------------------
/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
9 | #include
10 | #include
11 |
12 | //PCL
13 | #include
14 | #include
15 | #include
16 | #include
17 | #include
18 | #include
19 | #include
20 | #include
21 | #include
22 |
23 | //ceres
24 | #include
25 | #include
26 |
27 | //eigen
28 | #include
29 | #include
30 |
31 | //LOCAL LIB
32 | #include "lidar.h"
33 | #include "lidarOptimization.h"
34 | #include
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::Ptr& edge_in, const pcl::PointCloud::Ptr& surf_in);
44 | void updatePointsToMap(const pcl::PointCloud::Ptr& edge_in, const pcl::PointCloud::Ptr& surf_in);
45 | void getMap(pcl::PointCloud::Ptr& laserCloudMap);
46 |
47 | Eigen::Isometry3d odom;
48 | pcl::PointCloud::Ptr laserCloudCornerMap;
49 | pcl::PointCloud::Ptr laserCloudSurfMap;
50 | private:
51 | //optimization variable
52 | double parameters[7] = {0, 0, 0, 1, 0, 0, 0};
53 | Eigen::Map q_w_curr = Eigen::Map(parameters);
54 | Eigen::Map t_w_curr = Eigen::Map(parameters + 4);
55 |
56 | Eigen::Isometry3d last_odom;
57 |
58 | //kd-tree
59 | pcl::KdTreeFLANN::Ptr kdtreeEdgeMap;
60 | pcl::KdTreeFLANN::Ptr kdtreeSurfMap;
61 |
62 | //points downsampling before add to map
63 | pcl::VoxelGrid downSizeFilterEdge;
64 | pcl::VoxelGrid downSizeFilterSurf;
65 |
66 | //local map
67 | pcl::CropBox cropBoxFilter;
68 |
69 | //optimization count
70 | int optimization_count;
71 |
72 | //function
73 | void addEdgeCostFactor(const pcl::PointCloud::Ptr& pc_in, const pcl::PointCloud::Ptr& map_in, ceres::Problem& problem, ceres::LossFunction *loss_function);
74 | void addSurfCostFactor(const pcl::PointCloud::Ptr& pc_in, const pcl::PointCloud::Ptr& map_in, ceres::Problem& problem, ceres::LossFunction *loss_function);
75 | void addPointsToMap(const pcl::PointCloud::Ptr& downsampledEdgeCloud, const pcl::PointCloud::Ptr& downsampledSurfCloud);
76 | void pointAssociateToMap(pcl::PointXYZI const *const pi, pcl::PointXYZI *const po);
77 | void downSamplingToMap(const pcl::PointCloud::Ptr& edge_pc_in, pcl::PointCloud::Ptr& edge_pc_out, const pcl::PointCloud::Ptr& surf_pc_in, pcl::PointCloud::Ptr& surf_pc_out);
78 | };
79 |
80 | #endif // _ODOM_ESTIMATION_CLASS_H_
81 |
82 |
--------------------------------------------------------------------------------
/floam/launch/floam.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
10 |
11 |
14 |
15 |
16 |
17 |
18 |
19 |
20 |
21 |
22 |
23 |
24 |
25 |
26 |
27 |
28 |
29 |
30 |
31 |
32 |
33 |
34 |
35 |
36 |
37 |
38 |
39 |
40 |
41 |
42 |
43 |
44 |
45 |
46 |
47 |
48 |
49 |
50 |
51 |
--------------------------------------------------------------------------------
/floam/launch/floam_mapping.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
10 |
11 |
14 |
15 |
16 |
17 |
18 |
19 |
20 |
21 |
22 |
23 |
24 |
25 |
26 |
27 |
28 |
29 |
30 |
31 |
32 |
33 |
34 |
35 |
36 |
37 |
38 |
39 |
40 |
41 |
42 |
43 |
44 |
45 |
46 |
47 |
48 |
49 |
50 |
51 |
52 |
53 |
--------------------------------------------------------------------------------
/floam/launch/floam_rsbpearl32.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
6 |
7 |
8 |
9 |
10 |
13 |
14 |
15 |
16 |
17 |
18 |
21 |
22 |
23 |
24 |
25 |
26 |
27 |
28 |
29 |
30 |
31 |
32 |
33 |
34 |
35 |
36 |
37 |
38 |
39 |
40 |
41 |
42 |
43 |
44 |
45 |
46 |
52 |
53 |
54 |
--------------------------------------------------------------------------------
/floam/launch/floam_rslidar32.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
6 |
7 |
8 |
9 |
10 |
13 |
14 |
15 |
16 |
17 |
18 |
21 |
22 |
23 |
24 |
25 |
26 |
27 |
28 |
29 |
30 |
31 |
32 |
33 |
34 |
35 |
36 |
37 |
38 |
39 |
40 |
41 |
42 |
43 |
44 |
45 |
46 |
52 |
53 |
54 |
--------------------------------------------------------------------------------
/floam/launch/floam_velodyne.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
9 |
10 |
11 |
14 |
15 |
18 |
19 |
20 |
21 |
22 |
23 |
24 |
25 |
26 |
27 |
28 |
29 |
30 |
31 |
32 |
33 |
34 |
35 |
36 |
37 |
38 |
39 |
40 |
41 |
42 |
43 |
44 |
45 |
46 |
47 |
48 |
--------------------------------------------------------------------------------
/floam/launch/floam_velodyne1.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
7 |
8 |
9 |
10 |
11 |
12 |
13 |
14 |
15 |
16 |
17 |
20 |
21 |
24 |
25 |
26 |
27 |
28 |
29 |
30 |
31 |
32 |
33 |
34 |
35 |
36 |
37 |
38 |
39 |
40 |
41 |
42 |
43 |
44 |
45 |
46 |
47 |
48 |
49 |
55 |
56 |
57 |
--------------------------------------------------------------------------------
/floam/package.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 | floam
4 | 0.0.0
5 |
6 |
7 | This work is created based on some optimization on ALOAM by HKUST and LOAM by CMU
8 |
9 |
10 | Han Wang
11 |
12 | BSD
13 | https://wanghan.pro
14 | Han Wang
15 |
16 | catkin
17 | geometry_msgs
18 | nav_msgs
19 | roscpp
20 | rospy
21 | std_msgs
22 | rosbag
23 | sensor_msgs
24 | tf
25 | eigen_conversions
26 | geometry_msgs
27 | nav_msgs
28 | sensor_msgs
29 | roscpp
30 | rospy
31 | std_msgs
32 | rosbag
33 | tf
34 | eigen_conversions
35 |
36 |
37 |
38 |
39 |
--------------------------------------------------------------------------------
/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:
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:
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/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/Frames1
9 | - /map1
10 | - /map1/Autocompute Value Bounds1
11 | Splitter Ratio: 0.5
12 | Tree Height: 462
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: map
32 | Preferences:
33 | PromptSaveOnExit: true
34 | Toolbars:
35 | toolButtonStyle: 2
36 | Visualization Manager:
37 | Class: ""
38 | Displays:
39 | - Alpha: 0.5
40 | Cell Size: 5
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:
56 | Value: true
57 | - Class: rviz/TF
58 | Enabled: true
59 | Frame Timeout: 15
60 | Frames:
61 | All Enabled: false
62 | base_link:
63 | Value: true
64 | map:
65 | Value: true
66 | world:
67 | Value: false
68 | Marker Scale: 3
69 | Name: TF
70 | Show Arrows: false
71 | Show Axes: true
72 | Show Names: true
73 | Tree:
74 | world:
75 | map:
76 | base_link:
77 | {}
78 | Update Interval: 0
79 | Value: true
80 | - Alpha: 1
81 | Autocompute Intensity Bounds: true
82 | Autocompute Value Bounds:
83 | Max Value: 0.21922564506530762
84 | Min Value: -11.99945068359375
85 | Value: true
86 | Axis: Z
87 | Channel Name: intensity
88 | Class: rviz/PointCloud2
89 | Color: 255; 255; 255
90 | Color Transformer: Intensity
91 | Decay Time: 0
92 | Enabled: false
93 | Invert Rainbow: false
94 | Max Color: 255; 255; 255
95 | Max Intensity: 255
96 | Min Color: 0; 0; 0
97 | Min Intensity: 0
98 | Name: raw_data
99 | Position Transformer: XYZ
100 | Queue Size: 10
101 | Selectable: true
102 | Size (Pixels): 1
103 | Size (m): 0.009999999776482582
104 | Style: Points
105 | Topic: /velodyne_points_filtered
106 | Unreliable: false
107 | Use Fixed Frame: true
108 | Use rainbow: true
109 | Value: false
110 | - Alpha: 0.10000000149011612
111 | Autocompute Intensity Bounds: true
112 | Autocompute Value Bounds:
113 | Max Value: 2.685699939727783
114 | Min Value: -1.4920799732208252
115 | Value: false
116 | Axis: Z
117 | Channel Name: intensity
118 | Class: rviz/PointCloud2
119 | Color: 255; 255; 255
120 | Color Transformer: AxisColor
121 | Decay Time: 100
122 | Enabled: true
123 | Invert Rainbow: false
124 | Max Color: 255; 255; 255
125 | Max Intensity: 0.9565645456314087
126 | Min Color: 0; 0; 0
127 | Min Intensity: 0
128 | Name: map
129 | Position Transformer: XYZ
130 | Queue Size: 10
131 | Selectable: true
132 | Size (Pixels): 2
133 | Size (m): 0.05000000074505806
134 | Style: Flat Squares
135 | Topic: /map
136 | Unreliable: false
137 | Use Fixed Frame: true
138 | Use rainbow: true
139 | Value: true
140 | - Alpha: 1
141 | Buffer Length: 1
142 | Class: rviz/Path
143 | Color: 25; 255; 0
144 | Enabled: true
145 | Head Diameter: 0.30000001192092896
146 | Head Length: 0.20000000298023224
147 | Length: 0.30000001192092896
148 | Line Style: Lines
149 | Line Width: 0.029999999329447746
150 | Name: floam_result
151 | Offset:
152 | X: 0
153 | Y: 0
154 | Z: 0
155 | Pose Color: 255; 85; 255
156 | Pose Style: None
157 | Radius: 0.029999999329447746
158 | Shaft Diameter: 0.10000000149011612
159 | Shaft Length: 0.10000000149011612
160 | Topic: /base_link/trajectory
161 | Unreliable: false
162 | Value: true
163 | Enabled: true
164 | Global Options:
165 | Background Color: 0; 0; 0
166 | Default Light: true
167 | Fixed Frame: map
168 | Frame Rate: 30
169 | Name: root
170 | Tools:
171 | - Class: rviz/Interact
172 | Hide Inactive Objects: true
173 | - Class: rviz/MoveCamera
174 | - Class: rviz/Select
175 | - Class: rviz/FocusCamera
176 | - Class: rviz/Measure
177 | - Class: rviz/SetInitialPose
178 | Theta std deviation: 0.2617993950843811
179 | Topic: /initialpose
180 | X std deviation: 0.5
181 | Y std deviation: 0.5
182 | - Class: rviz/SetGoal
183 | Topic: /move_base_simple/goal
184 | - Class: rviz/PublishPoint
185 | Single click: true
186 | Topic: /clicked_point
187 | Value: true
188 | Views:
189 | Current:
190 | Class: jsk_rviz_plugin/TabletViewController
191 | Control Mode: Orbit
192 | Distance: 13.431673049926758
193 | Enable Stereo Rendering:
194 | Stereo Eye Separation: 0.05999999865889549
195 | Stereo Focal Distance: 1
196 | Swap Stereo Eyes: false
197 | Value: false
198 | Eye:
199 | X: -2.6345107555389404
200 | Y: -1.1982510089874268
201 | Z: -7.475274085998535
202 | Focus:
203 | X: 0.5860311985015869
204 | Y: -1.4869129657745361
205 | Z: 5.561393737792969
206 | Invert Z Axis: false
207 | Maintain Vertical Axis: true
208 | Mouse Enabled: true
209 | Name: Current View
210 | Near Clip Distance: 0.009999999776482582
211 | Placement Mouse Point: /rviz/current_mouse_point
212 | Placement Publish Topic: /rviz/current_camera_placement
213 | Placement Topic: /rviz/camera_placement
214 | Target Frame: body
215 | Transition Time: 0.5
216 | Up:
217 | X: 0
218 | Y: 0
219 | Z: 1
220 | Value: TabletViewController (jsk_rviz_plugin)
221 | Saved: ~
222 | Window Geometry:
223 | Displays:
224 | collapsed: false
225 | Height: 759
226 | Hide Left Dock: false
227 | Hide Right Dock: false
228 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000259fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000259000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000002e0000000c00000000000000000fb0000000600490053004300000002d9000000c70000000000000000000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004b80000025900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
229 | Selection:
230 | collapsed: false
231 | Time:
232 | collapsed: false
233 | Tool Properties:
234 | collapsed: false
235 | Views:
236 | collapsed: false
237 | Width: 1853
238 | X: 67
239 | Y: 54
240 |
--------------------------------------------------------------------------------
/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::Ptr>> map_height_temp;
12 | for(int j=0;j::Ptr> map_depth_temp;
14 | for(int k=0;k::Ptr point_cloud_temp(new pcl::PointCloud);
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::Ptr>> map_height_temp;
36 | for(int j=0; j < map_height;j++){
37 | std::vector::Ptr> map_depth_temp;
38 | for(int k=0;k< map_depth;k++){
39 | pcl::PointCloud::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::Ptr>> map_height_temp;
51 | for(int j=0; j < map_height;j++){
52 | std::vector::Ptr> map_depth_temp;
53 | for(int k=0;k< map_depth;k++){
54 | pcl::PointCloud::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::Ptr> map_depth_temp;
65 | for(int k=0;k::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::Ptr> map_depth_temp;
77 | for(int k=0;k::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::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::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::Ptr point_cloud_temp(new pcl::PointCloud());
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::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::Ptr transformed_pc(new pcl::PointCloud());
158 | pcl::transformPointCloud(*pc_in, *transformed_pc, pose_current.cast());
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::Ptr LaserMappingClass::getMap(void){
189 | pcl::PointCloud::Ptr laserCloudMap = pcl::PointCloud::Ptr(new pcl::PointCloud());
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 |
--------------------------------------------------------------------------------
/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
7 | #include
8 | #include
9 | #include
10 | #include
11 | #include
12 |
13 | //ros lib
14 | #include
15 | #include
16 | #include
17 | #include
18 | #include
19 |
20 | //pcl lib
21 | #include
22 | #include
23 | #include
24 |
25 | //local lib
26 | #include "laserMappingClass.h"
27 | #include "lidar.h"
28 |
29 | #define pcd_save_en 1
30 | int pcd_index = 0;
31 | pcl::PointCloud::Ptr pcl_wait_save(new pcl::PointCloud());
32 |
33 | LaserMappingClass laserMapping;
34 | lidar::Lidar lidar_param;
35 | std::mutex mutex_lock;
36 | std::queue odometryBuf;
37 | std::queue pointCloudBuf;
38 |
39 | ros::Publisher map_pub;
40 | void odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
41 | {
42 | mutex_lock.lock();
43 | odometryBuf.push(msg);
44 | mutex_lock.unlock();
45 | }
46 |
47 | void velodyneHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
48 | {
49 | mutex_lock.lock();
50 | pointCloudBuf.push(laserCloudMsg);
51 | mutex_lock.unlock();
52 | }
53 |
54 |
55 | void laser_mapping(){
56 | while(1){
57 | if(!odometryBuf.empty() && !pointCloudBuf.empty()){
58 |
59 | //read data
60 | mutex_lock.lock();
61 | if(!pointCloudBuf.empty() && pointCloudBuf.front()->header.stamp.toSec()header.stamp.toSec()-0.5*lidar_param.scan_period){
62 | ROS_WARN("time stamp unaligned error and pointcloud discarded, pls check your data --> laser mapping node");
63 | pointCloudBuf.pop();
64 | mutex_lock.unlock();
65 | continue;
66 | }
67 |
68 | if(!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < pointCloudBuf.front()->header.stamp.toSec()-0.5*lidar_param.scan_period){
69 | odometryBuf.pop();
70 | ROS_INFO("time stamp unaligned with path final, pls check your data --> laser mapping node");
71 | mutex_lock.unlock();
72 | continue;
73 | }
74 |
75 | //if time aligned
76 | pcl::PointCloud::Ptr pointcloud_in(new pcl::PointCloud());
77 | pcl::fromROSMsg(*pointCloudBuf.front(), *pointcloud_in);
78 | ros::Time pointcloud_time = (pointCloudBuf.front())->header.stamp;
79 |
80 | Eigen::Isometry3d current_pose = Eigen::Isometry3d::Identity();
81 | 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));
82 | current_pose.pretranslate(Eigen::Vector3d(odometryBuf.front()->pose.pose.position.x,odometryBuf.front()->pose.pose.position.y,odometryBuf.front()->pose.pose.position.z));
83 | pointCloudBuf.pop();
84 | odometryBuf.pop();
85 | mutex_lock.unlock();
86 |
87 |
88 | laserMapping.updateCurrentPointsToMap(pointcloud_in,current_pose);
89 |
90 | pcl::PointCloud::Ptr pc_map = laserMapping.getMap();
91 | sensor_msgs::PointCloud2 PointsMsg;
92 | pcl::toROSMsg(*pc_map, PointsMsg);
93 | PointsMsg.header.stamp = pointcloud_time;
94 | PointsMsg.header.frame_id = "/map";
95 | map_pub.publish(PointsMsg);
96 |
97 | // save full pointcloud
98 | if (pcd_save_en)
99 | {
100 | int size = pointcloud_in->points.size();
101 | pcl::PointCloud::Ptr transformed_pc(new pcl::PointCloud(size, 1));
102 | pcl::transformPointCloud(*pointcloud_in, *transformed_pc, current_pose.cast());
103 |
104 | *pcl_wait_save += *transformed_pc;
105 |
106 | // if (pcl_wait_save->size() > 0)
107 | // {
108 | // pcd_index ++;
109 | // std::string all_points_dir(std::string(std::string(ROOT_DIR) + "PCD/scans_") + std::to_string(pcd_index) + std::string(".pcd"));
110 | // pcl::PCDWriter pcd_writer;
111 | // std::cout << "current scan saved to /PCD/" << all_points_dir << std::endl;
112 | // pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);
113 | // pcl_wait_save->clear();
114 | // }
115 | }
116 | }
117 |
118 | //sleep 2 ms every time
119 | std::chrono::milliseconds dura(2);
120 | std::this_thread::sleep_for(dura);
121 | }
122 | }
123 |
124 | int main(int argc, char **argv)
125 | {
126 | ros::init(argc, argv, "main");
127 | ros::NodeHandle nh;
128 |
129 | int scan_line = 64;
130 | double vertical_angle = 2.0;
131 | double scan_period= 0.1;
132 | double max_dis = 60.0;
133 | double min_dis = 2.0;
134 | double map_resolution = 0.4;
135 | std::string ROOT_DIR;
136 |
137 | nh.getParam("/scan_period", scan_period);
138 | nh.getParam("/vertical_angle", vertical_angle);
139 | nh.getParam("/max_dis", max_dis);
140 | nh.getParam("/min_dis", min_dis);
141 | nh.getParam("/scan_line", scan_line);
142 | nh.getParam("/map_resolution", map_resolution);
143 | nh.getParam("/map_save_ROOT", ROOT_DIR);
144 |
145 | lidar_param.setScanPeriod(scan_period);
146 | lidar_param.setVerticalAngle(vertical_angle);
147 | lidar_param.setLines(scan_line);
148 | lidar_param.setMaxDistance(max_dis);
149 | lidar_param.setMinDistance(min_dis);
150 |
151 | laserMapping.init(map_resolution);
152 | ros::Subscriber subLaserCloud = nh.subscribe("/velodyne_points_filtered", 100, velodyneHandler);
153 | ros::Subscriber subOdometry = nh.subscribe("/odom", 100, odomCallback);
154 |
155 | map_pub = nh.advertise("/map", 100);
156 | std::thread laser_mapping_process{laser_mapping};
157 |
158 | ros::spin();
159 |
160 | // exit the progroam and save the full map
161 | if (pcl_wait_save->size() > 0)
162 | {
163 | // pcd_index ++;
164 | std::string all_points_dir(std::string(std::string(ROOT_DIR) + "/PCD/map_") + std::string(".pcd"));
165 | pcl::PCDWriter pcd_writer;
166 | std::cout << "current scan saved to /PCD/:" << all_points_dir << std::endl;
167 | pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);
168 | pcl_wait_save->clear();
169 | }
170 |
171 | return 0;
172 | }
173 |
--------------------------------------------------------------------------------
/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::Ptr& pc_in, pcl::PointCloud::Ptr& pc_out_edge, pcl::PointCloud::Ptr& pc_out_surf){
13 |
14 | std::vector indices;
15 | pcl::removeNaNFromPointCloud(*pc_in, indices);
16 |
17 |
18 | int N_SCANS = lidar_param.num_lines;
19 | std::vector::Ptr> laserCloudScans;
20 | for(int i=0;i::Ptr(new pcl::PointCloud()));
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(distancelidar_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 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 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::rsbp_featureExtraction(const pcl::PointCloud::Ptr& pc_in, pcl::PointCloud::Ptr& pc_out_edge, pcl::PointCloud::Ptr& pc_out_surf){
102 |
103 | std::vector indices;
104 | pcl::removeNaNFromPointCloud(*pc_in, indices);
105 |
106 | int cloudSize = (int) pc_in->points.size();
107 | int count = cloudSize;
108 | int N_SCANS = lidar_param.num_lines;
109 | std::vector::Ptr> laserCloudScans;
110 | for(int i=0;i::Ptr(new pcl::PointCloud()));
112 | }
113 |
114 | for (int i = 0; i < (int) pc_in->points.size(); i++)
115 | {
116 | int scanID=0;
117 | double distance = sqrt(pc_in->points[i].x * pc_in->points[i].x + pc_in->points[i].y * pc_in->points[i].y);
118 | if(distancelidar_param.max_distance)
119 | continue;
120 | double angle = atan(pc_in->points[i].z / distance) * 180 / M_PI;
121 |
122 | scanID = int((89.5 - angle) / 2.81 + 0.5);
123 |
124 | if (scanID > (32 - 1) || scanID < 0) {
125 | count--;
126 | continue;
127 | }
128 |
129 | laserCloudScans[scanID]->push_back(pc_in->points[i]);
130 |
131 | }
132 |
133 | cloudSize = count;
134 | for(int i = 0; i < N_SCANS; i++){
135 | if(laserCloudScans[i]->points.size()<131){
136 | continue;
137 | }
138 |
139 | std::vector cloudCurvature;
140 | int total_points = laserCloudScans[i]->points.size()-10;
141 | for(int j = 5; j < (int)laserCloudScans[i]->points.size() - 5; j++){
142 | 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;
143 | 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;
144 | 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;
145 | Double2d distance(j,diffX * diffX + diffY * diffY + diffZ * diffZ);
146 | cloudCurvature.push_back(distance);
147 |
148 | }
149 | for(int j=0;j<6;j++){
150 | int sector_length = (int)(total_points/6);
151 | int sector_start = sector_length *j;
152 | int sector_end = sector_length *(j+1)-1;
153 | if (j==5){
154 | sector_end = total_points - 1;
155 | }
156 | std::vector subCloudCurvature(cloudCurvature.begin()+sector_start,cloudCurvature.begin()+sector_end);
157 |
158 | featureExtractionFromSector(laserCloudScans[i],subCloudCurvature, pc_out_edge, pc_out_surf);
159 |
160 | }
161 |
162 | }
163 |
164 | }
165 |
166 | void LaserProcessingClass::featureExtractionFromSector(const pcl::PointCloud::Ptr& pc_in, std::vector& cloudCurvature, pcl::PointCloud::Ptr& pc_out_edge, pcl::PointCloud::Ptr& pc_out_surf){
167 |
168 | std::sort(cloudCurvature.begin(), cloudCurvature.end(), [](const Double2d & a, const Double2d & b)
169 | {
170 | return a.value < b.value;
171 | });
172 |
173 |
174 | int largestPickedNum = 0;
175 | std::vector picked_points;
176 | int point_info_count =0;
177 | for (int i = cloudCurvature.size()-1; i >= 0; i--)
178 | {
179 | int ind = cloudCurvature[i].id;
180 | if(std::find(picked_points.begin(), picked_points.end(), ind)==picked_points.end()){
181 | if(cloudCurvature[i].value <= 0.1){
182 | break;
183 | }
184 |
185 | largestPickedNum++;
186 | picked_points.push_back(ind);
187 |
188 | if (largestPickedNum <= 20){
189 | pc_out_edge->push_back(pc_in->points[ind]);
190 | point_info_count++;
191 | }else{
192 | break;
193 | }
194 |
195 | for(int k=1;k<=5;k++){
196 | double diffX = pc_in->points[ind + k].x - pc_in->points[ind + k - 1].x;
197 | double diffY = pc_in->points[ind + k].y - pc_in->points[ind + k - 1].y;
198 | double diffZ = pc_in->points[ind + k].z - pc_in->points[ind + k - 1].z;
199 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){
200 | break;
201 | }
202 | picked_points.push_back(ind+k);
203 | }
204 | for(int k=-1;k>=-5;k--){
205 | double diffX = pc_in->points[ind + k].x - pc_in->points[ind + k + 1].x;
206 | double diffY = pc_in->points[ind + k].y - pc_in->points[ind + k + 1].y;
207 | double diffZ = pc_in->points[ind + k].z - pc_in->points[ind + k + 1].z;
208 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){
209 | break;
210 | }
211 | picked_points.push_back(ind+k);
212 | }
213 |
214 | }
215 | }
216 |
217 | //find flat points
218 | // point_info_count =0;
219 | // int smallestPickedNum = 0;
220 |
221 | // for (int i = 0; i <= (int)cloudCurvature.size()-1; i++)
222 | // {
223 | // int ind = cloudCurvature[i].id;
224 |
225 | // if( std::find(picked_points.begin(), picked_points.end(), ind)==picked_points.end()){
226 | // if(cloudCurvature[i].value > 0.1){
227 | // //ROS_WARN("extracted feature not qualified, please check lidar");
228 | // break;
229 | // }
230 | // smallestPickedNum++;
231 | // picked_points.push_back(ind);
232 |
233 | // if(smallestPickedNum <= 4){
234 | // //find all points
235 | // pc_surf_flat->push_back(pc_in->points[ind]);
236 | // pc_surf_lessFlat->push_back(pc_in->points[ind]);
237 | // point_info_count++;
238 | // }
239 | // else{
240 | // break;
241 | // }
242 |
243 | // for(int k=1;k<=5;k++){
244 | // double diffX = pc_in->points[ind + k].x - pc_in->points[ind + k - 1].x;
245 | // double diffY = pc_in->points[ind + k].y - pc_in->points[ind + k - 1].y;
246 | // double diffZ = pc_in->points[ind + k].z - pc_in->points[ind + k - 1].z;
247 | // if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){
248 | // break;
249 | // }
250 | // picked_points.push_back(ind+k);
251 | // }
252 | // for(int k=-1;k>=-5;k--){
253 | // double diffX = pc_in->points[ind + k].x - pc_in->points[ind + k + 1].x;
254 | // double diffY = pc_in->points[ind + k].y - pc_in->points[ind + k + 1].y;
255 | // double diffZ = pc_in->points[ind + k].z - pc_in->points[ind + k + 1].z;
256 | // if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){
257 | // break;
258 | // }
259 | // picked_points.push_back(ind+k);
260 | // }
261 |
262 | // }
263 | // }
264 |
265 | for (int i = 0; i <= (int)cloudCurvature.size()-1; i++)
266 | {
267 | int ind = cloudCurvature[i].id;
268 | if( std::find(picked_points.begin(), picked_points.end(), ind)==picked_points.end())
269 | {
270 | pc_out_surf->push_back(pc_in->points[ind]);
271 | }
272 | }
273 |
274 |
275 |
276 | }
277 | LaserProcessingClass::LaserProcessingClass(){
278 |
279 | }
280 |
281 | Double2d::Double2d(int id_in, double value_in){
282 | id = id_in;
283 | value =value_in;
284 | };
285 |
286 | PointsInfo::PointsInfo(int layer_in, double time_in){
287 | layer = layer_in;
288 | time = time_in;
289 | };
290 |
--------------------------------------------------------------------------------
/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
7 | #include
8 | #include
9 | #include
10 | #include
11 | #include
12 |
13 | //ros lib
14 | #include
15 | #include
16 | #include
17 | #include
18 | #include
19 | #include
20 | #include
21 |
22 | //pcl lib
23 | #include
24 | #include
25 | #include
26 |
27 |
28 | //local lib
29 | #include "lidar.h"
30 | #include "laserProcessingClass.h"
31 |
32 |
33 | LaserProcessingClass laserProcessing;
34 | std::mutex mutex_lock;
35 | std::queue pointCloudBuf;
36 | lidar::Lidar lidar_param;
37 |
38 | std::string lidar_type;
39 |
40 | ros::Publisher pubEdgePoints;
41 | ros::Publisher pubSurfPoints;
42 | ros::Publisher pubLaserCloudFiltered;
43 |
44 | size_t convertMyLidarToPCLPointCloud(const sensor_msgs::PointCloud2ConstPtr &pcd_msg, pcl::PointCloud &pcd)
45 | {
46 | if (pcd_msg)
47 | {
48 | size_t pt_size = pcd_msg->width * pcd_msg->height;
49 |
50 | // output.points.resize (pcd_msg->width * pcd_msg->height);
51 | // output.channels.resize (pcd_msg->fields.size () - 3);
52 | // Get the x/y/z field offsets
53 | int x_idx = sensor_msgs::getPointCloud2FieldIndex(*pcd_msg, "x");
54 | int y_idx = sensor_msgs::getPointCloud2FieldIndex(*pcd_msg, "y");
55 | int z_idx = sensor_msgs::getPointCloud2FieldIndex(*pcd_msg, "z");
56 | int inten_idx = sensor_msgs::getPointCloud2FieldIndex(*pcd_msg, "intensity");
57 |
58 | int x_offset = pcd_msg->fields[x_idx].offset;
59 | int y_offset = pcd_msg->fields[y_idx].offset;
60 | int z_offset = pcd_msg->fields[z_idx].offset;
61 | int inten_offset = pcd_msg->fields[inten_idx].offset;
62 |
63 | uint8_t x_datatype = pcd_msg->fields[x_idx].datatype;
64 | uint8_t y_datatype = pcd_msg->fields[y_idx].datatype;
65 | uint8_t z_datatype = pcd_msg->fields[z_idx].datatype;
66 | uint8_t inten_datatype = pcd_msg->fields[inten_idx].datatype;
67 |
68 | // Copy the data points
69 | size_t validCount = 0;
70 | for (size_t cp = 0; cp < pt_size; ++cp)
71 | {
72 | // Copy x/y/z/intensity
73 | pcl::PointXYZI pt;
74 | pt.x = sensor_msgs::readPointCloud2BufferValue(&pcd_msg->data[cp * pcd_msg->point_step + x_offset], x_datatype);
75 | pt.y = sensor_msgs::readPointCloud2BufferValue(&pcd_msg->data[cp * pcd_msg->point_step + y_offset], y_datatype);
76 | pt.z = sensor_msgs::readPointCloud2BufferValue(&pcd_msg->data[cp * pcd_msg->point_step + z_offset], z_datatype);
77 |
78 | if (inten_idx >= 0)
79 | pt.intensity = sensor_msgs::readPointCloud2BufferValue(&pcd_msg->data[cp * pcd_msg->point_step + inten_offset], inten_datatype);
80 |
81 | pcd.push_back(pt);
82 | validCount++;
83 | }
84 | return validCount;
85 | }
86 | return 0;
87 | }
88 |
89 | void velodyneHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
90 | {
91 | mutex_lock.lock();
92 | pointCloudBuf.push(laserCloudMsg);
93 | mutex_lock.unlock();
94 |
95 | }
96 |
97 | double total_time =0;
98 | int total_frame=0;
99 |
100 | void laser_processing(){
101 | while(1){
102 | if(!pointCloudBuf.empty()){
103 | //read data
104 | mutex_lock.lock();
105 | pcl::PointCloud::Ptr pointcloud_in(new pcl::PointCloud());
106 | if(lidar_type == "rsbpearl")
107 | convertMyLidarToPCLPointCloud(pointCloudBuf.front(), *pointcloud_in);
108 | else
109 | pcl::fromROSMsg(*pointCloudBuf.front(), *pointcloud_in);
110 |
111 | ros::Time pointcloud_time = (pointCloudBuf.front())->header.stamp;
112 | pointCloudBuf.pop();
113 | mutex_lock.unlock();
114 |
115 | pcl::PointCloud::Ptr pointcloud_edge(new pcl::PointCloud());
116 | pcl::PointCloud::Ptr pointcloud_surf(new pcl::PointCloud());
117 |
118 | std::chrono::time_point start, end;
119 | start = std::chrono::system_clock::now();
120 | if(lidar_type == "rsbpearl"){
121 | laserProcessing.rsbp_featureExtraction(pointcloud_in,pointcloud_edge,pointcloud_surf);
122 | }else
123 | laserProcessing.featureExtraction(pointcloud_in,pointcloud_edge,pointcloud_surf);
124 | end = std::chrono::system_clock::now();
125 | std::chrono::duration elapsed_seconds = end - start;
126 | total_frame++;
127 | float time_temp = elapsed_seconds.count() * 1000;
128 | total_time+=time_temp;
129 | //ROS_INFO("average laser processing time %f ms \n \n", total_time/total_frame);
130 |
131 | sensor_msgs::PointCloud2 laserCloudFilteredMsg;
132 | pcl::PointCloud::Ptr pointcloud_filtered(new pcl::PointCloud());
133 | *pointcloud_filtered+=*pointcloud_edge;
134 | *pointcloud_filtered+=*pointcloud_surf;
135 | pcl::toROSMsg(*pointcloud_filtered, laserCloudFilteredMsg);
136 | laserCloudFilteredMsg.header.stamp = pointcloud_time;
137 | laserCloudFilteredMsg.header.frame_id = "/base_link";
138 | pubLaserCloudFiltered.publish(laserCloudFilteredMsg);
139 |
140 | // sensor_msgs::PointCloud2 laserCloudFilteredMsg;
141 | // pcl::toROSMsg(*pointcloud_in, laserCloudFilteredMsg);
142 | // laserCloudFilteredMsg.header.stamp = pointcloud_time;
143 | // laserCloudFilteredMsg.header.frame_id = "/base_link";
144 | // pubLaserCloudFiltered.publish(laserCloudFilteredMsg);
145 |
146 | sensor_msgs::PointCloud2 edgePointsMsg;
147 | pcl::toROSMsg(*pointcloud_edge, edgePointsMsg);
148 | edgePointsMsg.header.stamp = pointcloud_time;
149 | edgePointsMsg.header.frame_id = "/base_link";
150 | pubEdgePoints.publish(edgePointsMsg);
151 |
152 |
153 | sensor_msgs::PointCloud2 surfPointsMsg;
154 | pcl::toROSMsg(*pointcloud_surf, surfPointsMsg);
155 | surfPointsMsg.header.stamp = pointcloud_time;
156 | surfPointsMsg.header.frame_id = "/base_link";
157 | pubSurfPoints.publish(surfPointsMsg);
158 |
159 | }
160 | //sleep 2 ms every time
161 | std::chrono::milliseconds dura(2);
162 | std::this_thread::sleep_for(dura);
163 | }
164 | }
165 |
166 | int main(int argc, char **argv)
167 | {
168 | ros::init(argc, argv, "main");
169 | ros::NodeHandle nh;
170 |
171 | int scan_line = 64;
172 | double vertical_angle = 2.0;
173 | double scan_period= 0.1;
174 | double max_dis = 60.0;
175 | double min_dis = 2.0;
176 |
177 | nh.getParam("/scan_period", scan_period);
178 | nh.getParam("/vertical_angle", vertical_angle);
179 | nh.getParam("/max_dis", max_dis);
180 | nh.getParam("/min_dis", min_dis);
181 | nh.getParam("/scan_line", scan_line);
182 | nh.getParam("lidar_type",lidar_type);
183 |
184 | lidar_param.setScanPeriod(scan_period);
185 | lidar_param.setVerticalAngle(vertical_angle);
186 | lidar_param.setLines(scan_line);
187 | lidar_param.setMaxDistance(max_dis);
188 | lidar_param.setMinDistance(min_dis);
189 |
190 | laserProcessing.init(lidar_param);
191 |
192 | ros::Subscriber subLaserCloud = nh.subscribe("/velodyne_points", 100, velodyneHandler);
193 |
194 | pubLaserCloudFiltered = nh.advertise("/velodyne_points_filtered", 1000);
195 |
196 | pubEdgePoints = nh.advertise("/laser_cloud_edge", 10000);
197 |
198 | pubSurfPoints = nh.advertise("/laser_cloud_surf", 10000);
199 |
200 | std::thread laser_processing_process{laser_processing};
201 |
202 | ros::spin();
203 |
204 | return 0;
205 | }
206 |
207 |
--------------------------------------------------------------------------------
/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/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 q_last_curr(parameters[0]);
16 | Eigen::Map t_last_curr(parameters[0] + 4);
17 | Eigen::Vector3d lp;
18 | lp = q_last_curr * curr_point + t_last_curr; //new point
19 | Eigen::Vector3d nu = (lp - last_point_a).cross(lp - last_point_b);
20 | Eigen::Vector3d de = last_point_a - last_point_b;
21 |
22 | residuals[0] = nu.x() / de.norm();
23 | residuals[1] = nu.y() / de.norm();
24 | residuals[2] = nu.z() / de.norm();
25 |
26 | if(jacobians != NULL)
27 | {
28 | if(jacobians[0] != NULL)
29 | {
30 | Eigen::Matrix3d skew_lp = skew(lp);
31 | Eigen::Matrix dp_by_so3;
32 | dp_by_so3.block<3,3>(0,0) = -skew_lp;
33 | (dp_by_so3.block<3,3>(0, 3)).setIdentity();
34 | Eigen::Map > J_se3(jacobians[0]);
35 | J_se3.setZero();
36 | Eigen::Vector3d re = last_point_b - last_point_a;
37 | Eigen::Matrix3d skew_re = skew(re);
38 |
39 | J_se3.block<3,6>(0,0) = skew_re * dp_by_so3/de.norm();
40 |
41 | }
42 | }
43 |
44 | return true;
45 |
46 | }
47 |
48 | //surf norm cost
49 |
50 | SurfNormAnalyticCostFunction::SurfNormAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, double negative_OA_dot_norm_)
51 | : curr_point(curr_point_), plane_unit_norm(plane_unit_norm_), negative_OA_dot_norm(negative_OA_dot_norm_) {
52 |
53 | }
54 |
55 | bool SurfNormAnalyticCostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
56 | {
57 | Eigen::Map q_w_curr(parameters[0]);
58 | Eigen::Map t_w_curr(parameters[0] + 4);
59 | Eigen::Vector3d point_w = q_w_curr * curr_point + t_w_curr;
60 |
61 | residuals[0] = plane_unit_norm.dot(point_w) + negative_OA_dot_norm;
62 |
63 | if(jacobians != NULL)
64 | {
65 | if(jacobians[0] != NULL)
66 | {
67 | Eigen::Matrix3d skew_point_w = skew(point_w);
68 |
69 | Eigen::Matrix dp_by_so3;
70 | dp_by_so3.block<3,3>(0,0) = -skew_point_w;
71 | (dp_by_so3.block<3,3>(0, 3)).setIdentity();
72 | Eigen::Map > J_se3(jacobians[0]);
73 | J_se3.setZero();
74 | J_se3.block<1,6>(0,0) = plane_unit_norm.transpose() * dp_by_so3;
75 |
76 | }
77 | }
78 | return true;
79 |
80 | }
81 |
82 |
83 | bool PoseSE3Parameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const
84 | {
85 | Eigen::Map trans(x + 4);
86 |
87 | Eigen::Quaterniond delta_q;
88 | Eigen::Vector3d delta_t;
89 | getTransformFromSe3(Eigen::Map>(delta), delta_q, delta_t);
90 | Eigen::Map quater(x);
91 | Eigen::Map quater_plus(x_plus_delta);
92 | Eigen::Map trans_plus(x_plus_delta + 4);
93 |
94 | quater_plus = delta_q * quater;
95 | trans_plus = delta_q * trans + delta_t;
96 |
97 | return true;
98 | }
99 |
100 |
101 | bool PoseSE3Parameterization::ComputeJacobian(const double *x, double *jacobian) const
102 | {
103 | Eigen::Map> j(jacobian);
104 | (j.topRows(6)).setIdentity();
105 | (j.bottomRows(1)).setZero();
106 |
107 | return true;
108 | }
109 |
110 |
111 | void getTransformFromSe3(const Eigen::Matrix& se3, Eigen::Quaterniond& q, Eigen::Vector3d& t){
112 | Eigen::Vector3d omega(se3.data());
113 | Eigen::Vector3d upsilon(se3.data()+3);
114 | Eigen::Matrix3d Omega = skew(omega);
115 |
116 | double theta = omega.norm();
117 | double half_theta = 0.5*theta;
118 |
119 | double imag_factor;
120 | double real_factor = cos(half_theta);
121 | if(theta<1e-10)
122 | {
123 | double theta_sq = theta*theta;
124 | double theta_po4 = theta_sq*theta_sq;
125 | imag_factor = 0.5-0.0208333*theta_sq+0.000260417*theta_po4;
126 | }
127 | else
128 | {
129 | double sin_half_theta = sin(half_theta);
130 | imag_factor = sin_half_theta/theta;
131 | }
132 |
133 | q = Eigen::Quaterniond(real_factor, imag_factor*omega.x(), imag_factor*omega.y(), imag_factor*omega.z());
134 |
135 |
136 | Eigen::Matrix3d J;
137 | if (theta<1e-10)
138 | {
139 | J = q.matrix();
140 | }
141 | else
142 | {
143 | Eigen::Matrix3d Omega2 = Omega*Omega;
144 | J = (Eigen::Matrix3d::Identity() + (1-cos(theta))/(theta*theta)*Omega + (theta-sin(theta))/(pow(theta,3))*Omega2);
145 | }
146 |
147 | t = J*upsilon;
148 | }
149 |
150 | Eigen::Matrix skew(Eigen::Matrix& mat_in){
151 | Eigen::Matrix skew_mat;
152 | skew_mat.setZero();
153 | skew_mat(0,1) = -mat_in(2);
154 | skew_mat(0,2) = mat_in(1);
155 | skew_mat(1,2) = -mat_in(0);
156 | skew_mat(1,0) = mat_in(2);
157 | skew_mat(2,0) = -mat_in(1);
158 | skew_mat(2,1) = mat_in(0);
159 | return skew_mat;
160 | }
161 |
--------------------------------------------------------------------------------
/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::Ptr(new pcl::PointCloud());
10 | laserCloudSurfMap = pcl::PointCloud::Ptr(new pcl::PointCloud());
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::Ptr(new pcl::KdTreeFLANN());
18 | kdtreeSurfMap = pcl::KdTreeFLANN::Ptr(new pcl::KdTreeFLANN());
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::Ptr& edge_in, const pcl::PointCloud::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::Ptr& edge_in, const pcl::PointCloud::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::Ptr downsampledEdgeCloud(new pcl::PointCloud());
45 | pcl::PointCloud::Ptr downsampledSurfCloud(new pcl::PointCloud());
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::Ptr& edge_pc_in, pcl::PointCloud::Ptr& edge_pc_out, const pcl::PointCloud::Ptr& surf_pc_in, pcl::PointCloud::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::Ptr& pc_in, const pcl::PointCloud::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 pointSearchInd;
109 | std::vector pointSearchSqDis;
110 | kdtreeEdgeMap->nearestKSearch(point_temp, 5, pointSearchInd, pointSearchSqDis);
111 | if (pointSearchSqDis[4] < 1.0)
112 | {
113 | std::vector 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 tmpZeroMean = nearCorners[j] - center;
129 | covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
130 | }
131 |
132 | Eigen::SelfAdjointEigenSolver 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::Ptr& pc_in, const pcl::PointCloud::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 pointSearchInd;
162 | std::vector pointSearchSqDis;
163 | kdtreeSurfMap->nearestKSearch(point_temp, 5, pointSearchInd, pointSearchSqDis);
164 |
165 | Eigen::Matrix matA0;
166 | Eigen::Matrix matB0 = -1 * Eigen::Matrix::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::Ptr& downsampledEdgeCloud, const pcl::PointCloud::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::Ptr tmpCorner(new pcl::PointCloud