雷达点云畸变校正

具体原理可参见详解2D激光雷达运动畸变去除_白茶-清欢的博客-CSDN博客_运动畸变

本文的主要工作是重新写了上文链接文章中的代码中的标定主要函数（不关注细节只实现功能），具体代码如下

void LidarMotionCalibrator::Lidar_MotionCalibration ( 
tf::Stamped<tf::Pose> frame_base_pose, 
tf::Stamped<tf::Pose> frame_start_pose, 
tf::Stamped<tf::Pose> frame_end_pose, 
std::vector<double>& ranges,
std::vector<double>& angles, 
int startIndex, 
int& beam_number
)
{
       for(int i = startIndex; i < startIndex + beam_number; ++i)
       {
            double x = frame_start_pose.getOrigin().getX() + (frame_end_pose.getOrigin().getX() - frame_start_pose.getOrigin().getX()) / beam_number * ( i - startIndex );
            double y = frame_start_pose.getOrigin().getY() + (frame_end_pose.getOrigin().getY() - frame_start_pose.getOrigin().getY()) / beam_number * ( i - startIndex );
            double yaw = tf::getYaw(frame_start_pose.getRotation()) + (tf::getYaw(frame_end_pose.getRotation()) - tf::getYaw(frame_start_pose.getRotation())) / beam_number * ( i - startIndex );

            Eigen::Matrix3d x_i;
            x_i <<  1, 0, ranges[i] * cos(angles[i]),
                    0, 1, ranges[i] * sin(angles[i]),
                    0, 0, 1;
            Eigen::Matrix3d T_B_O_2;
            T_B_O_2 << cos(yaw), -sin(yaw), x,
                        sin(yaw), cos(yaw), y,
                        0, 0, 1;
            Eigen::Matrix3d T_B_O_1;
            T_B_O_1 << cos(tf::getYaw(frame_base_pose.getRotation())), -sin(tf::getYaw(frame_base_pose.getRotation())), frame_base_pose.getOrigin().getX(),
                        sin(tf::getYaw(frame_base_pose.getRotation())), cos(tf::getYaw(frame_base_pose.getRotation())),frame_base_pose.getOrigin().getY(),
                        0, 0, 1;
            tf::Stamped<tf::Pose> current_pose;
            Eigen::Matrix3d T_O_1_L;
            T_O_1_L = T_B_O_1.inverse() * T_B_O_2 * x_i;
            ranges[i] = std::sqrt(T_O_1_L(0, 2) * T_O_1_L(0, 2) + T_O_1_L(1, 2) * T_O_1_L(1, 2));
            angles[i] = atan2(T_O_1_L(1, 2), T_O_1_L(0, 2));
       }
}