Motion Distortion Correction of Laser Point Cloud Using IMU

Motion Distortion Correction of Laser Point Cloud Using IMU

1. Principles of Thought

In the process of laser SLAM positioning and mapping, when the lidar moves slowly and does not have a relatively large rotation, the motion distortion of the point cloud can be ignored, and the pose of the radar can still be estimated by ICP registration during frame-to-frame matching. However, when the lidar motion range is relatively large, the motion distortion of the laser point cloud has to be considered, and the short-term pose estimation information provided by the IMU or encoder can be used to remove the motion distortion of the point cloud. Here I use the IMU to remove the motion distortion of the point cloud. The acceleration information integral output by the IMU is very divergent. If it is a worse IMU device, the acceleration information will not be used. Here, the angular velocity information of the IMU is considered. In the point cloud time of 1 frame, the angular velocity of all IMUs is integrated to obtain the rotation angle of the IMU, and then converted to the lidar, and the attitude quaternion spherical linear interpolation is performed on the attitude of the radar to convert the corresponding point cloud to the attitude of the final state to realize the removal of point cloud motion distortion.

2. Operation process

Equipment used:

• Lidar: Velodyne VLP16, frequency 10HZ, mechanical rotation, 16 lines, in the vertical direction $(-15^{\circ },15^{\circ })$, the angle between each line is$2^{\circ }$ , there are 1800 laser points on the horizontal plane, the average resolution$0.2^{\circ }$。
• IMU: D435i, the frequency is 200HZ.

specific process:

1. Angular velocity integration for the IMU : The angular velocity integration refers to the pre-integration process of VINS-Mono, and the frequency of the lidar is much lower than the frequency of the IMU. In one cycle of a point cloud data (100ms), the IMU will generate about $200÷10=20$个，根据公式：
   $$\begin{gathered} a_t=\frac{{oh}_{t_0}+{oh}_t}{2}\cdot dt \\ dt=t-t_0 \\ a=\sum _{i=0}^{n-1}{a}_i \end{gathered}$$
   where $a_t$for $Time\; t$ is relative to the previous time$t_0$The angle change of ${oh}_{t_0},{oh}_t$are the angular velocity at the corresponding moment, $n$ IMU data. In this way, the IMU rotation angle in one cycle can be obtained by continuously integrating, accumulating and iterating.

   Here, the calculation method of quaternion is adopted. Quaternion $q=\left[\begin{array}{c}cos(\frac{}{2})\\ sin(\frac{}{2})\cdot r\end{array}\right]=\left[\begin{array}{c}w\\ x\\ y\\ z\end{array}\right]$, for adjacent IMU data $\frac{}{2}$All tend to 0, available equivalent infinitesimal
   $$\begin{gathered} \underset{dog\to 0}{\lim }\frac{sin(dog}{dog}=1 \\ \underset{dog\to 0}{\lim }cos\left(dog\right)=1 \end{gathered}$$
   则有：
   $$\begin{gathered} q_t=q_{t_0}\cdot q_{dt} \\ {q}_{dt}=(1,\frac{a_x}{2},\frac{a_y}{2},\frac{a_z}{2}) \end{gathered}$$
   where$q_t$Attitude quaternion at the moment, $q_{t_0}$is the attitude quaternion at the previous moment, $q_{dt}$is the change value of quaternion in this time period, $a_x$, $a_y$, $a_z$is the corresponding XYZ three-axis angle change.

   Attach part of the code of ROS+Eigen

   std::vector<sensor_msgs::Imu::ConstPtr> imu_msg_vector;
   std::mutex mutex_lock;
   
   // IMU消息回调存储
   void imu_cb(const sensor_msgs::Imu::ConstPtr &imu_msg)
   {
         
         
       mutex_lock.lock();
       imu_msg_vector.push_back(imu_msg);
       mutex_lock.unlock();
   }
   
   // IMU角度积分
   Eigen::Quaterniond imu_q = Eigen::Quaterniond::Identity(); // R
   Eigen::Vector3d angular_velocity_1;
   angular_velocity_1 << imu_msg_v[0]->angular_velocity.x, imu_msg_v[0]->angular_velocity.y, imu_msg_v[0]->angular_velocity.z;
   double lastest_time = imu_msg_v[0]->header.stamp.toSec();
   for (int i = 1; i < imu_msg_v.size(); i++)
   {
         
         
       double t = imu_msg_v[i]->header.stamp.toSec();
       double dt = t - lastest_time;
       lastest_time = t;
       Eigen::Vector3d angular_velocity_2;
       angular_velocity_2 << imu_msg_v[i]->angular_velocity.x, imu_msg_v[i]->angular_velocity.y, imu_msg_v[i]->angular_velocity.z;
       Eigen::Vector3d aver_angular_vel = (angular_velocity_1 + angular_velocity_2) / 2.0;
       imu_q = imu_q * Eigen::Quaterniond(1, 0.5 * aver_angular_vel(0) * dt, 0.5 * aver_angular_vel(1) * dt, 0.5 * aver_angular_vel(2) * dt);
   }
   

2. IMU->radar attitude transformation : The attitude angle change of the IMU measured above needs to be mapped to the lidar coordinate system. The implementation needs to know the external parameter transformation transformation matrix of the lidar and IMU (the translation vector is not used, but the rotation matrix must be), $R_{lidar}^{IMU}$It is the rotation matrix from lidar to IMU (need to be calibrated in advance). During the movement of lidar and IMU, it can be considered as $R_{lidar}^{IMU}$Unchangeable. Related:
   $$R_{lida{r}_0}^{lida{r}_1}\cdot R_{lidar}^{IMU}=R_{lidar}^{IMU}\cdot R_{IM{U}_0}^{IM{U}_1}$$
   任何$R_{lida{r}_0}^{lida{r}_1}$It is the attitude transformation from the beginning state to the end state of the radar, $R_{IMU_0}^{IMU_1} is the attitude $It\; isthe\; attitudetransformationfromtheinitialstatetothe\; finalstate\; ofIMU(thatis,theIMUattitudeanglequaternion$ q$$integrated$ $above$$).$Multiply the formula by the inverse at the same time, we can get:
   $$R_{lida{r}_0}^{lida{r}_1}=R_{lidar}^{IMU}\cdot R_{IM{U}_0}^{IM{U}_1}\cdot {R_{lidar}^{IMU}}^{-1}$$
   Eigen code:

   Eigen::Matrix3d lidar_imu_R << -1,     -1.22465e-16,            0,
             1.22465e-16,          -1,            0,
              0,            0,           1;     // lidar->IMU 外参数旋转矩阵
   Eigen::Matrix3d lidar_R;
   lidar_R = lidar_imu_R * (imu_q.toRotationMatrix()) * lidar_imu_R.inverse();
   

   The effect is as follows:
   

3. Perform spherical linear interpolation conversion on the attitude quaternion of the radar : in one cycle (100ms), the radar rotates clockwise on the top view, and the horizontal angle is as follows:
   

   In a horizontal plane of 360 degrees, divided into 1800 parts, first calculate the angle θ i \theta_i corresponding to each point cloud$i_i$:
   $$\begin{gathered} i_i=arccos\left(\frac{x_i}{\sqrt{x_i^2+y_i^2}}\right),if{y}_i<0 \\ {i}_i=2p.m-arccos\left(\frac{x_i}{\sqrt{x_i^2+y_i^2}}\right),ify{}_i>0 \end{gathered}$$
   再进行插值，注意是将当前时刻的点云转换到末状态雷达姿态上：
   $$q_{slerp}=Slerp\left(q_0,q_1;t\right)=q_0{\left(q_0^{-1}{q}_1\right)}^t=q_1{\left(q_1^{-1}{q}_0\right)}^{1-t}={\left(q_0{q}_1^{-1}\right)}^{1-t}{q}_1={\left(q_1{q}_0^{-1}\right)}^t{q}_0$$
   where $q_0$is the starting quaternion, $q_1$is the final state quaternion, $t$ is the step size of interpolation,$t\in [0,1]$。

   The quaternion $q_{slerp}$Convert to rotation matrix $R_{slerp}$The form can then use the coordinate transformation formula:
   $$P_1=\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\\ 1\end{array}\right]=T_{slerp}\cdot P_0=\left[\begin{array}{cc}{R}_{slerp}& 0\\ 0& 1\end{array}\right]\cdot \left[\begin{array}{c}{x}_0\\ y_0\\ z_0\\ 1\end{array}\right]$$
   where $P_0$is the original point cloud coordinates, $P_1$is the converted point cloud coordinates.

   Attach part of the Eigen code:

   pcl::PointCloud<pcl::PointXYZI> cloud_undistorted;
   pcl::copyPointCloud(cloud_in, cloud_undistorted);
   
   for (int i = 0; i < cloud_undistorted.points.size(); i++)
   {
         
         
       double horizon_angle; // 化为角度为单位
       if (cloud_rgb.points[i].y < 0)
       {
         
         
           double theta = acos(cloud_undistorted.points[i].x / sqrt(cloud_undistorted.points[i].x * cloud_undistorted.points[i].x + cloud_undistorted.points[i].y * cloud_undistorted.points[i].y));
           horizon_angle = theta * 180.0 / M_PI;
       }
       else
       {
         
         
           double theta = acos(cloud_undistorted.points[i].x / sqrt(cloud_undistorted.points[i].x * cloud_undistorted.points[i].x + cloud_undistorted.points[i].y * cloud_undistorted.points[i].y));
           horizon_angle = (2 * M_PI - theta) * 180 / M_PI;
       }
       int id = int(horizon_angle / 360.0 * 1800);
       // std::cout << "horizon_angle: " << horizon_angle << ", id: " << id << std::endl;
   
       Eigen::Quaterniond q_slerp = lidar_q0.slerp((1.0 - horizon_angle / 360.0), lidar_q);
       Eigen::Matrix3d R_slerp = q_slerp.toRotationMatrix();
       Eigen::Matrix4d T_lidar_slerp;
       T_lidar_slerp << R_slerp(0, 0), R_slerp(0, 1), R_slerp(0, 2), 0,
           R_slerp(1, 0), R_slerp(1, 1), R_slerp(1, 2), 0,
           R_slerp(2, 0), R_slerp(2, 1), R_slerp(2, 2), 0,
           0, 0, 0, 1;
       Eigen::Vector4d Pi, Pj;
       Pi << cloud_undistorted.points[i].x, cloud_undistorted.points[i].y, cloud_undistorted.points[i].z, 1;
       Pj = T_lidar_slerp.inverse() * Pi;
       cloud_undistorted.points[i].x = Pj(0);
       cloud_undistorted.points[i].y = Pj(1);
       cloud_undistorted.points[i].z = Pj(2);
   }
   

4. Effect:

   Single-frame point cloud processing effect:
   
   the white point cloud in the figure is the original data, and the green point cloud is the de-distorted point cloud effect. In this way, the actual effect cannot be seen, and then there is an example of building a map to illustrate.

   Outdoor corridor environment:
   
   red is the direction of travel of the handheld lidar. Using A-LOAM for mapping, without de-distortion point cloud construction:
   
   After de-distortion, point cloud construction is still as stable as an old dog in a large environment with large rotation, the effect is as follows: