A-LOAM源码解读

文件目录

  • 代码
    • 1、kittiHelper.cpp
    • 2、scanRegistration.cpp
    • 3、laserOdometry.cpp
    • 4、laserMapping.cpp
    • 5、lidarFactor.cpp
  • 参考

代码

1、kittiHelper.cpp

/*
 * @Author: your name
 * @Date: 2020-09-18 09:28:16
 * @LastEditTime: 2020-10-05 21:29:26
 * @LastEditors: Please set LastEditors
 * @Description: 读取kitti文件,发布odomGT,pathGT
 * @FilePath: \A-LOAM-NOTED-devel\src\kittiHelper.cpp
 */

// Author:   Tong Qin               [email protected]
// 	         Shaozu Cao 		    [email protected]

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

std::vector<float> read_lidar_data(const std::string lidar_data_path)
{
     
    std::ifstream lidar_data_file(lidar_data_path, std::ifstream::in | std::ifstream::binary);
    lidar_data_file.seekg(0, std::ios::end);
    const size_t num_elements = lidar_data_file.tellg() / sizeof(float);
    lidar_data_file.seekg(0, std::ios::beg);

    std::vector<float> lidar_data_buffer(num_elements);
    lidar_data_file.read(reinterpret_cast<char*>(&lidar_data_buffer[0]), num_elements*sizeof(float));
    return lidar_data_buffer;
}

int main(int argc, char** argv)
{
     
    ros::init(argc, argv, "kitti_helper");
    ros::NodeHandle n("~");
    std::string dataset_folder, sequence_number, output_bag_file;
    n.getParam("dataset_folder", dataset_folder);               // 数据集文件夹
    n.getParam("sequence_number", sequence_number);             // 序列号
    std::cout << "Reading sequence " << sequence_number << " from " << dataset_folder << '\n';
    bool to_bag;
    n.getParam("to_bag", to_bag);                               // 是否转换为bag
    if (to_bag)
        n.getParam("output_bag_file", output_bag_file);
    int publish_delay;
    n.getParam("publish_delay", publish_delay);
    publish_delay = publish_delay <= 0 ? 1 : publish_delay;

    ros::Publisher pub_laser_cloud = n.advertise<sensor_msgs::PointCloud2>("/velodyne_points", 2);          // 发布点云

    // 发布图像
    image_transport::ImageTransport it(n);
    image_transport::Publisher pub_image_left = it.advertise("/image_left", 2); 
    image_transport::Publisher pub_image_right = it.advertise("/image_right", 2);

    // 发布odomGT
    ros::Publisher pubOdomGT = n.advertise<nav_msgs::Odometry> ("/odometry_gt", 5);
    nav_msgs::Odometry odomGT;         
    odomGT.header.frame_id = "/camera_init";            // 发布从camera_init到 ground_trurh的坐标系变换 已经转换为lidar的odom了
    odomGT.child_frame_id = "/ground_truth";

    // 发布pathGT
    ros::Publisher pubPathGT = n.advertise<nav_msgs::Path> ("/path_gt", 5);
    nav_msgs::Path pathGT;
    pathGT.header.frame_id = "/camera_init";

    // 读取时间戳
    std::string timestamp_path = "sequences/" + sequence_number + "/times.txt";
    std::ifstream timestamp_file(dataset_folder + timestamp_path, std::ifstream::in);

    // 读取groundtruth_path
    std::string ground_truth_path = "results/" + sequence_number + ".txt";
    std::ifstream ground_truth_file(dataset_folder + ground_truth_path, std::ifstream::in);

    // 是否导出未bag文件
    rosbag::Bag bag_out;
    if (to_bag)
        bag_out.open(output_bag_file, rosbag::bagmode::Write);
    
    // camera相对于lidar的旋转矩阵
    Eigen::Matrix3d R_transform;
    R_transform << 0, 0, 1, -1, 0, 0, 0, -1, 0;
    Eigen::Quaterniond q_transform(R_transform);

    std::string line;
    std::size_t line_num = 0;

    ros::Rate r(10.0 / publish_delay);
    while (std::getline(timestamp_file, line) && ros::ok())
    {
        
        float timestamp = stof(line);       // 时间戳

        // 读取图片路径
        std::stringstream left_image_path, right_image_path;
        left_image_path << dataset_folder << "sequences/" + sequence_number + "/image_0/" << std::setfill('0') << std::setw(6) << line_num << ".png";
        cv::Mat left_image = cv::imread(left_image_path.str(), CV_LOAD_IMAGE_GRAYSCALE);
        right_image_path << dataset_folder << "sequences/" + sequence_number + "/image_1/" << std::setfill('0') << std::setw(6) << line_num << ".png";
        cv::Mat right_image = cv::imread(left_image_path.str(), CV_LOAD_IMAGE_GRAYSCALE);

        // 读取ground_truth_path
        std::getline(ground_truth_file, line);
        std::stringstream pose_stream(line);
        std::string s;
        Eigen::Matrix<double, 3, 4> gt_pose;
        for (std::size_t i = 0; i < 3; ++i)
        {
     
            for (std::size_t j = 0; j < 4; ++j)
            {
     
                std::getline(pose_stream, s, ' ');
                gt_pose(i, j) = stof(s);
            }
        }

        Eigen::Quaterniond q_w_i(gt_pose.topLeftCorner<3, 3>());            // q_w_i代表camera的odom
//        Eigen::Quaterniond q = q_transform * q_w_i;
//        此处应该添加 * q_transform.inverse(),如下所示
        Eigen::Quaterniond q = q_transform * q_w_i *q_transform.inverse();          // camera-odom变为lidar的odom
        q.normalize();
        Eigen::Vector3d t = q_transform * gt_pose.topRightCorner<3, 1>();           // lidar-->camera的旋转矩阵*camera坐标系的平移


        // odomGT代表lidar的pose
        odomGT.header.stamp = ros::Time().fromSec(timestamp);
        odomGT.pose.pose.orientation.x = q.x();
        odomGT.pose.pose.orientation.y = q.y();
        odomGT.pose.pose.orientation.z = q.z();
        odomGT.pose.pose.orientation.w = q.w();
        odomGT.pose.pose.position.x = t(0);
        odomGT.pose.pose.position.y = t(1);
        odomGT.pose.pose.position.z = t(2);
        pubOdomGT.publish(odomGT);              // 发布odomGT


        // 发布lidar的pathGT
        geometry_msgs::PoseStamped poseGT;
        poseGT.header = odomGT.header;
        poseGT.pose = odomGT.pose.pose;
        pathGT.header.stamp = odomGT.header.stamp;
        pathGT.poses.push_back(poseGT);
        pubPathGT.publish(pathGT);              // 发布lidar的pathGT
        
        // 读取点云数据
        // read lidar point cloud
        std::stringstream lidar_data_path;
        lidar_data_path << dataset_folder << "velodyne/sequences/" + sequence_number + "/velodyne/" 
                        << std::setfill('0') << std::setw(6) << line_num << ".bin";         // 设置前面5个0
        std::vector<float> lidar_data = read_lidar_data(lidar_data_path.str());
        std::cout << "totally " << lidar_data.size() / 4.0 << " points in this lidar frame \n";

        std::vector<Eigen::Vector3d> lidar_points;
        std::vector<float> lidar_intensities;
        pcl::PointCloud<pcl::PointXYZI> laser_cloud;

        // 读取当前帧的点云
        for (std::size_t i = 0; i < lidar_data.size(); i += 4)
        {
     
            lidar_points.emplace_back(lidar_data[i], lidar_data[i+1], lidar_data[i+2]);
            lidar_intensities.push_back(lidar_data[i+3]);

            pcl::PointXYZI point;
            point.x = lidar_data[i];
            point.y = lidar_data[i + 1];
            point.z = lidar_data[i + 2];
            point.intensity = lidar_data[i + 3];
            laser_cloud.push_back(point);
        }

        sensor_msgs::PointCloud2 laser_cloud_msg;
        pcl::toROSMsg(laser_cloud, laser_cloud_msg);
        laser_cloud_msg.header.stamp = ros::Time().fromSec(timestamp);
        laser_cloud_msg.header.frame_id = "/camera_init";
        pub_laser_cloud.publish(laser_cloud_msg);           //  发布点云信息

        sensor_msgs::ImagePtr image_left_msg = cv_bridge::CvImage(laser_cloud_msg.header, "mono8", left_image).toImageMsg();
        sensor_msgs::ImagePtr image_right_msg = cv_bridge::CvImage(laser_cloud_msg.header, "mono8", right_image).toImageMsg();
        pub_image_left.publish(image_left_msg);             // 发布图像信息
        pub_image_right.publish(image_right_msg);           // 发布图像信息

        // 保存为bag文件
        if (to_bag)
        {
     
            bag_out.write("/image_left", ros::Time::now(), image_left_msg);
            bag_out.write("/image_right", ros::Time::now(), image_right_msg);
            bag_out.write("/velodyne_points", ros::Time::now(), laser_cloud_msg);
            bag_out.write("/path_gt", ros::Time::now(), pathGT);
            bag_out.write("/odometry_gt", ros::Time::now(), odomGT);
        }

        line_num ++;
        r.sleep();
    }
    bag_out.close();
    std::cout << "Done \n";


    return 0;
}

2、scanRegistration.cpp

scanRegistration的功能就是提取特征点。

  • comp函数,比较两个点的曲率
bool comp (int i,int j) {
      return (cloudCurvature[i]<cloudCurvature[j]); }
  • removeClosedPointCloud函数,丢弃一定距离以内的点
// 丢弃一定距离以内的点
template <typename PointT>
void removeClosedPointCloud(const pcl::PointCloud<PointT> &cloud_in,
                              pcl::PointCloud<PointT> &cloud_out, float thres)
{
     
    if (&cloud_in != &cloud_out)
    {
     
        cloud_out.header = cloud_in.header;
        cloud_out.points.resize(cloud_in.points.size());
    }

    size_t j = 0;

    for (size_t i = 0; i < cloud_in.points.size(); ++i)
    {
     
        if (cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y + cloud_in.points[i].z * cloud_in.points[i].z < thres * thres)
            continue;
        cloud_out.points[j] = cloud_in.points[i];
        j++;
    }
    if (j != cloud_in.points.size())
    {
     
        cloud_out.points.resize(j);
    }

    cloud_out.height = 1;
    cloud_out.width = static_cast<uint32_t>(j);
    cloud_out.is_dense = true;
}
  • laserCloudHandler函数

这个函数主要就是对接收到的点云数据进行处理,最终发布关键点(laser_cloud_sharp、laser_cloud_less_sharp、laser_cloud_flat、laser_cloud_less_flat),以方便后续使用。

1、首先计算当前帧的第一个点和最后一个点的角度


    // 计算起始点和终止点角度
    // 解释一下:atan2的范围是[-180,180],atan的范围是[-90,90]
    int cloudSize = laserCloudIn.points.size();
    float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);        // 第一个扫描点的角度
    float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,                         // 最后一个扫描点的角度
                          laserCloudIn.points[cloudSize - 1].x) +
                   2 * M_PI;

    // 最后一个点的角度 - 第一个点的角度>PI
    if (endOri - startOri > 3 * M_PI)
    {
     
        endOri -= 2 * M_PI;
    }
    else if (endOri - startOri < M_PI)
    {
     
        endOri += 2 * M_PI;
    }
    printf("[scanRegistration] start Ori %f\n",startOri);
    printf("[scanRegistration] end Ori %f\n", endOri);

2、对每一个点计算其scanID和该点对应的扫描到的时间,将结果放在intensity中。

    // 对每一个点计算scanID和扫描的点的时间
    bool halfPassed = false;
    int count = cloudSize;          // 所有的点数
    PointType point;
    std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);       // 每一行扫描视为一个点云
    for (int i = 0; i < cloudSize; i++)
    {
     
        point.x = laserCloudIn.points[i].x;
        point.y = laserCloudIn.points[i].y;
        point.z = laserCloudIn.points[i].z;

        // 根据激光扫描线的俯仰角获取scanid[0,15]
        float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;         // 俯仰角[-90,90]
        int scanID = 0;

        if (N_SCANS == 16)
        {
     
            scanID = int((angle + 15) / 2 + 0.5);
            if (scanID > (N_SCANS - 1) || scanID < 0)
            {
     
                count--;
                continue;
            }
        }
        else if (N_SCANS == 32)
        {
     
            scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);
            if (scanID > (N_SCANS - 1) || scanID < 0)
            {
     
                count--;
                continue;
            }
        }
        else if (N_SCANS == 64)
        {
        
            if (angle >= -8.83)
                scanID = int((2 - angle) * 3.0 + 0.5);
            else
                scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);

            // use [0 50]  > 50 remove outlies 
            if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
            {
     
                count--;
                continue;
            }
        }
        else
        {
     
            printf("[scanRegistration] wrong scan number\n");
            ROS_BREAK();
        }
        //printf("angle %f scanID %d \n", angle, scanID);

        // 获取每一个点对应的角度
        float ori = -atan2(point.y, point.x);           // 每一个点的水平角度
        if (!halfPassed)                                // 判断当前点过没过一半(开始的时候是没过一半的)
        {
      
            if (ori < startOri - M_PI / 2)
            {
     
                ori += 2 * M_PI;
            }
            else if (ori > startOri + M_PI * 3 / 2)
            {
     
                ori -= 2 * M_PI;
            }

            if (ori - startOri > M_PI)
            {
     
                halfPassed = true;
            }
        }
        else            // 后半段时间
        {
     
            ori += 2 * M_PI;
            if (ori < endOri - M_PI * 3 / 2)
            {
     
                ori += 2 * M_PI;
            }
            else if (ori > endOri + M_PI / 2)
            {
     
                ori -= 2 * M_PI;
            }
        }

        float relTime = (ori - startOri) / (endOri - startOri);     // 时间占比
        point.intensity = scanID + scanPeriod * relTime;            // intensity=scanID+点对应的在线上的时间
        laserCloudScans[scanID].push_back(point);                   // 把当前点放在对应的scanID对应的线上
    }

3、计算16条scan的起始点的索引和终止点的索引

    // 记录16个scan的每一个的startInd,endInd
    pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
    for (int i = 0; i < N_SCANS; i++)
    {
      
        scanStartInd[i] = laserCloud->size() + 5;
        *laserCloud += laserCloudScans[i];
        scanEndInd[i] = laserCloud->size() - 6; 
    }

4、对每一个点计算曲率


    // 有序地计算曲率
    for (int i = 5; i < cloudSize - 5; i++)     // 忽略前后的5个点
    {
      
        float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;
        float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;
        float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;

        cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
        cloudSortInd[i] = i;
        cloudNeighborPicked[i] = 0;             // 记录这个点是否被选中了
        cloudLabel[i] = 0;// Label 2: corner_sharp
                          // Label 1: corner_less_sharp, ????Label 2
                          // Label -1: surf_flat
                          // Label 0: surf_less_flat?? ????Label -1??????????????????
    }

5、对每一个scan分为6大块,在每一个块中,分别根据曲率提取角点(2个)和平面点(20个)

    pcl::PointCloud<PointType> cornerPointsSharp;
    pcl::PointCloud<PointType> cornerPointsLessSharp;
    pcl::PointCloud<PointType> surfPointsFlat;
    pcl::PointCloud<PointType> surfPointsLessFlat;

    // 提取特征点
    float t_q_sort = 0;
    for (int i = 0; i < N_SCANS; i++)// 0-15
    {
     
        if( scanEndInd[i] - scanStartInd[i] < 6)// 每一个scan的start和end
            continue;
        pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
        for (int j = 0; j < 6; j++)// 把每一个scan均分为6份,0-5
        {
     
            int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;// subscan?????index 
            int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;// subscan?????index

            TicToc t_tmp;
            std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);// 对这6份中的每一份按照曲率小到大排序
            t_q_sort += t_tmp.toc();

            // 提取角点
            int largestPickedNum = 0;
            for (int k = ep; k >= sp; k--)// 在subscan中 倒序查找
            {
     
                int ind = cloudSortInd[k]; 

                if (cloudNeighborPicked[ind] == 0 &&
                    cloudCurvature[ind] > 0.1)// ????????б??????????????????0.1
                {
     
                    largestPickedNum++;
                    if (largestPickedNum <= 2)// 一个subscan中最多有 2个corner_sharp
                    {
                             
                        cloudLabel[ind] = 2;   //corner_sharp
                        cornerPointsSharp.push_back(laserCloud->points[ind]);
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    else if (largestPickedNum <= 20)// 一个subscan中最多有20个corner_less_sharp,corner_less_sharp包括corner_sharp
                    {
                             
                        cloudLabel[ind] = 1;            // corner_less_sharp
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    else    // corner_less_sharp>20了
                    {
     
                        break;
                    }

                    cloudNeighborPicked[ind] = 1;       // 该点已经被处理过了,记为1

                    // 与临近点的距离的平方 <= 0.05的点标记为选择过,避免特征点密集分布
                    for (int l = 1; l <= 5; l++)
                    {
     
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)       // 如果相邻两根scan的距离大于0.05,那么退出
                        {
     
                            break;
                        }

                        cloudNeighborPicked[ind + l] = 1;
                    }

                    // 与临近点的距离的平方 <= 0.05的点标记为选择过,避免特征点密集分布,越往上scanID越大越远,所以可以直接break
                    for (int l = -1; l >= -5; l--)
                    {
     
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                        {
     
                            break;
                        }

                        cloudNeighborPicked[ind + l] = 1;
                    }
                }
            }

            // 提取平面点
            // 提取surf平面feature,与上述类似,选取该subscan曲率最小的前4个点为surf_flat
            int smallestPickedNum = 0;
            for (int k = sp; k <= ep; k++)      // 寻找平的,曲率由小到大排序
            {
     
                int ind = cloudSortInd[k];

                // 判断是否满足平面点的条件
                if (cloudNeighborPicked[ind] == 0 &&
                    cloudCurvature[ind] < 0.1)
                {
     

                    cloudLabel[ind] = -1; 
                    surfPointsFlat.push_back(laserCloud->points[ind]);

                    smallestPickedNum++;
                    if (smallestPickedNum >= 4)
                    {
      
                        break;
                    }

                    cloudNeighborPicked[ind] = 1;

                    // 根据scanID往上查找,防止关键点密集
                    for (int l = 1; l <= 5; l++)
                    {
      
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                        {
     
                            break;
                        }

                        cloudNeighborPicked[ind + l] = 1;
                    }
                    for (int l = -1; l >= -5; l--)
                    {
     
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                        {
     
                            break;
                        }

                        cloudNeighborPicked[ind + l] = 1;
                    }
                }
            }

            
            // 对剩下的点提取为lessFlat
            for (int k = sp; k <= ep; k++)
            {
     
                if (cloudLabel[k] <= 0)
                {
     
                    surfPointsLessFlatScan->push_back(laserCloud->points[k]);
                }
            }
        }

        // 对lessFlat进行降采样
        pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
        pcl::VoxelGrid<PointType> downSizeFilter;
        downSizeFilter.setInputCloud(surfPointsLessFlatScan);
        downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
        downSizeFilter.filter(surfPointsLessFlatScanDS);

        surfPointsLessFlat += surfPointsLessFlatScanDS;
    }

3、laserOdometry.cpp

laserOdometry的作用就是对相邻帧的两帧点云做帧间匹配得到位姿的变换

1、当前帧的在world世界坐标系下的位姿

// Lidar Odometry线程估计的frame在world坐标系的位姿P,Transformation from current frame to world frame
Eigen::Quaterniond q_w_curr(1, 0, 0, 0);
Eigen::Vector3d t_w_curr(0, 0, 0);

2、待优化变量

// 点云特征匹配时的优化变量
double para_q[4] = {
     0, 0, 0, 1};
double para_t[3] = {
     0, 0, 0};

// 下面的2个分别是优化变量para_q和para_t的映射:表示的是两个world坐标系下的位姿P之间的增量,例如△P = P0.inverse() * P1
Eigen::Map<Eigen::Quaterniond> q_last_curr(para_q);         //  相当于引用
Eigen::Map<Eigen::Vector3d> t_last_curr(para_t);

3、将当前帧点云转换到上一帧,根据运动模型对点云去畸变

TransformToStart:将当前帧Lidar坐标系下的点云变换到上一帧Lidar坐标系下(也就是当前帧的初始位姿,起始位姿,所以函数名是TransformToStart),因为kitti点云已经去除了畸变,所以不再考虑运动补偿。(如果点云没有去除畸变,用slerp差值的方式计算出每个点在fire时刻的位姿,然后进行TransformToStart的坐标变换,一方面实现了变换到上一帧Lidar坐标系下;另一方面也可以理解成点都将fire时刻统一到了开始时刻,即去除了畸变,完成了运动补偿)

// undistort lidar point
// 将当前帧的点云转为上一帧,其中根据运动模型对点云进行了去畸变 
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;
}

4、接收上游的5个topic函数,将消息放在queue中,方便后续处理

// 存放数据的queue
std::queue<sensor_msgs::PointCloud2ConstPtr> cornerSharpBuf;        // 只能访问 queue 容器适配器的第一个和最后一个元素。只能在容器的末尾添加新元素,只能从头部移除元素。
std::queue<sensor_msgs::PointCloud2ConstPtr> cornerLessSharpBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> surfFlatBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> surfLessFlatBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> fullPointsBuf;

//...

// 之后的5个Handler函数为接受上游5个topic的回调函数,作用是将消息缓存到对应的queue中,以便后续处理。
void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsSharp2)
{
     
    mBuf.lock();
    cornerSharpBuf.push(cornerPointsSharp2);
    mBuf.unlock();
}

void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsLessSharp2)
{
     
    mBuf.lock();
    cornerLessSharpBuf.push(cornerPointsLessSharp2);
    mBuf.unlock();
}

void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsFlat2)
{
     
    mBuf.lock();
    surfFlatBuf.push(surfPointsFlat2);
    mBuf.unlock();
}

void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsLessFlat2)
{
     
    mBuf.lock();
    surfLessFlatBuf.push(surfPointsLessFlat2);
    mBuf.unlock();
}

//receive all point cloud
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2)
{
     
    mBuf.lock();
    fullPointsBuf.push(laserCloudFullRes2);
    mBuf.unlock();
}

5、main函数中的发布者


    ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 100);

    ros::Publisher pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 100);

    ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 100);

    ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry>("/laser_odom_to_init", 100);

    ros::Publisher pubLaserPath = nh.advertise<nav_msgs::Path>("/laser_odom_path", 100);

5、消息同步机制

        ros::spinOnce();

        if (!cornerSharpBuf.empty() && !cornerLessSharpBuf.empty() &&
            !surfFlatBuf.empty() && !surfLessFlatBuf.empty() &&
            !fullPointsBuf.empty())
        {
     
            timeCornerPointsSharp = cornerSharpBuf.front()->header.stamp.toSec();
            timeCornerPointsLessSharp = cornerLessSharpBuf.front()->header.stamp.toSec();
            timeSurfPointsFlat = surfFlatBuf.front()->header.stamp.toSec();
            timeSurfPointsLessFlat = surfLessFlatBuf.front()->header.stamp.toSec();
            timeLaserCloudFullRes = fullPointsBuf.front()->header.stamp.toSec();

            // 如果关键点的时间戳和完整数据的时间戳有一个不相等,那么报错
            if (timeCornerPointsSharp != timeLaserCloudFullRes ||
                timeCornerPointsLessSharp != timeLaserCloudFullRes ||
                timeSurfPointsFlat != timeLaserCloudFullRes ||
                timeSurfPointsLessFlat != timeLaserCloudFullRes)
            {
     
                printf("unsync messeage!");
                ROS_BREAK();
            }

            // 5个点云的时间同步
            mBuf.lock();
            cornerPointsSharp->clear();
            pcl::fromROSMsg(*cornerSharpBuf.front(), *cornerPointsSharp);
            cornerSharpBuf.pop();

            cornerPointsLessSharp->clear();
            pcl::fromROSMsg(*cornerLessSharpBuf.front(), *cornerPointsLessSharp);
            cornerLessSharpBuf.pop();

            surfPointsFlat->clear();
            pcl::fromROSMsg(*surfFlatBuf.front(), *surfPointsFlat);
            surfFlatBuf.pop();

            surfPointsLessFlat->clear();
            pcl::fromROSMsg(*surfLessFlatBuf.front(), *surfPointsLessFlat);
            surfLessFlatBuf.pop();

            laserCloudFullRes->clear();
            pcl::fromROSMsg(*fullPointsBuf.front(), *laserCloudFullRes);
            fullPointsBuf.pop();
            mBuf.unlock();

6、帧间匹配优化机制

            // initializing
            if (!systemInited)// 第一帧不进行匹配,仅仅将 cornerPointsLessSharp 保存至 laserCloudCornerLast
                              //                       将 surfPointsLessFlat    保存至 laserCloudSurfLast
                              // 为下次匹配提供target
            {
     
                systemInited = true;
                std::cout << "Initialization finished \n";
            }
            else              // 第二帧开始
            {
     
                int cornerPointsSharpNum = cornerPointsSharp->points.size();
                int surfPointsFlatNum = surfPointsFlat->points.size();

                TicToc t_opt;
                for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)// 点到线以及点到面的ICP,迭代2次
                {
     
                    corner_correspondence = 0;          // 角点的误差项数量
                    plane_correspondence = 0;           // 平面点的误差项数量

                    //ceres::LossFunction *loss_function = NULL;
                    ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
                    ceres::LocalParameterization *q_parameterization =
                        new ceres::EigenQuaternionParameterization();
                    ceres::Problem::Options problem_options;

                    ceres::Problem problem(problem_options);
                    problem.AddParameterBlock(para_q, 4, q_parameterization);
                    problem.AddParameterBlock(para_t, 3);

                    pcl::PointXYZI pointSel;
                    std::vector<int> pointSearchInd;
                    std::vector<float> pointSearchSqDis;

                    TicToc t_data;
                    // 基于最近邻原理建立corner特征点之间关联,find correspondence for corner features
                    for (int i = 0; i < cornerPointsSharpNum; ++i)
                    {
     
                        TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);// 将当前帧的corner_sharp特征点O_cur,从当前帧的Lidar坐标系下变换到上一帧的Lidar坐标系下(记为点O,注意与前面的点O_cur不同),以利于寻找corner特征点的correspondence
                        kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);// kdtree中的点云是上一帧的corner_less_sharp,所以这是在上一帧
                                                                                                        // 的corner_less_sharp中寻找当前帧corner_sharp特征点O的最近邻点(记为A)

                        int closestPointInd = -1, minPointInd2 = -1;
                        if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)// 如果最近邻的corner特征点之间距离平方小于阈值,则最近邻点A有效
                        {
     
                            closestPointInd = pointSearchInd[0];
                            int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);  // 最近点的scanID,第几条线

                            double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;
                            // 寻找点O的另外一个最近邻的点(记为点B) in the direction of increasing scan line
                            // 顺着index增加的方向查找
                            // 遍历范围内的点,选择距离pointSel最近的点(除掉kd-tree直接检索到的最近点之外)
                            for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j)// laserCloudCornerLast 来自上一帧的corner_less_sharp特征点,由于提取特征时是
                            {
                                                                                        // 按照scan的顺序提取的,所以laserCloudCornerLast中的点也是按照scanID递增的顺序存放的
                                // if in the same scan line, continue
                                // 如果查找到的scanID在closestPointScanID下面,直接跳过
                                if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)// intensity整数部分存放的是scanID
                                    continue;
                                
                                // 第二近的点距离closestPointScanID也不能太远
                                // 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);

                                if (pointSqDis < minPointSqDis2)// 第二个最近邻点有效,,更新点B
                                {
     
                                    // find nearer point
                                    minPointSqDis2 = pointSqDis;
                                    minPointInd2 = j;
                                }
                            }

                            // 寻找点O的另外一个最近邻的点B 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);

                                if (pointSqDis < minPointSqDis2)// 第二个最近邻点有效,更新点B
                                {
     
                                    // find nearer point
                                    minPointSqDis2 = pointSqDis;
                                    minPointInd2 = j;
                                }
                            }
                        }

                        // 构造误差项
                        if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid
                        {
                           // 即特征点O的两个最近邻点A和B都有效
                            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;// 运动补偿系数,kitti数据集的点云已经被补偿过,所以s = 1.0
                            if (DISTORTION)
                                s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD;
                            else
                                s = 1.0;
                            // 用点O,A,B构造点到线的距离的残差项,注意这三个点都是在上一帧的Lidar坐标系下,即,残差 = 点O到直线AB的距离
                            // 具体到介绍lidarFactor.cpp时再说明该残差的具体计算方法
                            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++;
                        }
                    }

                    // 下面说的点符号与上述相同
                    // 与上面的建立corner特征点之间的关联类似,寻找平面特征点O的最近邻点ABC,即基于最近邻原理建立surf特征点之间的关联,find correspondence for plane features
                    for (int i = 0; i < surfPointsFlatNum; ++i)
                    {
     
                        TransformToStart(&(surfPointsFlat->points[i]), &pointSel);
                        kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);

                        int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
                        if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)// 找到的最近邻点A有效
                        {
     
                            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;// 找到的第2个最近邻点有效,更新点B,注意如果scanID准确的话,一般点A和点B的scanID相同
                                    minPointInd2 = j;
                                }
                                // if in the higher scan line
                                else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
                                {
     
                                    minPointSqDis3 = pointSqDis;// 找到的第3个最近邻点有效,更新点C,注意如果scanID准确的话,一般点A和点B的scanID相同,且与点C的scanID不同,与LOAM的paper叙述一致
                                    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;
                            // 用点O,A,B,C构造点到面的距离的残差项,注意这三个点都是在上一帧的Lidar坐标系下,即,残差 = 点O到平面ABC的距离
                            // 同样的,具体到介绍lidarFactor.cpp时再说明该残差的具体计算方法
                            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++;
                        
                        }
                    }

                    printf("data association time %f ms \n", t_data.toc());

                    if ((corner_correspondence + plane_correspondence) < 10)
                    {
     
                        printf("less correspondence! *************************************************\n");
                    }

                    TicToc t_solver;
                    ceres::Solver::Options options;
                    options.linear_solver_type = ceres::DENSE_QR;
                    options.max_num_iterations = 4;
                    options.minimizer_progress_to_stdout = false;           // 输出到cout
                    ceres::Solver::Summary summary;
                    // 基于构建的所有残差项,求解最优的当前帧位姿与上一帧位姿的位姿增量:para_q和para_t
                    ceres::Solve(options, &problem, &summary);
                    printf("solver time %f ms \n", t_solver.toc());
                }
                printf("optimization twice time %f \n", t_opt.toc());

                // 用最新计算出的位姿增量,更新上一帧的位姿,得到当前帧的位姿,注意这里说的位姿都指的是世界坐标系下的位姿
                t_w_curr = t_w_curr + q_w_curr * t_last_curr;
                q_w_curr = q_w_curr * q_last_curr;
            }

7、发布odom和path

            // publish odometry
            nav_msgs::Odometry laserOdometry;
            laserOdometry.header.frame_id = "/camera_init";
            laserOdometry.child_frame_id = "/laser_odom";
            laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
            laserOdometry.pose.pose.orientation.x = q_w_curr.x();
            laserOdometry.pose.pose.orientation.y = q_w_curr.y();
            laserOdometry.pose.pose.orientation.z = q_w_curr.z();
            laserOdometry.pose.pose.orientation.w = q_w_curr.w();
            laserOdometry.pose.pose.position.x = t_w_curr.x();
            laserOdometry.pose.pose.position.y = t_w_curr.y();
            laserOdometry.pose.pose.position.z = t_w_curr.z();
            pubLaserOdometry.publish(laserOdometry);

            geometry_msgs::PoseStamped laserPose;
            laserPose.header = laserOdometry.header;
            laserPose.pose = laserOdometry.pose.pose;
            laserPath.header.stamp = laserOdometry.header.stamp;
            laserPath.poses.push_back(laserPose);
            laserPath.header.frame_id = "/camera_init";
            pubLaserPath.publish(laserPath);

8、更新配准的source

            // 当前帧变上一帧
            pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
            cornerPointsLessSharp = laserCloudCornerLast;
            laserCloudCornerLast = laserCloudTemp;

            laserCloudTemp = surfPointsLessFlat;
            surfPointsLessFlat = laserCloudSurfLast;
            laserCloudSurfLast = laserCloudTemp;

            laserCloudCornerLastNum = laserCloudCornerLast->points.size();
            laserCloudSurfLastNum = laserCloudSurfLast->points.size();

            // std::cout << "the size of corner last is " << laserCloudCornerLastNum << ", and the size of surf last is " << laserCloudSurfLastNum << '\n';

            // 使用上一帧的点云更新kd-tree,如果是第一帧的话是直接将其保存为这个的
            kdtreeCornerLast->setInputCloud(laserCloudCornerLast);// 更新kdtree的点云
            kdtreeSurfLast->setInputCloud(laserCloudSurfLast);

9、每5帧执行一次发布

            // 每隔5帧执行一次
            if (frameCount % skipFrameNum == 0)
            {
     
                frameCount = 0;

                // 发布上一帧的Corner点
                sensor_msgs::PointCloud2 laserCloudCornerLast2;
                pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
                laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
                laserCloudCornerLast2.header.frame_id = "/camera";
                pubLaserCloudCornerLast.publish(laserCloudCornerLast2);         

                // 发布上一帧的Surf点
                sensor_msgs::PointCloud2 laserCloudSurfLast2;
                pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
                laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
                laserCloudSurfLast2.header.frame_id = "/camera";
                pubLaserCloudSurfLast.publish(laserCloudSurfLast2);

                // 发布全部完整点云
                sensor_msgs::PointCloud2 laserCloudFullRes3;
                pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
                laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
                laserCloudFullRes3.header.frame_id = "/camera";
                pubLaserCloudFullRes.publish(laserCloudFullRes3);
            }

4、laserMapping.cpp

0、submap的维护

参考:LOAM笔记及A-LOAM源码阅读

1、优化的变量,是当前帧在世界坐标系下的pose

// 点云特征匹配时的优化变量
double parameters[7] = {
     0, 0, 0, 1, 0, 0, 0};

// Mapping线程估计的frame在world坐标系的位姿P,因为Mapping的算法耗时很有可能会超过100ms,所以
// 这个位姿P不是实时的,LOAM最终输出的实时位姿P_realtime,需要Mapping线程计算的相对低频位姿和
// Odometry线程计算的相对高频位姿做整合,详见后面laserOdometryHandler函数分析。此外需要注意
// 的是,不同于Odometry线程,这里的位姿P,即q_w_curr和t_w_curr,本身就是匹配时的优化变量。

// mapping线程计算的scan-to-localmap匹配后得到的位姿
Eigen::Map<Eigen::Quaterniond> q_w_curr(parameters);			//  map计算后的在world下的pose
Eigen::Map<Eigen::Vector3d> t_w_curr(parameters + 4);

2、mapping线程到Odometry线程的pose变换,Odometry线程计算得到的当前帧在world坐标系下的pose

// 下面的两个变量是world坐标系下的Odometry计算的位姿和Mapping计算的位姿之间的增量(也即变换,transformation)
// wmap_odom * wodom_curr = wmap_curr(即前面的q/t_w_curr)
// transformation between odom's world and map's world frame
Eigen::Quaterniond q_wmap_wodom(1, 0, 0, 0);					// map到odom的变换
Eigen::Vector3d t_wmap_wodom(0, 0, 0);

// Odometry线程计算的frame在world坐标系的位姿
Eigen::Quaterniond q_wodom_curr(1, 0, 0, 0);					// odom计算的在world坐标系下的pose
Eigen::Vector3d t_wodom_curr(0, 0, 0);

3、得到当前帧的在world世界坐标系下的pose

// set initial guess,上一帧的Transform(增量)wmap_wodom * 本帧Odometry位姿wodom_curr,旨在为本帧Mapping位姿w_curr设置一个初始值
void transformAssociateToMap()									// 获取Mapping的初值
{
     
	q_w_curr = q_wmap_wodom * q_wodom_curr;		
	t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
}

4、利用mapping计算得到的pose,计算mapping线程和Odometry线程之间的pose变换

// 用在最后,当Mapping的位姿w_curr计算完毕后,更新增量wmap_wodom,旨在为下一次执行transformAssociateToMap函数时做准备
void transformUpdate()											// 获取map到odom的变换
{
     
	q_wmap_wodom = q_w_curr * q_wodom_curr.inverse();
	t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr;
}

5、用mapping的位姿将Lidar坐标系下的点转换到world坐标系下

// 用Mapping的位姿w_curr,将Lidar坐标系下的点变换到world坐标系下.q_w_curr为优化量,代表lidar在世界坐标系中的pose
void pointAssociateToMap(PointType const *const pi, PointType *const po)
{
     
	Eigen::Vector3d point_curr(pi->x, pi->y, pi->z);
	Eigen::Vector3d point_w = q_w_curr * point_curr + t_w_curr;
	po->x = point_w.x();
	po->y = point_w.y();
	po->z = point_w.z();
	po->intensity = pi->intensity;
	//po->intensity = 1.0;
}

6、Odometry的回调函数

// receive odomtry
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &laserOdometry)
{
     
	mBuf.lock();
	odometryBuf.push(laserOdometry);
	mBuf.unlock();

	// high frequence publish
	Eigen::Quaterniond q_wodom_curr;
	Eigen::Vector3d t_wodom_curr;
	q_wodom_curr.x() = laserOdometry->pose.pose.orientation.x;
	q_wodom_curr.y() = laserOdometry->pose.pose.orientation.y;
	q_wodom_curr.z() = laserOdometry->pose.pose.orientation.z;
	q_wodom_curr.w() = laserOdometry->pose.pose.orientation.w;
	t_wodom_curr.x() = laserOdometry->pose.pose.position.x;
	t_wodom_curr.y() = laserOdometry->pose.pose.position.y;
	t_wodom_curr.z() = laserOdometry->pose.pose.position.z;

	// 为了保证LOAM整体的实时性,防止Mapping线程耗时>100ms导致丢帧,用上一次的增量wmap_wodom来更新
	// Odometry的位姿,旨在用Mapping位姿的初始值(也可以理解为预测值)来实时输出,进而实现LOAM整体的实时性
	// 这里为啥不直接用transformAssociateToMap()函数来获取mapping的初始值?
	Eigen::Quaterniond q_w_curr = q_wmap_wodom * q_wodom_curr;
	Eigen::Vector3d t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom; 

	nav_msgs::Odometry odomAftMapped;
	odomAftMapped.header.frame_id = "/camera_init";
	odomAftMapped.child_frame_id = "/aft_mapped";
	odomAftMapped.header.stamp = laserOdometry->header.stamp;
	odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
	odomAftMapped.pose.pose.orientation.y = q_w_curr.y();
	odomAftMapped.pose.pose.orientation.z = q_w_curr.z();
	odomAftMapped.pose.pose.orientation.w = q_w_curr.w();
	odomAftMapped.pose.pose.position.x = t_w_curr.x();
	odomAftMapped.pose.pose.position.y = t_w_curr.y();
	odomAftMapped.pose.pose.position.z = t_w_curr.z();
	pubOdomAftMappedHighFrec.publish(odomAftMapped);
}

7、process函数

  • 先读取Odometry线程的pose
			// odom线程得到的pose
			q_wodom_curr.x() = odometryBuf.front()->pose.pose.orientation.x;
			q_wodom_curr.y() = odometryBuf.front()->pose.pose.orientation.y;
			q_wodom_curr.z() = odometryBuf.front()->pose.pose.orientation.z;
			q_wodom_curr.w() = odometryBuf.front()->pose.pose.orientation.w;
			t_wodom_curr.x() = odometryBuf.front()->pose.pose.position.x;
			t_wodom_curr.y() = odometryBuf.front()->pose.pose.position.y;
			t_wodom_curr.z() = odometryBuf.front()->pose.pose.position.z;
			odometryBuf.pop();
  • 更新当前帧在mapping线程中的pose
			// 上一帧的增量wmap_wodom * 本帧Odometry位姿wodom_curr,旨在为本帧Mapping位姿w_curr设置一个初始值
			transformAssociateToMap();			 // 第一次运行时,wmap_wodom=E
  • 地图维护
			// 地图维护
			TicToc t_shift;
			// 下面这是计算当前帧位置t_w_curr(在上图中用红色五角星表示的位置)IJK坐标(见上图中的坐标轴),
			// 参照LOAM_NOTED的注释,下面有关25呀,50啥的运算,等效于以50m为单位进行缩放,因为LOAM用1维数组
			// 进行cube的管理,而数组的index只用是正数,所以要保证IJK坐标都是正数,所以加了laserCloudCenWidth/Heigh/Depth
			// 的偏移,来使得当前位置尽量位于submap的中心处,也就是使得上图中的五角星位置尽量处于所有格子的中心附近,
			// 偏移laserCloudCenWidth/Heigh/Depth会动态调整,来保证当前位置尽量位于submap的中心处。

			// 当前帧在world中的IJK坐标
			int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth;		// 加上偏移
			int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight;
			int centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth;

			// 由于计算机求余是向零取整,为了不使(-50.0,50.0)求余后都向零偏移,当被求余数为负数时求余结果统一向左偏移一个单位,也即减一
			if (t_w_curr.x() + 25.0 < 0)
				centerCubeI--;
			if (t_w_curr.y() + 25.0 < 0)
				centerCubeJ--;
			if (t_w_curr.z() + 25.0 < 0)
				centerCubeK--;

			// 输出偏移量
			printf("new----laserCloudCenWidth:%d,laserCloudCenHeight:%d,laserCloudCenDepth:%d",laserCloudCenWidth,laserCloudCenHeight,laserCloudCenDepth);
			printf("I:%f, J:%f, K:%f",centerCubeI,centerCubeJ,centerCubeK);

			// 以下注释部分参照LOAM_NOTED,结合我画的submap的示意图说明下面的6个while loop的作用:要
			// 注意世界坐标系下的点云地图是固定的,但是IJK坐标系我们是可以移动的,所以这6个while loop
			// 的作用就是调整IJK坐标系(也就是调整所有cube位置),使得五角星在IJK坐标系的坐标范围处于
		    // 3 <= centerCubeI < 18, 3 < centerCubeJ < 8, 3 < centerCubeK < 18,目的是为了防止后续向
			// 四周拓展cube(图中的黄色cube就是拓展的cube)时,index(即IJK坐标)成为负数。
			while (centerCubeI < 3)			// 以i轴为例,如果当前帧的(五角星)坐标小于3,那么要将其向I轴的正方向移动,
			{
     
				for (int j = 0; j < laserCloudHeight; j++)
				{
     
					for (int k = 0; k < laserCloudDepth; k++)
					{
      
						int i = laserCloudWidth - 1;			// 先把最大的i的对应的点云取出来
						pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
							laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k]; 
						pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
							laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
						for (; i >= 1; i--)// 在I方向上,将cube[I] = cube[I-1],最后一个空出来的cube清空点云,实现IJK坐标系向I轴负方向移动一个cube的
										   // 效果,从相对运动的角度看,就是图中的五角星在IJK坐标系下向I轴正方向移动了一个cube,如下面的动图所示,所
				                           // 以centerCubeI最后++,laserCloudCenWidth也会++,为下一帧Mapping时计算五角星的IJK坐标做准备。
						{
     
							laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =	
								laserCloudCornerArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];	// i处的点云被i-1处的点云替代
							laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
								laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
						}
						laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
							laserCloudCubeCornerPointer;		// 把之前最大的i对应的点云赋值给最小的i
						laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
							laserCloudCubeSurfPointer;
						laserCloudCubeCornerPointer->clear();
						laserCloudCubeSurfPointer->clear();
					}
				}

				centerCubeI++;

				// 这里为啥要对laserCloudCenWidth加一行?
				// 答:不是加一行,因为laserCloudCenWidth代表的当前帧坐标在IJK坐标系下的偏移量,而不是submap的尺寸,这个偏移量是可变的,目的就是为了使得submap位于中心,submap的尺寸是固定的5*5*5
				laserCloudCenWidth++;			
			}

			while (centerCubeI >= laserCloudWidth - 3)
			{
      
				for (int j = 0; j < laserCloudHeight; j++)
				{
     
					for (int k = 0; k < laserCloudDepth; k++)
					{
     
						int i = 0;
						pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
							laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
						pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
							laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];		// 把最上面一行的点云取出来
						for (; i < laserCloudWidth - 1; i++)
						{
     
							laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =		
								laserCloudCornerArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];	// i行的点云被i+1行的点云替代
							laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
								laserCloudSurfArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
						}
						laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
							laserCloudCubeCornerPointer;			// 把最小的行的点云放在新增加的一行
						laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
							laserCloudCubeSurfPointer;
						laserCloudCubeCornerPointer->clear();
						laserCloudCubeSurfPointer->clear();
					}
				}

				centerCubeI--;
				laserCloudCenWidth--;
			}

			while (centerCubeJ < 3)
			{
     
				for (int i = 0; i < laserCloudWidth; i++)
				{
     
					for (int k = 0; k < laserCloudDepth; k++)
					{
     
						int j = laserCloudHeight - 1;
						pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
							laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
						pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
							laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
						for (; j >= 1; j--)
						{
     
							laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
								laserCloudCornerArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];
							laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
								laserCloudSurfArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];
						}
						laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
							laserCloudCubeCornerPointer;
						laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
							laserCloudCubeSurfPointer;
						laserCloudCubeCornerPointer->clear();
						laserCloudCubeSurfPointer->clear();
					}
				}

				centerCubeJ++;
				laserCloudCenHeight++;
			}

			while (centerCubeJ >= laserCloudHeight - 3)
			{
     
				for (int i = 0; i < laserCloudWidth; i++)
				{
     
					for (int k = 0; k < laserCloudDepth; k++)
					{
     
						int j = 0;
						pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
							laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
						pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
							laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
						for (; j < laserCloudHeight - 1; j++)
						{
     
							laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
								laserCloudCornerArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];
							laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
								laserCloudSurfArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];
						}
						laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
							laserCloudCubeCornerPointer;
						laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
							laserCloudCubeSurfPointer;
						laserCloudCubeCornerPointer->clear();
						laserCloudCubeSurfPointer->clear();
					}
				}

				centerCubeJ--;
				laserCloudCenHeight--;
			}

			while (centerCubeK < 3)
			{
     
				for (int i = 0; i < laserCloudWidth; i++)
				{
     
					for (int j = 0; j < laserCloudHeight; j++)
					{
     
						int k = laserCloudDepth - 1;
						pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
							laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
						pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
							laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
						for (; k >= 1; k--)
						{
     
							laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
								laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];
							laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
								laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];
						}
						laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
							laserCloudCubeCornerPointer;
						laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
							laserCloudCubeSurfPointer;
						laserCloudCubeCornerPointer->clear();
						laserCloudCubeSurfPointer->clear();
					}
				}

				centerCubeK++;
				laserCloudCenDepth++;
			}

			while (centerCubeK >= laserCloudDepth - 3)
			{
     
				for (int i = 0; i < laserCloudWidth; i++)
				{
     
					for (int j = 0; j < laserCloudHeight; j++)
					{
     
						int k = 0;
						pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
							laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
						pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
							laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
						for (; k < laserCloudDepth - 1; k++)
						{
     
							laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
								laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];
							laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
								laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];
						}
						laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
							laserCloudCubeCornerPointer;
						laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
							laserCloudCubeSurfPointer;
						laserCloudCubeCornerPointer->clear();
						laserCloudCubeSurfPointer->clear();
					}
				}

				centerCubeK--;
				laserCloudCenDepth--;
			}

			// 输出最终的偏移量
			printf("new----laserCloudCenWidth:%d,laserCloudCenHeight:%d,laserCloudCenDepth:%d",laserCloudCenWidth,laserCloudCenHeight,laserCloudCenDepth);
			// 输出最终的当前帧坐标
			printf("new-I:%f, new-J:%f, new-K:%f",centerCubeI,centerCubeJ,centerCubeK);


			// -------------------至此,IJK坐标系维护完成,当前帧位于IJK坐标系中心-------------------
  • 以当前帧为中心,扩展(5,5,3)的黄色小地图
			int laserCloudValidNum = 0;
			int laserCloudSurroundNum = 0;

			// 向IJ坐标轴的正负方向各拓展2个cube,K坐标轴的正负方向各拓展1个cube,上图中五角星所在的蓝色cube就是当前位置
			// 所处的cube,拓展的cube就是黄色的cube,这些cube就是submap的范围			submap的大小就是5,5,3
			for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++)
			{
     
				for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++)
				{
     
					for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++)
					{
     
						if (i >= 0 && i < laserCloudWidth &&   
							j >= 0 && j < laserCloudHeight &&
							k >= 0 && k < laserCloudDepth)// 如果坐标合法  [0,21), [0,21),[0,10]
						{
      
							// 记录submap中的所有cube的index,记为有效index
							laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
							laserCloudValidNum++;
							laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
							laserCloudSurroundNum++;
						}
					}
				}
			}
			
			// submap大小
			printf("submap size: %d\n",laserCloudValidNum);
			// -------------------至此,得到当前帧的局部地图索引-------------------
  • 将submap的点云加在一起
			laserCloudCornerFromMap->clear();
			laserCloudSurfFromMap->clear();
			for (int i = 0; i < laserCloudValidNum; i++)
			{
     
				// 将有效index的cube中的点云叠加到一起组成submap的特征点云
				*laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
				*laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
			}
			int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size();
			int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size();
			// -------------------至此,得到当前帧的局部地图的特征点云-------------------

			// 对上一帧的特征点云下采样
			pcl::PointCloud<PointType>::Ptr laserCloudCornerStack(new pcl::PointCloud<PointType>());
			downSizeFilterCorner.setInputCloud(laserCloudCornerLast);		
			downSizeFilterCorner.filter(*laserCloudCornerStack);
			int laserCloudCornerStackNum = laserCloudCornerStack->points.size();

			pcl::PointCloud<PointType>::Ptr laserCloudSurfStack(new pcl::PointCloud<PointType>());
			downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
			downSizeFilterSurf.filter(*laserCloudSurfStack);
			int laserCloudSurfStackNum = laserCloudSurfStack->points.size();

			printf("submap prepare time %f ms\n", t_shift.toc());
			printf("submap corner num %d  surf num %d \n", laserCloudCornerFromMapNum, laserCloudSurfFromMapNum);	// 输出(5,5,3)个cube中的点云的总数量
  • 如果小地图的点足够多,开始优化匹配
			if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 50)			// 如果submap的特征点云数足够多,对其构建kd-tree
			{
     
				TicToc t_opt;
				TicToc t_tree;
				kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);  // (5,5,3)的submap构建kd-tree
				kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);
				printf("build tree time %f ms \n", t_tree.toc());      

				for (int iterCount = 0; iterCount < 2; iterCount++)		// 迭代两次
				{
     
					//ceres::LossFunction *loss_function = NULL;
					ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);		// LossFunction 
					ceres::LocalParameterization *q_parameterization =
						new ceres::EigenQuaternionParameterization();
					ceres::Problem::Options problem_options;

					ceres::Problem problem(problem_options);					// Problem
					problem.AddParameterBlock(parameters, 4, q_parameterization);		//添加参数块
					problem.AddParameterBlock(parameters + 4, 3);

					TicToc t_data;
					int corner_num = 0;

					for (int i = 0; i < laserCloudCornerStackNum; i++)		// 遍历上一帧的所有Corner点
					{
     
						pointOri = laserCloudCornerStack->points[i];
						// 需要注意的是submap中的点云都是world坐标系,而当前帧的点云都是Lidar坐标系,所以
						// 在搜寻最近邻点时,先用预测的Mapping位姿w_curr,将Lidar坐标系下的特征点变换到world坐标系下
						pointAssociateToMap(&pointOri, &pointSel);
						// 在submap的corner特征点(target)中,寻找距离当前帧corner特征点(source)最近的5个点
						kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); 

						if (pointSearchSqDis[4] < 1.0)
						{
      
							std::vector<Eigen::Vector3d> nearCorners;
							Eigen::Vector3d center(0, 0, 0);
							for (int j = 0; j < 5; j++)
							{
     
								Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,
													laserCloudCornerFromMap->points[pointSearchInd[j]].y,
													laserCloudCornerFromMap->points[pointSearchInd[j]].z);
								center = center + tmp;
								nearCorners.push_back(tmp);
							}
							// 计算这个5个最近邻点的中心
							center = center / 5.0;

							// 协方差矩阵
							Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
							for (int j = 0; j < 5; j++)
							{
     
								Eigen::Matrix<double, 3, 1> tmpZeroMean = nearCorners[j] - center;
								covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
							}

							// 计算协方差矩阵的特征值和特征向量,用于判断这5个点是不是呈线状分布,此为PCA的原理
							Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);

							// if is indeed line feature
							// note Eigen library sort eigenvalues in increasing order
							Eigen::Vector3d unit_direction = saes.eigenvectors().col(2);// 如果5个点呈线状分布,最大的特征值对应的特征向量就是该线的方向向量
							Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
							if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])// 如果最大的特征值 >> 其他特征值,则5个点确实呈线状分布,否则认为直线“不够直”
							{
      
								Eigen::Vector3d point_on_line = center;
								Eigen::Vector3d point_a, point_b;
								// 从中心点沿着方向向量向两端移动0.1m,构造线上的两个点
								point_a = 0.1 * unit_direction + point_on_line;
								point_b = -0.1 * unit_direction + point_on_line;

								// 然后残差函数的形式就跟Odometry一样了,残差距离即点到线的距离,到介绍lidarFactor.cpp时再说明具体计算方法
								ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);
								problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
								corner_num++;	
							}							
						}
						/*
						else if(pointSearchSqDis[4] < 0.01 * sqrtDis)
						{
							Eigen::Vector3d center(0, 0, 0);
							for (int j = 0; j < 5; j++)
							{
								Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,
													laserCloudCornerFromMap->points[pointSearchInd[j]].y,
													laserCloudCornerFromMap->points[pointSearchInd[j]].z);
								center = center + tmp;
							}
							center = center / 5.0;	
							Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
							ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);
							problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
						}
						*/
					}

					int surf_num = 0;
					for (int i = 0; i < laserCloudSurfStackNum; i++)
					{
     
						pointOri = laserCloudSurfStack->points[i];
			
						pointAssociateToMap(&pointOri, &pointSel);
						kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

						// 求面的法向量就不是用的PCA了(虽然论文中说还是PCA),使用的是最小二乘拟合,是为了提效?不确定
						// 假设平面不通过原点,则平面的一般方程为Ax + By + Cz + 1 = 0,用这个假设可以少算一个参数,提效。
						Eigen::Matrix<double, 5, 3> matA0;
						Eigen::Matrix<double, 5, 1> matB0 = -1 * Eigen::Matrix<double, 5, 1>::Ones();
						// 用上面的2个矩阵表示平面方程就是 matA0 * norm(A, B, C) = matB0,这是个超定方程组,因为数据个数超过未知数的个数
						if (pointSearchSqDis[4] < 1.0)
						{
     
							for (int j = 0; j < 5; j++)
							{
     
								matA0(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;
								matA0(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;
								matA0(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;
							}
							// 求解这个最小二乘问题,可得平面的法向量,find the norm of plane
							Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0);
							// Ax + By + Cz + 1 = 0,全部除以法向量的模长,方程依旧成立,而且使得法向量归一化了
							double negative_OA_dot_norm = 1 / norm.norm();
							norm.normalize();

							// Here n(pa, pb, pc) is unit norm of plane
							bool planeValid = true;
							for (int j = 0; j < 5; j++)
							{
     
								// 点(x0, y0, z0)到平面Ax + By + Cz + D = 0 的距离公式 = fabs(Ax0 + By0 + Cz0 + D) / sqrt(A^2 + B^2 + C^2)
								if (fabs(norm(0) * laserCloudSurfFromMap->points[pointSearchInd[j]].x +
										 norm(1) * laserCloudSurfFromMap->points[pointSearchInd[j]].y +
										 norm(2) * laserCloudSurfFromMap->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2)
								{
     
									planeValid = false;// 平面没有拟合好,平面“不够平”
									break;
								}
							}
							Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
							if (planeValid)
							{
     
								// 构造点到面的距离残差项,同样的,具体到介绍lidarFactor.cpp时再说明该残差的具体计算方法
								ceres::CostFunction *cost_function = LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm);
								problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
								surf_num++;
							}
						}
						/*
						else if(pointSearchSqDis[4] < 0.01 * sqrtDis)
						{
							Eigen::Vector3d center(0, 0, 0);
							for (int j = 0; j < 5; j++)
							{
								Eigen::Vector3d tmp(laserCloudSurfFromMap->points[pointSearchInd[j]].x,
													laserCloudSurfFromMap->points[pointSearchInd[j]].y,
													laserCloudSurfFromMap->points[pointSearchInd[j]].z);
								center = center + tmp;
							}
							center = center / 5.0;	
							Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
							ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);
							problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
						}
						*/
					}

					//printf("corner num %d used corner num %d \n", laserCloudCornerStackNum, corner_num);
					//printf("surf num %d used surf num %d \n", laserCloudSurfStackNum, surf_num);

					printf("mapping data assosiation time %f ms \n", t_data.toc());

					TicToc t_solver;
					ceres::Solver::Options options;
					options.linear_solver_type = ceres::DENSE_QR;
					options.max_num_iterations = 4;
					options.minimizer_progress_to_stdout = false;
					options.check_gradients = false;
					options.gradient_check_relative_precision = 1e-4;
					ceres::Solver::Summary summary;
					ceres::Solve(options, &problem, &summary);
					printf("mapping solver time %f ms \n", t_solver.toc());

					//printf("time %f \n", timeLaserOdometry);
					//printf("corner factor num %d surf factor num %d\n", corner_num, surf_num);
					//printf("result q %f %f %f %f result t %f %f %f\n", parameters[3], parameters[0], parameters[1], parameters[2],
					//	   parameters[4], parameters[5], parameters[6]);
				}
				printf("mapping optimization twice time %f \n", t_opt.toc());
				// -------------------至此,两次迭代完成了,得到w_curr,当前帧在map坐标系下的pose----------------------------
			}
			else		// submap的点数量不够
			{
     
				ROS_WARN("time Map corner and surf num are not enough");			
			}
  • 更新mapping到Odometry线程的T
			// 完成ICP(迭代2次)的特征匹配后,用最后匹配计算出的优化变量w_curr,更新增量wmap_wodom,为下一次
			// Mapping做准备
			transformUpdate();			// 获取map到odom的变换Transform
  • 更新submap的点云
			TicToc t_add;


			// 创建两个indices,用来存放当前帧的点所处cube的index
			std::vector<int> CornerInd();
			std::vector<int> SurfInd();

			// 下面两个for loop的作用就是将当前帧的特征点云,逐点进行操作:转换到world坐标系并添加到对应位置的cube中
			for (int i = 0; i < laserCloudCornerStackNum; i++)
			{
     
				// Lidar坐标系转到world坐标系
				pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);

				// 计算本次的特征点的IJK坐标,进而确定添加到哪个cube中
				int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
				int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
				int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;

				if (pointSel.x + 25.0 < 0)
					cubeI--;
				if (pointSel.y + 25.0 < 0)
					cubeJ--;
				if (pointSel.z + 25.0 < 0)
					cubeK--;

				if (cubeI >= 0 && cubeI < laserCloudWidth &&
					cubeJ >= 0 && cubeJ < laserCloudHeight &&
					cubeK >= 0 && cubeK < laserCloudDepth)
				{
     
					int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
					laserCloudCornerArray[cubeInd]->push_back(pointSel);		// PointCloud.push_back
					CornerInd.append(cubeInd);
				}
			}

			for (int i = 0; i < laserCloudSurfStackNum; i++)
			{
     
				pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);

				int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
				int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
				int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;

				if (pointSel.x + 25.0 < 0)
					cubeI--;
				if (pointSel.y + 25.0 < 0)
					cubeJ--;
				if (pointSel.z + 25.0 < 0)
					cubeK--;

				if (cubeI >= 0 && cubeI < laserCloudWidth &&
					cubeJ >= 0 && cubeJ < laserCloudHeight &&
					cubeK >= 0 && cubeK < laserCloudDepth)
				{
     
					int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
					laserCloudSurfArray[cubeInd]->push_back(pointSel);
					SurfInd.append(cubeInd);
				}
			}
			printf("add current points to submap time %f ms\n", t_add.toc());

			
			TicToc t_filter;

			// 对submap的cube点云进行下采样
			// 因为新增加了点云,对之前已经存有点云的submap cube全部重新进行一次降采样
			// 这个地方可以简单优化一下:如果之前的cube没有新添加点就不需要再降采样
			for (int i = 0; i < laserCloudValidNum; i++)		// 遍历submap的所有cube
			{
     
				int ind = laserCloudValidInd[i];		// submap中每一个cube的索引
				// 判断当前的submap的cube id 是否在当前帧的索引的vector中


				pcl::PointCloud<PointType>::Ptr tmpCorner(new pcl::PointCloud<PointType>());
				downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);
				downSizeFilterCorner.filter(*tmpCorner);
				laserCloudCornerArray[ind] = tmpCorner;

				pcl::PointCloud<PointType>::Ptr tmpSurf(new pcl::PointCloud<PointType>());
				downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);
				downSizeFilterSurf.filter(*tmpSurf);
				laserCloudSurfArray[ind] = tmpSurf;
			}
			printf("filter time %f ms \n", t_filter.toc());
  • 发布
			// 每5帧发布一次submap  (5,5,3)的cube大小
			TicToc t_pub;
			//publish surround map for every 5 frames
			if (frameCount % 5 == 0)
			{
     
				laserCloudSurround->clear();
				for (int i = 0; i < laserCloudSurroundNum; i++)		// 遍历submap
				{
     
					int ind = laserCloudSurroundInd[i];
					*laserCloudSurround += *laserCloudCornerArray[ind];
					*laserCloudSurround += *laserCloudSurfArray[ind];
				}

				sensor_msgs::PointCloud2 laserCloudSurround3;
				pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);
				laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
				laserCloudSurround3.header.frame_id = "/camera_init";		// world坐标系
				pubLaserCloudSurround.publish(laserCloudSurround3);
			}

			// 每20帧发布IJK局部地图
			// pub laserCloudMap for every 20 frames
			if (frameCount % 20 == 0)
			{
     
				pcl::PointCloud<PointType> laserCloudMap;
				for (int i = 0; i < 4851; i++)			// 遍历IJK坐标系
				{
     
					laserCloudMap += *laserCloudCornerArray[i];
					laserCloudMap += *laserCloudSurfArray[i];
				}
				sensor_msgs::PointCloud2 laserCloudMsg;
				pcl::toROSMsg(laserCloudMap, laserCloudMsg);
				laserCloudMsg.header.stamp = ros::Time().fromSec(timeLaserOdometry);
				laserCloudMsg.header.frame_id = "/camera_init";
				pubLaserCloudMap.publish(laserCloudMsg);
			}

			// 全部点云转化到world坐标系,并发布
			int laserCloudFullResNum = laserCloudFullRes->points.size();
			for (int i = 0; i < laserCloudFullResNum; i++)
			{
     
				pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);		
			}

			sensor_msgs::PointCloud2 laserCloudFullRes3;
			pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
			laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
			laserCloudFullRes3.header.frame_id = "/camera_init";
			pubLaserCloudFullRes.publish(laserCloudFullRes3);

			printf("mapping pub time %f ms \n", t_pub.toc());

			printf("whole mapping time %f ms +++++\n", t_whole.toc());

			// Odometry
			nav_msgs::Odometry odomAftMapped;
			odomAftMapped.header.frame_id = "/camera_init";
			odomAftMapped.child_frame_id = "/aft_mapped";
			odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);
			odomAftMapped.pose.pose.orientation.x = q_w_curr.x();			// 当前帧在world下的pose,ceres的优化变量
			odomAftMapped.pose.pose.orientation.y = q_w_curr.y();
			odomAftMapped.pose.pose.orientation.z = q_w_curr.z();
			odomAftMapped.pose.pose.orientation.w = q_w_curr.w();
			odomAftMapped.pose.pose.position.x = t_w_curr.x();
			odomAftMapped.pose.pose.position.y = t_w_curr.y();
			odomAftMapped.pose.pose.position.z = t_w_curr.z();
			pubOdomAftMapped.publish(odomAftMapped);
			
			// Path
			geometry_msgs::PoseStamped laserAfterMappedPose;
			laserAfterMappedPose.header = odomAftMapped.header;
			laserAfterMappedPose.pose = odomAftMapped.pose.pose;
			laserAfterMappedPath.header.stamp = odomAftMapped.header.stamp;
			laserAfterMappedPath.header.frame_id = "/camera_init";
			laserAfterMappedPath.poses.push_back(laserAfterMappedPose);
			pubLaserAfterMappedPath.publish(laserAfterMappedPath);

			// 发布tf变换,(world)/camera_init--->(当前帧)/aft_mapped
			static tf::TransformBroadcaster br;
			tf::Transform transform;
			tf::Quaternion q;
			transform.setOrigin(tf::Vector3(t_w_curr(0),
											t_w_curr(1),
											t_w_curr(2)));
			q.setW(q_w_curr.w());
			q.setX(q_w_curr.x());
			q.setY(q_w_curr.y());
			q.setZ(q_w_curr.z());
			transform.setRotation(q);
			br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "/camera_init", "/aft_mapped"));

			frameCount++;

5、lidarFactor.cpp

1、构建误差项

// 点到线的残差距离计算
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 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]};		// last到curr的旋转四元数
		Eigen::Quaternion<T> q_identity{
     T(1), T(0), T(0), T(0)};
		// 考虑运动补偿(四元数插值),ktti点云已经补偿过所以可以忽略下面的对四元数slerp插值以及对平移的线性插值
		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;
		// Odometry线程时,下面是将当前帧Lidar坐标系下的cp点变换到上一帧的Lidar坐标系下,然后在上一帧的Lidar坐标系计算点到线的残差距离
		// Mapping线程时,下面是将当前帧Lidar坐标系下的cp点变换到world坐标系下,然后在world坐标系下计算点到线的残差距离
		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.norm() / de.norm(); 为啥也分成3个,我也不知
		// 道,从我试验的效果来看,确实是下面的残差函数形式,最后输出的pose精度会好一点点,这里需要
		// 注意的是,所有的residual都不用加fabs,因为Ceres内部会对其求 平方 作为最终的残差项
		residual[0] = nu.x() / de.norm();
		residual[1] = nu.y() / de.norm();
		residual[2] = nu.z() / de.norm();

		return true;
	}

	// 返回函数的指针 
	static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,
									   const Eigen::Vector3d last_point_b_, const double s_)
	{
     
		return (new ceres::AutoDiffCostFunction<
				LidarEdgeFactor, 3, 4, 3>(
//					             ^  ^  ^
//					             |  |  |
//			      残差的维度 ____|  |  |
//			 优化变量q的维度 _______|  |
//			 优化变量t的维度 __________|
			new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));
	}

	// 一些变量
	Eigen::Vector3d curr_point, last_point_a, last_point_b;
	double s;
};

// 计算Odometry线程中点到面的残差距离
struct LidarPlaneFactor
{
     
	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_)
	{
     
		// 点l、j、m就是搜索到的最近邻的3个点,下面就是计算出这三个点构成的平面ljlm的法向量
		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())};				// cp
		Eigen::Matrix<T, 3, 1> lpj{
     T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())};		// last point j
		//Eigen::Matrix lpl{T(last_point_l.x()), T(last_point_l.y()), T(last_point_l.z())};
		//Eigen::Matrix lpm{T(last_point_m.x()), T(last_point_m.y()), T(last_point_m.z())};
		Eigen::Matrix<T, 3, 1> ljm{
     T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())};		// 法向量

		//Eigen::Quaternion 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;

		// 计算点到平面的残差距离,如下图所示
		residual[0] = (lp - lpj).dot(ljm);

		return true;
	}

	static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_,
									   const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_,
									   const double s_)
	{
     
		return (new ceres::AutoDiffCostFunction<
				LidarPlaneFactor, 1, 4, 3>(
//				 	              ^  ^  ^
//			 		              |  |  |
//			       残差的维度 ____|  |  |
//			  优化变量q的维度 _______|  |
//		 	  优化变量t的维度 __________|
			new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_)));
	}

	Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;
	Eigen::Vector3d ljm_norm;
	double s;
};

struct LidarPlaneNormFactor
{
     

	LidarPlaneNormFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_,
						 double negative_OA_dot_norm_) : curr_point(curr_point_), plane_unit_norm(plane_unit_norm_),
														 negative_OA_dot_norm(negative_OA_dot_norm_) {
     }

	template <typename T>
	bool operator()(const T *q, const T *t, T *residual) const
	{
     
		Eigen::Quaternion<T> q_w_curr{
     q[3], q[0], q[1], q[2]};
		Eigen::Matrix<T, 3, 1> t_w_curr{
     t[0], t[1], t[2]};
		Eigen::Matrix<T, 3, 1> cp{
     T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
		Eigen::Matrix<T, 3, 1> point_w;
		point_w = q_w_curr * cp + t_w_curr;

		Eigen::Matrix<T, 3, 1> norm(T(plane_unit_norm.x()), T(plane_unit_norm.y()), T(plane_unit_norm.z()));
		residual[0] = norm.dot(point_w) + T(negative_OA_dot_norm);
		return true;
	}

	static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d plane_unit_norm_,
									   const double negative_OA_dot_norm_)
	{
     
		return (new ceres::AutoDiffCostFunction<
				LidarPlaneNormFactor, 1, 4, 3>(
			new LidarPlaneNormFactor(curr_point_, plane_unit_norm_, negative_OA_dot_norm_)));
	}

	Eigen::Vector3d curr_point;
	Eigen::Vector3d plane_unit_norm;
	double negative_OA_dot_norm;
};


struct LidarDistanceFactor
{
     

	LidarDistanceFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d closed_point_) 
						: curr_point(curr_point_), closed_point(closed_point_){
     }

	template <typename T>
	bool operator()(const T *q, const T *t, T *residual) const
	{
     
		Eigen::Quaternion<T> q_w_curr{
     q[3], q[0], q[1], q[2]};
		Eigen::Matrix<T, 3, 1> t_w_curr{
     t[0], t[1], t[2]};
		Eigen::Matrix<T, 3, 1> cp{
     T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
		Eigen::Matrix<T, 3, 1> point_w;
		point_w = q_w_curr * cp + t_w_curr;


		residual[0] = point_w.x() - T(closed_point.x());
		residual[1] = point_w.y() - T(closed_point.y());
		residual[2] = point_w.z() - T(closed_point.z());
		return true;
	}

	static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d closed_point_)
	{
     
		return (new ceres::AutoDiffCostFunction<
				LidarDistanceFactor, 3, 4, 3>(
			new LidarDistanceFactor(curr_point_, closed_point_)));
	}

	Eigen::Vector3d curr_point;
	Eigen::Vector3d closed_point;
};

参考

LOAM笔记及A-LOAM源码阅读

你可能感兴趣的:(SLAM,slam,a-loam)