Function pack Name: lidar_align

git website: link

# Function introduction

A method for calibrating external parameters of 3D lidar and 6-DOF pose sensor

Applicable ROS versions include Indigo, Kinect and melody

Accurate results require a lot of non planar motion, which makes this method not suitable for calibrating sensors installed on vehicles

It is thought that the final result can be calculated only when there is a change in the data in that direction.

The whole function package basically realizes the following functions:

1. Read the data of lidar and pose sensor

2. The time stamp is used to match the coordinate transformation of each point in each frame of lidar and pose sensor

2. Through the above transformation matrix, each frame of lidar will be spliced into a point cloud using pose information

3. The sum of the distance between each point and its nearest neighbor is obtained. In the optimization, it is continuous iteration to find the coordinate transformation to minimize the distance

Overall idea of function package algorithm:

1. For each point of lidar in each frame, a pose data is matched by timestamp, and a more accurate pose value is obtained by interpolation. The time offset of each point is also one of the optimization factors

2. Nonlinear optimization method using NLopt Library

3. Objective function: minimize the nearest neighbor of each point of the spliced point cloud

4. Optimization vectors: x, y, z, roll, pitch, yaw, time_offset

In general, it is continuous iteration to find an appropriate optimization vector (that is, the coordinate transformation from lidar to odometer) to minimize the distance between the nearest neighbors of each point of the point cloud.

The advantage of this function pack is that it can be calculated offline, record a rosbag, and then run to find the external parameters

It only reads one bag, so lidar and pose should be in it

Final result:

When running, the function package will output the current estimated changes

At the end of optimization, the parameters of coordinate transformation will be printed on the terminal

A txt file and a ply file will also be saved under the path.

You can check the ply file, that is, whether the spliced point cloud is consistent with the scene

# compile

After downloading from git, you need to download the nlopt library

sudo apt-get install libnlopt-dev

Then the compiler will still report an error

Could not find a package configuration file provided by "NLOPT"

The solution will be lidar_ Copy NLOPTConfig.cmake in the align folder to the src path of your ROS workspace

# Related parameters

These are not in the configuration file. If you want to modify them, you need to change the source code and then compile them

In each class, there is a Config structure that initializes the relevant parameters

lidar related are set in the Scan class

class Scan { public: struct Config { float min_point_distance = 80;//If the minimum value of the distance between points in lidar data is greater than this value, 0.0 is used; float max_point_distance = 200;// The maximum distance between points in lidar data is less than this value, and the default value is 100.0; float keep_points_ratio = 0.01;// The scale used to optimize points (the larger the value, the greater the running time) is 0.01 by default float min_return_intensity = -1.0;//The minimum value of the strength of points in lidar data is greater than this value, and the default value is - 1.0 bool estimate_point_times = false;//Whether to estimate the time of each point of lidar, and use the following two variables to default to false bool clockwise_lidar = false;//The direction of lidar rotation is clockwise or counterclockwise. The default is false (counterclockwise) bool motion_compensation = true;//Is it necessary to run compensation? The default value is true float lidar_rpm = 600.0;// The speed of lidar is only used for estimate_point_times default 600 };

The relevant input and output parameters are set in the launch

<param name="input_bag_path" value="$(arg bag_file)" /> <param name="input_csv_path" value="$(arg csv_file)" /> <param name="output_pointcloud_path" value="$(find lidar_align)/results/$(anon lidar_points).ply" /> <param name="output_calibration_path" value="$(find lidar_align)/results/$(anon calibration).txt" /> <param name="transforms_from_csv" value="$(arg transforms_from_csv)"/>

use_ n_ The number of lidar frames at the beginning of scans optimization is 2147483647 by default

input_bag_path ros bag

transforms_ from_ Whether csv reads the location through the csv file. The default is false

input_csv_path csv path

output_pointcloud_path ply file output path

output_calibration_path calibration result output path

Correction related parameters are set in the Aligner class

class Aligner { public: struct Config { bool local = false;//Perform local optimization or global optimization, initialize to false, perform global optimization, and the results are used for initial estimation, and then perform local optimization std::vector<double> inital_guess{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};//The initial estimate of the calibration is only used to run local mode double max_time_offset = 0.1;//Up and down floating range of maximum time alignment deviation x[6] double angular_range = 0.5; // The search range (radians) angle offset of the initial estimated position is + - 0.5 radians double translation_range = 1.0;//The upper and lower limits of the search range (position) position offset at the initial estimated position are + - 1m double max_evals = 200;// Maximum number of evaluations double xtol = 0.0001;//Tolerance of x int knn_batch_size = 1000;//When the offset number of points is used to find the nearest neighbor int knn_k = 1;//To find the number of nearest neighbors, the program adds 1 by itself, because finding the nearest neighbor of one of the points in a point cloud is the real nearest neighbor float local_knn_max_dist = 0.1;//Maximum distance of nearest neighbor in local optimization float global_knn_max_dist = 1.0;//Maximum distance of nearest neighbor during global optimization bool time_cal = true; //Whether to perform time supplementary calculation is to interpolate the odometer according to time when finding the pose of each point std::string output_pointcloud_path = ""; //Point cloud file save path std::string output_calibration_path = ""; // Calibration information file saving path };

# Code file structure

There are four header files:

- Aligner. H: class used in lidar and Odom correction (external parameter calculation)
- **loader.h: * * functions related to loading data from Bag or CSV format of ROS
- **sensor.h: * * mainly includes Odom and LIdar related interfaces
- transform.h: Calculation and conversion of some SO3 changes, which are used in interpolation and optimization

cpp file has four:

- aligner.cpp: implementation function for Lidar and Odom correction (external parameter calculation)
- lidar_align_node.cpp: various classes of the start-up local instance of main
- loader.cpp: load data
- sensors.cpp: process lidar and odometer data

# Core code

The previous data reading and processing is not complete. Let's analyze the main code parts below

The following functions are called in the face function. In front of them is data reading and processing. The calibration process is started from this function

aligner.lidarOdomTransform(&lidar, &odom);//Perform calibration

In this function

/*The number of optimization parameters is determined according to whether time compensation is used*/ size_t num_params = 6;//Number of initialization optimization parameters if (config_.time_cal) { ++num_params;//The number of optimization parameters calculated by time compensation is 7 }

The number of optimization parameters is determined according to whether time compensation is used

/*The assignment of the initial angle is performed according to the configuration parameter (whether to perform global optimization). If global optimization is performed, an estimation is performed first to obtain the angle. Otherwise, the set initial value is directly attached */ if (!config_.local) { //Global optimization is performed by default and used for initial estimation //Printing is performing global optimization ROS_INFO("Performing Global Optimization... "); std::vector<double> lb = {-M_PI, -M_PI, -M_PI};//Set lower limit std::vector<double> ub = {M_PI, M_PI, M_PI};//set upper limit std::vector<double> global_x(3, 0.0);//Set the x parameter vector for global optimization to 3, and the initial value is 0.0 optimize(lb, ub, &opt_data, &global_x);//Execute optimization (Global Optimization) only seeks the optimization results of angles config_.local = true;//Turn off the globally optimized flag and perform local optimization next time //Result of assignment angle optimization x[3] = global_x[0]; x[4] = global_x[1]; x[5] = global_x[2]; } else {//If local is true, it is the default initial value x = config_.inital_guess;//If the initial values are all 0, the coordinate axes of lidar and imu are aligned. In other cases, it is best to roughly configure the initial values according to the actual situation }

The assignment of the initial angle is performed according to the configuration parameter (whether to perform global optimization). If global optimization is performed, an estimation is performed first to obtain the angle. Otherwise, the set initial value is directly attached

//Set the lower limit of the parameter vector. The default parameters are - 1, - 1, - 0.5, - 0.5, - 0.5 std::vector<double> lb = { -config_.translation_range, -config_.translation_range, -config_.translation_range, -config_.angular_range, -config_.angular_range, -config_.angular_range}; //Set the upper limit of the parameter vector. The default parameters are 1, 1, 1, 0.5, 0.5, 0.5 std::vector<double> ub = { config_.translation_range, config_.translation_range, config_.translation_range, config_.angular_range, config_.angular_range, config_.angular_range}; //The initial estimated value of the angle obtained by global optimization is superimposed into the upper and lower limits, that is, if the initial estimated roll is 0.1 radians, the optimization range is 0.1 + - 0.5 radians for (size_t i = 0; i < 6; ++i) { lb[i] += x[i];//Stacking lower limit ub[i] += x[i];//Upper stacking limit } //The optimization range of time compensation is added to the upper and lower limits if (config_.time_cal) { ub.push_back(config_.max_time_offset); lb.push_back(-config_.max_time_offset); }

Set the upper and lower limits of optimization factors

/*Perform local optimization*/ optimize(lb, ub, &opt_data, &x);

Perform local optimization

Let's take a look at the main contents of performing local optimization

That is, the optimize() function

Inside is the nonlinear optimization using the nlopt method

if (config_.local) {//It depends on whether the algorithm used for global optimization or local optimization is different. The number of x is also different in the previous configuration. There are 3 global optimizations and 7 local optimizations opt = nlopt::opt(nlopt::LN_BOBYQA, x->size()); } else { opt = nlopt::opt(nlopt::GN_DIRECT_L, x->size());//Select the algorithm direct which does not need derivation for the global optimization function_ The number of L x is three in the previous configuration }

Different algorithms are selected according to whether global optimization or local optimization is performed, and nlopt object instances are generated

//Set lower limit opt.set_lower_bounds(lb); //set upper limit opt.set_upper_bounds(ub); opt.set_maxeval(config_.max_evals);//Set maximum number of estimates opt.set_xtol_abs(config_.xtol);//Set x tolerance stop value opt.set_min_objective(LidarOdomMinimizer, opt_data);//Set the objective function LidarOdomMinimizer function name opt_data external data

nlopt related settings, in which LidarOdomMinimizer is the focus of the objective function

In the objective function

//The nearest neighbor distance sum of each point is calculated by kdtree method double error = d->aligner->lidarOdomKNNError(*(d->lidar));

ad locum

The nearest neighbor distance sum of each point is calculated by kdtree method

Then, keep iterating to minimize this distance, and the obtained optimization vector is the calibration information

The main core is these. Of course, there are too many small details in this. If there are problems with children's shoes, you can confide in me.

# result

Finally, the results of the last test

Active Transformation Vector (x,y,z,rx,ry,rz) from the Pose Sensor Frame to the Lidar Frame:

[0.770924, -0.25834, 0.105557, 1.67974, -1.88141, -1.40085]

Active Transformation Matrix from the Pose Sensor Frame to the Lidar

Frame:

-0.300413 -0.623731 -0.721603 0.770924

-0.870125 -0.130671 0.475193 -0.25834

-0.390685 0.770639 -0.503468 0.105557

0 0 0 1

Active Translation Vector (x,y,z) from the Pose Sensor Frame to the

Lidar Frame: [0.770924, -0.25834, 0.105557]

Active Hamiltonen Quaternion (w,x,y,z) the Pose Sensor Frame to the

Lidar Frame: [-0.127913, -0.577435, 0.646763, 0.481564]

Time offset that must be added to lidar timestamps in seconds:

-0.00263505

ROS Static TF Publisher:

In txt, the result of function package output is like this.