2D和3D激光slam的点云去运动畸变
在使用激光雷达设备采集点云的时候，我们都知道，激光雷达是边运动边采集的，每一个点云采集时的激光雷达的中心和姿态都是不一样的，如果不加以矫正，那么这一帧数据就会出现问题，比如采集一个平面的结构的时候，所采集的数据呈现出来的可能就是一个非平面。所以，需要对采集的一帧数据进行畸变矫正，使它呈现出来正确的结果。
去畸变按照传感器的类型可以分为2D激光雷达去畸变和3D激光雷达去畸变，按照去畸变的方法，也可以分为有imu辅助的激光雷达去畸变和使用匀速运动假设的帧间配准矩阵来去点云运动畸变。
总结一点就是，2D激光雷达一般只用imu获取的角度来矫正，也就是只用旋转矩阵来矫正。3D激光雷达一般用imu辅助矫正，如果没有imu信息，那么就用匀速矫正模型来矫正。匀速矫正模型在ALOAM中体现，而imu辅助的雷达点云矫正模型则在LOAM代码中有体现。
那么这次的博客分为两个部分：
一、2D激光雷达去畸变
1)我们来看看2d_lidar_undistortion-master这个代码
1. 同样使用的是imu辅助的点云去畸变方法，不过这里只用到了旋转，没有对平移进行畸变矫正，也可以能使2d激光雷达运动相对缓慢？不太清楚。但是看代码，就是很常规的，找到该雷达点时间戳所在的imu时间段，然后取该imu时间段的起始点和结束点的orientation也就是旋转四元素，然后使用四元素插值就可以得到该雷达点所在的坐标系相对与起始点的旋转矩阵。
bool getLaserPose(int point_index, ros::Time &_timestamp, Eigen::Quaternionf &quat_out){static int index_front = 0;static int index_back = 0;static ros::Time timestamp_front;static ros::Time timestamp_back;static Eigen::Quaternionf quat_front, quat_back;static bool index_updated_flag = false;static bool predict_orientation_flag = false;if (point_index == 0){predict_orientation_flag = false;int i = 0;while (_timestamp < imuCircularBuffer_[i].header.stamp){i++;}index_front = i - 1;index_back = i;index_updated_flag = true;}else{while (predict_orientation_flag == false&& _timestamp > imuCircularBuffer_[index_front].header.stamp&& _timestamp > imuCircularBuffer_[index_back].header.stamp){index_front--;index_back--;if (index_front < 0){//use predictionpredict_orientation_flag = true;index_front++;index_back++;}index_updated_flag = true;}}if (index_updated_flag == true){//cout << "index updated: " << index_front << " " << index_back << endl;timestamp_front = imuCircularBuffer_[index_front].header.stamp;timestamp_back = imuCircularBuffer_[index_back].header.stamp;quat_front = Eigen::Quaternionf(imuCircularBuffer_[index_front].orientation.w, imuCircularBuffer_[index_front].orientation.x, imuCircularBuffer_[index_front].orientation.y, imuCircularBuffer_[index_front].orientation.z);quat_back = Eigen::Quaternionf(imuCircularBuffer_[index_back].orientation.w, imuCircularBuffer_[index_back].orientation.x, imuCircularBuffer_[index_back].orientation.y, imuCircularBuffer_[index_back].orientation.z);index_updated_flag = false;}float alpha = (float)(_timestamp.toNSec() - timestamp_back.toNSec()) / (timestamp_front.toNSec() - timestamp_back.toNSec());if (alpha < 0){return false;}// 球面线性插值// Slerp.quat_out = quat_back.slerp(alpha, quat_front);return true;}2.矫正方法同样的比较简单
直接使用插值所的的旋转矩阵把雷达点变换到世界坐标系中，然后使用start点的旋转矩阵求逆，然后乘上世界点，就可以把imu世界点变换到start点所在的当前坐标系下，也就是假设start点的坐标系旋转矩阵是单位矩阵，平移矩阵是（0，0，0）。这样就完成了点的畸变矫正。
// 如果成功获取当前扫描点的姿态if (getLaserPose(i, point_timestamp, point_quat) == true){Eigen::Quaternionf delta_quat = current_quat.inverse() * point_quat;Eigen::Vector3f point_out = delta_quat * point_in;point_xyzi.x = point_out(0);point_xyzi.y = point_out(1);point_xyzi.z = 0.0;point_xyzi.intensity = current_point.intensity;pointcloud_pcl.push_back(point_xyzi);}2D激光雷达基本上就没有使用到平移量的。
二、3D激光雷达去畸变
1）使用imu去畸变
1.看fast-lio的点云去畸变
核心的代码：
M3D R_i(R_imu * Exp(angvel_avr, dt));这个就是计算出来该点所在的imu的姿态。
V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z); 这个就是激光雷达坐标系下的该点坐标
V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos);由速度和加速度所带来的位移增量。
V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate!
分开来看
imu_state.offset_R_L_I和imu_state.offset_T_L_I 分别表示雷达到imu的旋转和平移矩阵。1.就是把激光雷达坐标系中的点变换到imu当前坐标系下
imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I2. 就是把imu当前坐标系下的点变换到imu世界坐标系下
(R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei)3.这一句呢就是把点变换到该帧第一个点的坐标系下
(imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei)4.就是把点从imu当前帧起始imu坐标系下，变换到与之对应的激光雷达自身坐标系下
imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);这样就完成了激光点云的去畸变。
整体代码如下：
if(lidar_type != MARSIM){auto it_pcl = pcl_out.points.end() - 1;for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--){auto head = it_kp - 1;auto tail = it_kp;R_imu<<MAT_FROM_ARRAY(head->rot);// cout<<"head imu acc: "<<acc_imu.transpose()<<endl;vel_imu<<VEC_FROM_ARRAY(head->vel);pos_imu<<VEC_FROM_ARRAY(head->pos);acc_imu<<VEC_FROM_ARRAY(tail->acc);angvel_avr<<VEC_FROM_ARRAY(tail->gyr);for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --){dt = it_pcl->curvature / double(1000) - head->offset_time;/* Transform to the 'end' frame, using only the rotation* Note: Compensation direction is INVERSE of Frame's moving direction* So if we want to compensate a point at timestamp-i to the frame-e* P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */M3D R_i(R_imu * Exp(angvel_avr, dt));V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z);V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos);V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate!// save Undistorted points and their rotationit_pcl->x = P_compensate(0);it_pcl->y = P_compensate(1);it_pcl->z = P_compensate(2);if (it_pcl == pcl_out.points.begin()) break;}}}
}
2）看loam的去畸变
1.首先对imu的线加速度去掉重力干扰
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{double roll, pitch, yaw;tf::Quaternion orientation;//convert Quaternion msg to Quaterniontf::quaternionMsgToTF(imuIn->orientation, orientation);//This will get the roll pitch and yaw from the matrix about fixed axes X, Y, Z respectively. That's R = Rz(yaw)*Ry(pitch)*Rx(roll).//Here roll pitch yaw is in the global frametf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);//减去重力的影响,求出xyz方向的加速度实际值，并进行坐标轴交换，统一到z轴向前,x轴向左的右手坐标系, 交换过后RPY对应fixed axes ZXY(RPY---ZXY)。Now R = Ry(yaw)*Rx(pitch)*Rz(roll).float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;//循环移位效果，形成环形数组imuPointerLast = (imuPointerLast + 1) % imuQueLength;imuTime[imuPointerLast] = imuIn->header.stamp.toSec();imuRoll[imuPointerLast] = roll;imuPitch[imuPointerLast] = pitch;imuYaw[imuPointerLast] = yaw;imuAccX[imuPointerLast] = accX;imuAccY[imuPointerLast] = accY;imuAccZ[imuPointerLast] = accZ;AccumulateIMUShift();
}2.对imu数据进行积分
这里假设了当前帧的第一个点的初始速度是0，所以shiftx,shifty,shiftz和真实世界不是完全对应的。
//积分速度与位移
void AccumulateIMUShift()
{float roll = imuRoll[imuPointerLast];float pitch = imuPitch[imuPointerLast];float yaw = imuYaw[imuPointerLast];float accX = imuAccX[imuPointerLast];float accY = imuAccY[imuPointerLast];float accZ = imuAccZ[imuPointerLast];//将当前时刻的加速度值绕交换过的ZXY固定轴（原XYZ）分别旋转(roll, pitch, yaw)角，转换得到世界坐标系下的加速度值(right hand rule)//绕z轴旋转(roll)float x1 = cos(roll) * accX - sin(roll) * accY;float y1 = sin(roll) * accX + cos(roll) * accY;float z1 = accZ;//绕x轴旋转(pitch)float x2 = x1;float y2 = cos(pitch) * y1 - sin(pitch) * z1;float z2 = sin(pitch) * y1 + cos(pitch) * z1;//绕y轴旋转(yaw)accX = cos(yaw) * x2 + sin(yaw) * z2;accY = y2;accZ = -sin(yaw) * x2 + cos(yaw) * z2;//上一个imu点int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;//上一个点到当前点所经历的时间，即计算imu测量周期double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];//要求imu的频率至少比lidar高，这样的imu信息才使用，后面校正也才有意义if (timeDiff < scanPeriod) {//（隐含从静止开始运动）//求每个imu时间点的位移与速度,两点之间视为匀加速直线运动/*这里有一个问题，就是一开始imushiftx,imushifty,imushiftz都是0，inuvelox imuveloy imuveloz也都是0，那么是否就需要设备从静止开始采集点云呢？ 否则预积分累计出来的不是真实的速度，只是假设开始时刻速度和位置为0。位置为0尚且可以统一，速度为0那怎么来解释呢？*/imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff + accX * timeDiff * timeDiff / 2;imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff + accY * timeDiff * timeDiff / 2;imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff + accZ * timeDiff * timeDiff / 2;imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;}
}3.当然就是插值了
//点时间=点云时间+周期时间if (imuPointerLast >= 0) {//如果收到IMU数据,使用IMU矫正点云畸变float pointTime = relTime * scanPeriod;//计算点的周期时间//寻找是否有点云的时间戳小于IMU的时间戳的IMU位置:imuPointerFrontwhile (imuPointerFront != imuPointerLast) {if (timeScanCur + pointTime < imuTime[imuPointerFront]) {break;}imuPointerFront = (imuPointerFront + 1) % imuQueLength;}if (timeScanCur + pointTime > imuTime[imuPointerFront]) {//没找到,此时imuPointerFront==imtPointerLast,只能以当前收到的最新的IMU的速度，位移，欧拉角作为当前点的速度，位移，欧拉角使用imuRollCur = imuRoll[imuPointerFront];imuPitchCur = imuPitch[imuPointerFront];imuYawCur = imuYaw[imuPointerFront];imuVeloXCur = imuVeloX[imuPointerFront];imuVeloYCur = imuVeloY[imuPointerFront];imuVeloZCur = imuVeloZ[imuPointerFront];imuShiftXCur = imuShiftX[imuPointerFront];imuShiftYCur = imuShiftY[imuPointerFront];imuShiftZCur = imuShiftZ[imuPointerFront];} else {//找到了点云时间戳小于IMU时间戳的IMU位置,则该点必处于imuPointerBack和imuPointerFront之间，据此线性插值，计算点云点的速度，位移和欧拉角int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;//按时间距离计算权重分配比率,也即线性插值float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI) {imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack;} else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI) {imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;} else {imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;}//本质:imuVeloXCur = imuVeloX[imuPointerback] + (imuVelX[imuPointerFront]-imuVelX[imuPoniterBack])*ratioFrontimuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;}if (i == 0) {//如果是第一个点,记住点云起始位置的速度，位移，欧拉角imuRollStart = imuRollCur;imuPitchStart = imuPitchCur;imuYawStart = imuYawCur;imuVeloXStart = imuVeloXCur;imuVeloYStart = imuVeloYCur;imuVeloZStart = imuVeloZCur;imuShiftXStart = imuShiftXCur;imuShiftYStart = imuShiftYCur;imuShiftZStart = imuShiftZCur;} else {//计算之后每个点相对于第一个点的由于加减速非匀速运动产生的位移速度畸变，并对点云中的每个点位置信息重新补偿矫正ShiftToStartIMU(pointTime);VeloToStartIMU();TransformToStartIMU(&point);}}laserCloudScans[scanID].push_back(point);//将每个补偿矫正的点放入对应线号的容器}主要函数就是
ShiftToStartIMU(pointTime);VeloToStartIMU();TransformToStartIMU(&point); ShiftToStartIMU(pointTime);去掉了匀速运动所产生的位移偏移，只保留了加速度带来的位置偏移。并且把该加速度产生的位移量变换到了 start点坐标系下。
VeloToStartIMU();速度也变换到第一个点所在的坐标系下。
TransformToStartIMU(&point);把当前点从当前的imu坐标系下变换到imu世界坐标系中，然后在把点变换到imustart点坐标系下。然后有加上了ShiftToStartIMU函数所产生的在start坐标下下由加速度所产生的位移。
这样就完成了点云的畸变矫正。
3）看aloam中的畸变矫正
11.首先看看这两个参数是什么意思？ceres中优化的是param_q和para_t，param_q和para_t构建了q_last_curr和t_last_curr这两个变量。
Eigen::Map<Eigen::Quaterniond> q_last_curr(para_q);
Eigen::Map<Eigen::Vector3d> t_last_curr(para_t);2.接着看怎么去畸变
去畸变主要是在transofrmtostart中，而transoformtoend是把点云从start坐标系中变换到end坐标系中，当然这时候start和end之间的变换关系就是q_last_curr和t_last_curr，也就是说通过q_lat_curr和t_ladt_curr把start坐标系下的点变换到世界坐标系下。
那我们来看看如何去畸变，也就是把q_last_curr和t_last_curr进行插值来计算的。插值是根据时间算出来的，时间呢是根据水平角度差算出来的。
畸变矫正就是认为从start到end这一段时间内是匀速运动，
小点1. 平移量的插值
由于平移量t_last_curr本身就是假设的的最后一个点的坐标系（类比imu）的源点在start坐标系中的位置，也就是说获取最后一个点的时候，激光雷达运动到了t_last_curr这个位置（当然是在start系中），所以直接使用s*t_last_curr就可以了。
小点2. 旋转的插值
同样的认为从start到end是匀速运动的，那么角速度也是匀速的，所以直接用四元数插值就好了，就得到了当前i点所在的坐标系。
// undistort lidar point
void TransformToStart(PointType const *const pi, PointType *const po) // 去畸变
{//interpolation ratio 插值比double s;if (DISTORTION) // 默认为假，kitti数据集上的lidar已经做过了运动补偿，这里就不用了s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;elses = 1.0;//s = 1;//运动补偿：将一帧点云中所有不同时间辍的位置点补偿到同一时间戳，补偿需要知道每个时间戳对应的的点的位置，在这里模型为匀速模型假设Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr); // 两个四元数之间的插值，slerp()函数用于执行球面线性插值，s插值参数，指定两个四元数之间插的位置Eigen::Vector3d t_point_last = s * t_last_curr;Eigen::Vector3d point(pi->x, pi->y, pi->z);Eigen::Vector3d un_point = q_point_last * point + t_point_last;po->x = un_point.x();po->y = un_point.y();po->z = un_point.z();po->intensity = pi->intensity;
}// transform all lidar points to the start of the next framevoid TransformToEnd(PointType const *const pi, PointType *const po)
{// undistort point firstpcl::PointXYZI un_point_tmp;TransformToStart(pi, &un_point_tmp);Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z);Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr);po->x = point_end.x();po->y = point_end.y();po->z = point_end.z();//Remove distortion time infopo->intensity = int(pi->intensity);
}2.中分析的匀速运动假设的畸变矫正在ceres优化中同样可以看到
比如通过这样的匀速假设插值就得到了当前点的畸变校正坐标系。
q_last_curr = q_identity.slerp(T(s), q_last_curr);Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};
具体代码：
struct LidarEdgeFactor
{LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,Eigen::Vector3d last_point_b_, double s_): curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {}template <typename T>bool operator()(const T *q, const T *t, T *residual) const{Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};//Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};q_last_curr = q_identity.slerp(T(s), q_last_curr);Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};Eigen::Matrix<T, 3, 1> lp;lp = q_last_curr * cp + t_last_curr;Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb); // 叉乘Eigen::Matrix<T, 3, 1> de = lpa - lpb;residual[0] = nu.x() / de.norm();residual[1] = nu.y() / de.norm();residual[2] = nu.z() / de.norm();return true;}