当前位置：首页 > article >正文

A-LOAM工程笔记（三）：雷达里程计(lidar odometry)高频粗估计

article 2025/3/13 10:41:54

1.前言

雷达里程计部分实现点到线，点到面匹配，在雷达坐标系下，通过点到线及点到面距离，利用LM得到雷达位姿高频估计，输出10hz。主要包含点云运动补偿，点云匹配，线面误差项优化求解位姿三部分。

2.运动补偿

由于激光lidar在采集数据的时候是运动的，导致采集到的数据并不是在统一坐标系下的及在一个 sweep 期间，不同时刻 Lidar 坐标系不一样。所以需要对激光lidar进行补偿，将一帧数据转换到统一的lidar坐标系下的。及根据插值将Pk映射到k时刻sweep起始坐标系下(点云去掉运动漂移)，一般来说是转换到这一帧的起始时刻，运动补偿公式：

$P_{start} = T_{start2current} * P_{current}$

LOAM假设一帧内是匀速运动，基于上一帧的运动差值为这一帧运动作补偿。通过时间dt的线性插值计算补偿位姿量。以代码为例:

void TransformToStart(PointType const *const pi, PointType *const po)
{
    //interpolation ratio
    double s;
    if (DISTORTION)
        s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;
    else
        s = 1.0;
    //s = 1;
    Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);
    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;
}

s表示比例，其中pi->intensity表示点云的强度值，在前面的点云数据预处理中，强度值的实数部分存的是Scan_id，虚数部分存的是该点相对于起始时刻的漂移。这里算出的s就表示该点的时间相当于整个帧时间的比例。DISTORITI

查看全文

http://www.kler.cn/a/582632.html