LeGO-LOAM中地图配准的代码

LeGO-LOAM中的地图配准主要是通过ICP算法实现的。下面是配准的代码：

void mapOptimization::scan2MapOptimization(){

    ...
    
    // Step 1: Transform current scan to global map
    PointCloudT::Ptr transformed_scan_ptr (new PointCloudT());
    pcl::transformPointCloud(*laserCloudCornerLast, *transformed_scan_ptr, laserOdometry);

    // Step 2: Add current scan to global map
    *laserCloudCornerFromMap += *transformed_scan_ptr;
    laserCloudCornerFromMapDS->clear();
    downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
    downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);

    // Step 3: Apply ICP algorithm to align current scan with global map
    pcl::IterativeClosestPoint<PointT, PointT> icp;
    icp.setInputSource(transformed_scan_ptr);
    icp.setInputTarget(laserCloudCornerFromMapDS);
    icp.setMaxCorrespondenceDistance(0.5);
    icp.setMaximumIterations(50);
    icp.setTransformationEpsilon(1e-8);
    icp.align(*transformed_scan_ptr);

    // Step 4: Transform current scan back to its local frame
    if (icp.hasConverged()) {
        laserOdometry = icp.getFinalTransformation().inverse() * laserOdometry;
        pcl::transformPointCloud(*transformed_scan_ptr, *laserCloudCornerLast, laserOdometry);
    }
    
    ...
    
}

在这个函数中，我们首先将当前的点云数据通过位姿转换，转换到全局地图的坐标系下。然后将当前点云添加到全局地图中。接着，我们使用ICP算法，将当前点云与全局地图进行匹配，得到它们之间的变换关系。最后，我们将当前点云通过变换关系转换回到它的本地坐标系下。

需要注意的是，在LeGO-LOAM中，我们是将当前点云的角点部分与全局地图的角点部分进行匹配，因为角点更具有特征性。另外，我们使用了一个体素滤波器对全局地图进行了下采样，这样可以加快匹配速度，同时也可以去除一些不必要的噪声点。