LOAM: LiDAR Odometry and Mapping In Real Time源码解析(二)

LOAM：LiDAR Odometry and Mapping In Real Time源码解析（二）

LiDAR Odometry

LiDAR Odometry

输入

从laserOdometry.cpp文件中的主函数中可以看出，LiDAR Odometry部分的输入是scanRegistration部分(可见上一篇博客)提取的sharp corner、less sharp corner、flat surface以及less flat surface特征：

ros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100, laserCloudSharpHandler);
ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100, laserCloudLessSharpHandler);
ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100, laserCloudFlatHandler);
ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100, laserCloudLessFlatHandler);

去点云畸变

熟悉激光SLAM算法的小伙伴应该对激光雷达点云畸变并不陌生，由于激光雷达是一种连续测量传感器，而不是像摄像头那样可以理解为一瞬间对环境的采样。这导致激光雷达原始点云会产生畸变，为消除这种畸变，原始的LOAM开源代码中采用了两种方法来去点云畸变。一种是利用IMU积分出的位姿来插值每一个激光雷达扫描点扫描处相对于该帧点云起始处的相对位姿，并恢复出该点在该帧点云起始处坐标系下的位置；另一种是利用假设激光雷达匀速运动，利用上一帧的激光里程计帧间相对位姿，插值去除点云畸变。A-LOAM中复现了第二种方法，如下代码所示。其中宏DISTORTION默认为0，q_last_curr和t_last_curr为上一帧激光里程计计算出的帧间相对运动。人并不是很喜欢这种匀速运动假设的去点云畸变方法，即使假设激光雷达满足匀速运动假设，若上一帧激光里程计的帧间相对运动估计误差较大，则会影响这一帧的点云去畸变效果，则又会进一步影响这一帧的激光里程计相对运动估计。

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;
}

corner特征关联

LOAM算法中进行 corner特征关联时，实际上寻找的是当前sharp corner特征中的一个点 $p_{curr}$ 与上一帧less sharp corner特征中的两个点 $p_a, p_b$ 间的对应关系。为确定对应关系，首先需要将这些点统一到同一个坐标系下，即如代码中TransformToStart函数所示。统一到同一个坐标系后，首先利用最近邻搜索，在上一帧less sharp corner特征中搜索距离 $p_{curr}$ 的最近点 $p_a$ ，搜索到 $p_a$ 后，从其intensity信息中可以确定其对应的scanlineID。然后该scanline上下方附近的scanline对应的特征点云中，继续搜索距离 $p_{curr}$ 的最近点 $p_b$ 。

for (int i = 0; i < cornerPointsSharpNum; ++i){ 
    // 匀速运动假设去点云运动畸变，若将宏DISTORTION设置为0，则可以理解为利用上一帧的相对运动预测当前帧的相对运动
    TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);
    // 最近邻搜索，kdtreeCornerLast为上一帧corner特征对应的kd树
    kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
    int closestPointInd = -1, minPointInd2 = -1;                                 
    if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD){
        closestPointInd = pointSearchInd[0];                            
        int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);
        double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;                            
        // search in the direction of increasing scan line
        for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j){
            // if in the same scan line, continue                                
            if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)                                    
                continue;
            // if not in nearby scans, end the loop                                
            if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))                                    
                break;
            double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * (laserCloudCornerLast->points[j].x - pointSel.x) + (laserCloudCornerLast->points[j].y - pointSel.y) * (laserCloudCornerLast->points[j].y - pointSel.y) + (laserCloudCornerLast->points[j].z - pointSel.z) * (laserCloudCornerLast->points[j].z - pointSel.z);
            // 在closestPointScanID上方的scanline中找到另一个距离当前sharp corner较近的corner特征                                                                                                                                                                                                                                                                                 
            if (pointSqDis < minPointSqDis2){                                    
                // find nearer point                                    
                minPointSqDis2 = pointSqDis;
                minPointInd2 = j;
            }
        }                                                                                                     
        // search in the direction of decreasing scan line
        for (int j = closestPointInd - 1; j >= 0; --j){
            // if in the same scan line, continue
            if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID)
                continue;                                                                                                                                                       
            // if not in nearby scans, end the loop                                
            if (int(laserCloudCornerLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))                                    
                break;
            double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * (laserCloudCornerLast->points[j].x - pointSel.x) + (laserCloudCornerLast->points[j].y - pointSel.y) * (laserCloudCornerLast->points[j].y - pointSel.y) + (laserCloudCornerLast->points[j].z - pointSel.z) * (laserCloudCornerLast->points[j].z - pointSel.z);
            // 在closestPointScanID下方的scanline中找到另一个距离当前sharp corner较近的corner特征                                                                                                                                                                                                                                                                                    
            if (pointSqDis < minPointSqDis2){                                    
                // find nearer point                                    
                minPointSqDis2 = pointSqDis;
                minPointInd2 = j;
            }
        }
    }
    if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid
    {
        Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x, cornerPointsSharp->points[i].y, cornerPointsSharp->points[i].z);
        Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x, laserCloudCornerLast->points[closestPointInd].y, laserCloudCornerLast->points[closestPointInd].z);
        Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x, laserCloudCornerLast->points[minPointInd2].y, laserCloudCornerLast->points[minPointInd2].z);
        double s;
        if (DISTORTION)                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              
            s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD;
        else
            s = 1.0;
        ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
        problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
        corner_correspondence++;
    }
}

找到满足距离阈值要求的关联点 $p_a, p_b$ 后，点 $p_a, p_b$ 可确定一条直线，即可建立点到线距离的残差函数。LOAM原始开源代码中，Zhang Ji大佬手写的Gauss-Newton优化库，计算了各种Jocabian。A-LOAM简化了这一较为繁琐容易出错的过程，改为用Ceres实现。

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], 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;                          
}

简单解释一下残差函数的意义，即给定空间中不共线的三点 $A, B, C$ ，向量 $\vec{AC},\vec{BC}$ 叉乘后得到的向量的模表示三角形ABC的面积 $S_{ABC}$ ， $S_{ABC}$ 除以向量 $\vec{AB}$ 的模长，即为点C到线段AB的距离。

surface特征关联

LOAM算法中进行surface特征关联时，实际上寻找的是当前flat surface特征中的一个点 $p_{curr}$ 与上一帧less flat surface特征中的三个点 $p_a, p_b, p_c$ 间的对应关系。为确定对应关系，首先需要将这些点统一到同一个坐标系下，即如代码中TransformToStart函数所示。统一到同一个坐标系后，首先利用最近邻搜索，在上一帧lessflat surface特征中搜索距离 $p_{curr}$ 的最近点 $p_a$ ，搜索到 $p_a$ 后，从其intensity信息中可以确定其对应的scanlineID。然后在当前scanline内或当前scanline下方附近scanline内寻找距离 $p_{curr}$ 最近的特征点 $p_b$ ，在当前scanline上方附近scanline内寻找寻找距离 $p_{curr}$ 最近的特征点 $p_c$ 。

for (int i = 0; i < surfPointsFlatNum; ++i){
    // 匀速运动假设去点云运动畸变，若将宏DISTORTION设置为0，则可以理解为利用上一帧的相对运动预测当前帧的相对运动
    TransformToStart(&(surfPointsFlat->points[i]), &pointSel);
    // 最近邻搜索，在上一帧less flat surface特征中搜索距离当前flat surface最近的特征点
    kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);                                                  
    int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
    if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD){
        closestPointInd = pointSearchInd[0];                                                                            
        // get closest point's scan ID
        int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);                            
        double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;                                               
        // search in the direction of increasing scan line                           
        for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j){                                
            // if not in nearby scans, end the loop                                
            if (int(laserCloudSurfLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))                                    
                break;
            double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * (laserCloudSurfLast->points[j].x - pointSel.x) + (laserCloudSurfLast->points[j].y - pointSel.y) * (laserCloudSurfLast->points[j].y - pointSel.y) + (laserCloudSurfLast->points[j].z - pointSel.z) * (laserCloudSurfLast->points[j].z - pointSel.z);                                                                                                                                                                                                                                                                                 
            // if in the same or lower scan line                                
            if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2){                                    
                minPointSqDis2 = pointSqDis;
                minPointInd2 = j;
            }
            // if in the higher scan line
            else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3){
                minPointSqDis3 = pointSqDis;
                minPointInd3 = j;
            }
        }                                                                                                                                                                                                                                                                                                         
        // search in the direction of decreasing scan line                            
        for (int j = closestPointInd - 1; j >= 0; --j){                                
            // if not in nearby scans, end the loop                                
            if (int(laserCloudSurfLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))                                    
                break;
            double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) * (laserCloudSurfLast->points[j].x - pointSel.x) + (laserCloudSurfLast->points[j].y - pointSel.y) * (laserCloudSurfLast->points[j].y - pointSel.y) + (laserCloudSurfLast->points[j].z - pointSel.z) * (laserCloudSurfLast->points[j].z - pointSel.z);                                                                                                                                                                                                                                                                                  
            // if in the same or higher scan line                                
            if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2){                                    
                minPointSqDis2 = pointSqDis;                                    
                minPointInd2 = j;                                
            }                                
            else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3){                                    
                // find nearer point                                    
                minPointSqDis3 = pointSqDis;                                    
                minPointInd3 = j;                                
            }                            
        }
        if (minPointInd2 >= 0 && minPointInd3 >= 0){
            Eigen::Vector3d curr_point(surfPointsFlat->points[i].x, surfPointsFlat->points[i].y, surfPointsFlat->points[i].z);
            Eigen::Vector3d last_point_a(laserCloudSurfLast->points[closestPointInd].x, laserCloudSurfLast->points[closestPointInd].y, laserCloudSurfLast->points[closestPointInd].z);
            Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x, laserCloudSurfLast->points[minPointInd2].y, laserCloudSurfLast->points[minPointInd2].z);
            Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x, laserCloudSurfLast->points[minPointInd3].y, laserCloudSurfLast->points[minPointInd3].z);                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            
            double s;                                
            if (DISTORTION)                                    
                s = (surfPointsFlat->points[i].intensity - int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD;                                
            else                                    
                s = 1.0;                                
            ceres::CostFunction *cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s);
            problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
            plane_correspondence++;
        }
    }
}

在寻找到距离当前flat surface特征点 $p_{curr}$ 最近的三个less flat surface特征点 $p_a, p_b, p_c$ 后，即可建立由点到面距离构成的残差函数。原始LOAM开源代码中，Zhang Ji大佬同样是手写的Gauss-Newton算法，A-LOAM利用Ceres库简化了这一过程。不得不说Ceres、G2O等开源优化库给SLAM算法开发带来了极大的便利，否则反复推导各种Jacobian就要劝退很多人了。

LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_, Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_)
: curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_),          last_point_m(last_point_m_), s(s_){        
    ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);
    ljm_norm.normalize();            
}
    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> lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())};
        Eigen::Matrix<T, 3, 1> ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())};
        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;
        residual[0] = (lp - lpj).dot(ljm);
        return true;    
    }

简单介绍一下该误差函数的意义。给定空间中不共线的三个点A，B，C，则这三个点共面。向量 $\vec{AB},\vec{AC}$ 叉乘后得到的单位向量为面ABC的法向量。已知面的法向量后，即可利用向量点乘，求出点到面的距离。

其他

如下代码所示，A-LOAM复现的版本中，在计算激光里程计时共进行了两轮优化，每轮优化时，更新特征间对应关系。在每轮优化内，最多迭代4次。

ceres::Solver::Options options;                    
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 4;
options.minimizer_progress_to_stdout = false;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);

为避免博客篇幅过长，LiDAR Mapping部分见下一篇博客。