laserOdometry

最新推荐文章于 2022-11-05 21:13:44 发布
最新推荐文章于 2022-11-05 21:13:44 发布
阅读量274 收藏 1
点赞数 2
分类专栏： SLAM 文章标签： 自动驾驶 c++
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_37497304/article/details/123713587
版权

laserOdometry.cpp

在这里插入图片描述

订阅5个话题：/velodyne_cloud_2 、/laser_cloud_less_sharp、/laser_cloud_flat、/laser_cloud_less_flat、/laser_cloud_sharp。基于前述的4种feature进行帧与帧的点云特征配准。

全局变量

#define DISTORTION 0

int corner_correspondence = 0, plane_correspondence = 0;

constexpr double SCAN_PERIOD = 0.1;
constexpr double DISTANCE_SQ_THRESHOLD = 25; //进行距离比较的参数
constexpr double NEARBY_SCAN = 2.5;// 找点进行匹配优化时的线数距离

int skipFrameNum = 5;// 发给建图算法的频率为5Hz
bool systemInited = false;

double timeCornerPointsSharp = 0;
double timeCornerPointsLessSharp = 0;
double timeSurfPointsFlat = 0;
double timeSurfPointsLessFlat = 0;
double timeLaserCloudFullRes = 0;

//上一帧边缘点构成KDTree
pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<pcl::PointXYZI>());
//上一帧平面点构成KDTree
pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN<pcl::PointXYZI>());

//极大边缘点
pcl::PointCloud<PointType>::Ptr cornerPointsSharp(new pcl::PointCloud<PointType>());

//极小平面点
pcl::PointCloud<PointType>::Ptr cornerPointsLessSharp(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr surfPointsFlat(new pcl::PointCloud<PointType>());

//次小平面点
pcl::PointCloud<PointType>::Ptr surfPointsLessFlat(new pcl::PointCloud<PointType>());

//上一帧的边缘点
pcl::PointCloud<PointType>::Ptr laserCloudCornerLast(new pcl::PointCloud<PointType>());

//上一帧的平面点
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast(new pcl::PointCloud<PointType>());

//所有点云
pcl::PointCloud<PointType>::Ptr laserCloudFullRes(new pcl::PointCloud<PointType>());

int laserCloudCornerLastNum = 0;
int laserCloudSurfLastNum = 0;

// Lidar Odometry线程估计的当前帧在世界坐标系下的位姿
Eigen::Quaterniond q_w_curr(1, 0, 0, 0); //旋转四元数
Eigen::Vector3d t_w_curr(0, 0, 0);//平移向量

// 点云特征匹配时的优化变量
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);

//接收到的消息缓存队列
std::queue<sensor_msgs::PointCloud2ConstPtr> cornerSharpBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> cornerLessSharpBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> surfFlatBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> surfLessFlatBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> fullPointsBuf;
std::mutex mBuf; //互斥锁对象，为了避免多个线程在某一时刻同时操作一个共享资源

一些函数

void TransformToStart(PointType const *const pi, PointType *const po)
{	//将当前帧Lidar坐标系下的点云变换到一开始，去除因匀速运动产生的畸变
    //interpolation ratio
    double s; 
    if (DISTORTION) 
        s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;//intensity的整数部分存放scanID，小数部分存放归一化后的fireID，int强转向下取整。因此s=fireID/0.1
    else
        s = 1.0;
    //s = 1;
    //Eigen::Map<Eigen::Quaterniond> q_last_curr(para_q)，表示坐标变换的旋转增量
    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;
}
void TransformToEnd(PointType const *const pi, PointType *const po)
{   
    //传入参数：输入点云与输出点云
    // 先去运动畸变
    pcl::PointXYZI un_point_tmp;
    TransformToStart(pi, &un_point_tmp);

    Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z);//去畸变后的点
    Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr);

    po->x = point_end.x();
    po->y = point_end.y();
    po->z = point_end.z();

    //Remove distortion time info
    po->intensity = int(pi->intensity);
}

//一下函数都为缓存对应消息的回调函数
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();
}

main

int main(int argc, char **argv)
{
    ros::init(argc, argv, "laserOdometry");
    ros::NodeHandle nh;

    /*
     * 设定里程计的帧率（每跳过skipFrameNum帧采样1帧）
     */
    nh.param<int>("mapping_skip_frame", skipFrameNum, 2);

    printf("Mapping %d Hz \n", 10 / skipFrameNum);

    // 从scanRegistration订阅的消息话题
    // 极大边线点  次极大边线点   极小平面点  次极小平面点
    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);

    // 全部点云点
    ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100, laserCloudFullResHandler);


    // 发布上一帧的边线点
    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);
    // 发布全部有序点云，就是从scanRegistration订阅来的点云，未经其他处理
    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);

    nav_msgs::Path laserPath;

    int frameCount = 0;
    // 循环频率
    ros::Rate rate(100);

    while (ros::ok())
    {
        // 帧间位姿估计过程
        ...
        rate.sleep();
    }
    return 0;
}

帧间匹配与位姿估计

目标：希望找到位姿变换T，使得第k帧点云左乘T得到第k+1帧点云，或者与第k+1帧点云的误差最小。

边缘点匹配
while (ros::ok())
    {
        // 等待回调函数执行完毕，执行一次后续代码
        ros::spinOnce();

        // 如果所有订阅的话题都收到了
        if (!cornerSharpBuf.empty() && !cornerLessSharpBuf.empty() &&
            !surfFlatBuf.empty() && !surfLessFlatBuf.empty() &&
            !fullPointsBuf.empty())
        {
            // 这里是5个queue记录了最新的极大/次极大边线点，极小/次极小平面点，全部点云
            // 用队列头的时间戳，分配时间戳
            timeCornerPointsSharp = cornerSharpBuf.front()->header.stamp.toSec(); //double
            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)
            {
                // ROS出现丢包会导致这种情况
                printf("unsync messeage!");
                ROS_BREAK();
            }

            // 从队列中取出来对应的数据，并转换成pcl点云数据
            mBuf.lock(); //上锁
            cornerPointsSharp->clear(); //Removes all points in a cloud and sets the width and height to 0.
            //将缓存队列的front转换为pcl点云数据，再pop
            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();

            TicToc t_whole; 
            // initializing 和 特征点匹配位姿估计
            // 对初始帧的操作是，跳过位姿估计，直接作为第二帧的上一帧
   			if (!systemInited)
            {   
                // 主要用来跳过第一帧数据
                // 仅仅将cornerPointsLessSharp保存至laserCloudCornerLast
                // 将surfPointsLessFlat保存至laserCloudSurfLast，以及更新对应的kdtree
                systemInited = true;
                std::cout << "Initialization finished \n";
            }
            else         
            { // 主要的处理部分
                // 极大边线点的数量
                int cornerPointsSharpNum = cornerPointsSharp->points.size();
                // 极小平面点的数量
                int surfPointsFlatNum = surfPointsFlat->points.size();

                TicToc t_opt;
                // 点到线以及点到面的非线性优化，迭代2次（选择当前优化的位姿的特征点匹配，并优化位姿（4次迭代），然后重新选择特征点匹配并优化）
                for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)
                {
                    corner_correspondence = 0;
                    plane_correspondence = 0;

                    //配置优化求解器
                    // ceres::LossFunction *loss_function = NULL;
                    // 使用Huber核函数来减少外点的影响，Algorithm 1: Lidar Odometry中有
                    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;
                    // find correspondence for corner features
                    // 基于最近邻原理建立corner特征点之间关联，每一个极大边线点去上一帧的次极大边线点集中找匹配
                    for (int i = 0; i < cornerPointsSharpNum; ++i)
                    {   //在上一帧的点云中寻找边缘点的对应点
                        // 这个函数类似论文中公式（5）的功能
                        // 将当前帧的极大边线点（记为点O_cur），变换到上一帧Lidar坐标系（记为点O），以利于寻找极大边线点的correspondence。将转换的结果储存在pointSel中
                        TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);
                        //查询pointSel的最近邻
                        参数：查询点；邻近个数；储存搜索到的近邻点的索引；储存查询点与对应近邻点中心距离平方
                        //kdtreeCornerLast中存储的是上一帧的边缘点
                        kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);

                        int closestPointInd = -1, minPointInd2 = -1;
                        // 如果最近邻的corner特征点之间距离平方小于阈值，则最近邻点l有效
                        if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
                        {
                            closestPointInd = pointSearchInd[0]; //储存该有效的最近邻点的index
                            // 找到了目标点i的最邻近点m的id
                            int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);
                            double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;

                            // 在点m的两侧寻找附近扫描线上的最邻近点j
                            //向上搜寻
                            for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j)
                            {   
                                // 如果点的scanID小于等于该点的scanID，说明在下方或者在同一条扫描线上
                                if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)
                                    continue;

                                // if not in nearby scans, end the loop, NEARBY_SCAN=2.5
                                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)
                                {
                                    // find nearer point j
                                    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);

                                if (pointSqDis < minPointSqDis2)
                                {
                                    // find nearer point
                                    minPointSqDis2 = pointSqDis;
                                    minPointInd2 = j;
                                }
                            }
                        }
                        //经过以上两个搜寻步骤，找到了点m的最近邻点j的索引和距离
                        // 即特征点i的两个最近邻点j和m都有效，构建非线性优化问题
                        if (minPointInd2 >= 0) 
                        {
                            //当前点
                            Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x,
                                                       cornerPointsSharp->points[i].y,
                                                       cornerPointsSharp->points[i].z); 
                            //上一帧点m
                            Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x,
                                                         laserCloudCornerLast->points[closestPointInd].y,
                                                         laserCloudCornerLast->points[closestPointInd].z);
                            //上一帧点j
                            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;      
                            //利用这三点构建点到线的距离残差项
                            ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
                            
                            //待优化的参数为para_q,para_t
                            problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
                            corner_correspondence++;
                        }
                    }

                    // 后面是平面点匹配
                    ...
    }
平面点匹配

在这里插入图片描述

现在在第k+1帧中发现了平面点i，查询在第k帧（上一帧）中的最近邻点（by KD-tree）j，查询j的附近扫描线上的最近邻点l和同一条扫描线的最近邻点m，这三点确定一个平面，让点i与这个平面的距离最短。

// find correspondence for plane features
// 与上述类似，寻找平面点的最近邻点j,l,m
for (int i = 0; i < surfPointsFlatNum; ++i)
{
    TransformToStart(&(surfPointsFlat->points[i]), &pointSel); //把该点转换到上一帧坐标系并储存在pointSel中
    kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); //用kdTree搜寻临近点

    int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
    if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
    {
        closestPointInd = pointSearchInd[0];//点j的索引

        // 计算出点j的scanID
        int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);
        double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;
        
        // 遍历搜索点前后所有的点找到minPointSqDis2和minPointSqDis3，因此一定能找到值
        //向上搜索点l和点m
        for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j)
        {
            // 如果超出NEARBY_SCAN范围，结束循环
            if (int(laserCloudSurfLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
                break;

            //计算点j和该点的距离
            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);

            // 如果该点和点j在同一条scan上并且距离符合条件，则作为点l
            if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)
            {
                minPointSqDis2 = pointSqDis;
                minPointInd2 = j;
            }
            // 如果该点比点j的scan更高，同时符合条件，则作为点m
            else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
            {
                minPointSqDis3 = pointSqDis;
                minPointInd3 = j;
            }
        }

        // 向下搜索点l和点m
        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 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;
            }
        }
        //以上步骤找到了点j，点l和点m

        // 利用这四点构建ceres非线性优化问题
        if (minPointInd2 >= 0 && minPointInd3 >= 0)
        {
            //点i
            Eigen::Vector3d curr_point(surfPointsFlat->points[i].x,
                surfPointsFlat->points[i].y,
                surfPointsFlat->points[i].z);
            //点j
            Eigen::Vector3d last_point_a(laserCloudSurfLast->points[closestPointInd].x,
                laserCloudSurfLast->points[closestPointInd].y,
                laserCloudSurfLast->points[closestPointInd].z);
            //点l
            Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x,
                laserCloudSurfLast->points[minPointInd2].y,
                laserCloudSurfLast->points[minPointInd2].z);
            //点m
            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++; //平面点对应关系的数量
        }
    }
}

// 输出寻找关联点消耗的时间
printf("data association time %f ms \n", t_data.toc());
// 如果找到的关联点少于10个，则警告
if ((corner_correspondence + plane_correspondence) < 10)
{
    printf("less correspondence! *************************************************\n");
}

TicToc t_solver;
// 设定求解器类型，最大迭代次数，不输出过程信息，优化报告存入summary
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);
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;
}

发布结果与后处理

将结果发布出去，并为下一次的帧间匹配做准备。

			TicToc t_pub;

            // publish odometry
            // 创建nav_msgs::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);

            // publish path
            // geometry_msgs::PoseStamped消息是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);
          
            // transform corner features and plane features to the scan end point
            if (0)
            {
                int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size();
                for (int i = 0; i < cornerPointsLessSharpNum; i++)
                {
                    TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]);
                }

                int surfPointsLessFlatNum = surfPointsLessFlat->points.size();
                for (int i = 0; i < surfPointsLessFlatNum; i++)
                {
                    TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]);
                }

                int laserCloudFullResNum = laserCloudFullRes->points.size();
                for (int i = 0; i < laserCloudFullResNum; i++)
                {
                    TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
                }
            }
            // 位姿估计完毕之后，当前边线点和平面点就变成了上一帧的边线点和平面点

			//将cornerPointsLessSharp的内容赋给laserCloudCornerLast
            // 把索引和数量都转移过去
            pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
            cornerPointsLessSharp = laserCloudCornerLast;
            laserCloudCornerLast = laserCloudTemp;

			//将surfPointsLessFlat与laserCloudSurfLast交换
            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';

			//分别构建laserCloudCornerLast和laserCloudSurfLast的kdTree
            kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
            kdtreeSurfLast->setInputCloud(laserCloudSurfLast);

            // 为了控制最后一个节点的执行频率， 只有整除时才发布结果
            if (frameCount % skipFrameNum == 0)
            {
                frameCount = 0;

                // 发布次极大边线点给laserMapping
                // ros::Time().fromSec()实现将具体时间转换为时间戳
                sensor_msgs::PointCloud2 laserCloudCornerLast2;
                pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
                laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
                laserCloudCornerLast2.header.frame_id = "/camera";
                pubLaserCloudCornerLast.publish(laserCloudCornerLast2);

                // 发布次极小平面点
                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);
                
            }
            printf("publication time %f ms \n", t_pub.toc());
            printf("whole laserOdometry time %f ms \n \n", t_whole.toc());
            if(t_whole.toc() > 100)
                ROS_WARN("odometry process over 100ms");
            frameCount++;
        }
        rate.sleep();
    }
Demon_zzj
关注
  • 2
    点赞
  • 踩
  • 1
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
srf_laser_odometry:基于对称距离流和多扫描对齐的鲁棒平面里程计
05-30
srf_laser_odometry 基于对称距离流和多扫描对齐的鲁棒平面里程计 该 pkgs 提供了基于平面激光扫描的 2D 里程计的快速可靠估计。 SRF 是的延续， 是一种快速而精确的方法，可通过连续距离扫描来估计激光雷达的平面运动。 SRF 提出了一种使用激光扫描仪估计平面运动的密集方法。 从两次扫描之间的几何一致性的对称表示开始，我们得出精确的范围流量约束，并根据扫描仪的刚性运动来表达扫描观测值的运动。 与将传入扫描与前一个或最后一个选择的键扫描对齐的现有技术相比，我们提出了一种组合且有效的公式，以在每次迭代时联合对齐所有这三个扫描。 这种新公式保留了基于键扫描的策略的优点，但对键扫描的次优选择和移动对象的存在更加稳健。 这种方法的一个广泛的评估，提出在静态和动态环境模拟和真实数据。 结果表明，在所有进行的实验中，我们的方法都比现有方法快一个数量级，并且准确性大大提高。 运
LOAM_velodyne 2.laserOdometry 激光里程计
xiaoma_bk的博客
11-07 6257
注解： 功能：进行点云匹配，完成运动估计（帧间匹配）。 具体实现：利用ScanRegistration中提取到的特征点，建立相邻时间点云数据之间的关联，由此推断lidar的运动。 备注：绿色的函数指在下面解释，红色的函数指本文单独的函数解释 代码流程： 1、主函数： main /** Main node entry point. */ int main(int argc, char ...
loam源码解析3 ： laserOdometry（一）
SLAM小学僧
06-17 2031
laserOdometry.cpp解析一一、概述二、变量说明三、数据预处理1. 接受来自scanRegistration的ROS消息(1) laserCloudSharpHandler(2) laserCloudLessSharpHandler(3) laserCloudFlatHandler(4) laserCloudFlatHandler(5) laserCloudFullResHandler(6) imuTransHandler2. 定义ROS发布3. 创建相关的对象四、里程计初始化五、畸变去除1.去
loam算法_LOAM源码解析2——laserOdometry
weixin_39665787的博客
12-04 434
这是LOAM第二部分Lidar laserOdometry雷达里程计。 在第一章提取完特征点后，需要对特征点云进行关联匹配，之后估计姿态。 主要分为两部分：特征点关联使用scan-to-scan方式t和t+1时刻相邻两帧的点云数据进行配准，分为边缘点匹配和平面点匹配两部分。计算点到直线的距离和点到平面的距离。 姿态解算根据匹配的特征点云使用LM算法估计接收端位姿。这部分代码完全是放在main函数...
loam的学习记录2（laserOdometry节点）
HelloJinYe的博客
10-19 1342
laserOdometry的笔记
LOAM源码结合论文解析（二）laserOdometry
jtcap的博客
04-03 931
laserOdometry节点接收scanResgistraion传过来的点云，进行点云匹配来估计位姿，并发布里程计以及去畸变的点云给mapping节点。下面先大体分析下算法原理。 算法概要分析 算法流程： 将上一帧( — )的点云补偿到 时刻，表示为； 将当前帧点云中的特征点（边缘点和平面点）补偿也补偿到 时刻； 和特征点完成点线匹配和点面匹配 构建最小二乘问题进行优化 具体如何操作，在下面结合相关代码进行细致说明。 laserCloudXXXHandler() 订阅句柄的操作都
matlab的代码在相机上实现-visual_odom:该存储库是立体Odometry的C++OpenCV实现
05-19
matlab的代码在相机上实现立体视觉测程 该存储库是使用立体视觉calcOpticalFlowPyrLK C ++ OpenCV实现，使用OpenCV calcOpticalFlowPyrLK进行功能跟踪。 参考文件： 演示视频： 要求 如果您不使用CUDA： ...
ppt格式下载可编辑，包含ros架构，通讯机制，a-loam简介
最新发布
09-07
ROS架构、通讯机制、A-LOAM简介 ROS架构： ROS（Robot Operating System）是基于Linux的、开源的...• src源文件夹，包括前文提到的几个主要C++文件：scanRegistration、laserOdometry、laserOdometry、lidarFact
loam代码结构框图1
08-03
- `Vector3.h`, `LaserOdometry.h`, `LaserMapping.h`, `TransformMaintenance.h`等定义了算法中使用的数据结构和类，如向量、激光里程计、激光映射和坐标变换维护的接口。 - `Angle.h`, `CircularBuffer.h`, `...
基于ROS的无人载具上位机控制系统.zip
06-24
ROS中的`gmapping`或`loam_laserOdometry`等包可以帮助实现这一功能。 7. **上位机界面**：上位机通常会有一个人机交互界面，显示无人载具的状态、路径、传感器数据等。这可能通过`rqt`或`rviz`等ROS可视化工具实现...
visual_odometry:在KITTI数据集上进行视觉里程计，实现了单眼2D-2D和立体声2D-3D
05-16
视觉里程表 这是在上的OpenCV 3.4实现。 方法 2D-2D单眼VO和2D-3D立体声VO 实施细节 编译 在2目录中： mkdir build cd build cmake .. make 跑步 编译后，直接在构建中单核细胞增多症 ./vo [sequence ID] [dataset directory] 立体声 ./vo [sequence ID] [max frame] [optimize frame] [dataset directory] 表现
A-LOAM算法完全注释
05-18
本人(985控制硕士)花了一个月时间仔细研读了A-LOAM算法的3大核心模块：点云预处理(scanRegistration.cpp)、雷达里程计(laserOdometry.cpp)以及建图(laserOdometry.cpp)。并且给出了该算法每个模块乃至每句话的详细...
A-LOAM代码注释学习（四）laserOdometry.cpp
abanchao的博客
10-15 856
原理： 按照匀速运动假设，找到了一个衡量畸变的参数s，就是前面算出来的点的时间，保存在point.intensity的小数部分。也就是说，时间越晚，运动的时刻相对开始时刻越晚，产生的畸变越严重。在去畸变的时候，位移向量可以直接乘这个衡量参数s进行校正，而姿态四元数表征旋转，需要用slerp球面线性插值。思考： 先转换到初始点，因为可以依据匀速模型。 再转换到结束点，因为这个变化的位姿需要迭代优化。 t_last_curr表示current—>last的平移变化。 1.2 回调函数 这个节点的回调函数功能简单
五.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---3.laserOdometry.cpp--前端雷达里程计和位姿粗估计
weixin_36773706的博客
10-19 1301
laserOdometry这个节点订阅了5个话题：有序点云、极大边线点、次极大边线点、极小平面点、次极小平面点。发布了4个话题：有序点云、上一帧的平面点、上一帧的边线点、当前帧位姿粗估计。主要功能是前端的激光里程计和位姿粗估计。 我们先看主函数： int main(int argc, char **argv) { ros::init(argc, argv, "laserOdometry"); ros::NodeHandle nh; nh.param<int>("m
A-LOAM代码理解laserOdometry(二)
qq_20184333的博客
11-05 562
A-LOAM代码理解laserOdometry(二)
LOAM学习-代码解析（三）特征点运动估计 laserOdometry
weixin_44041199的博客
12-09 1150
LOAM学习-代码解析（三）特征点运动估计 laserOdometry前言一、初始化二、去除位移畸变 TransformToStart TransformToEnd三、去除角度畸变四、积累旋转量五、数据处理器六、主函数结语 前言 前两篇文章LOAM学习-代码解析（一）点云数据配准和LOAM学习-代码解析（二）点云数据配准 scanRegistration，对点云数据配准的源代码进行详细完整的即系，这里继续LOAM的代码解析工作。 LOAM代码(带中文注释)的地址：https://github.com/cu
LOAM 系列(3): laserOdometrynode 分析
weixin_40072161的博客
11-23 574
一、对应源文件 add_executable(alaserOdometry src/laserOdometry.cpp) target_link_libraries(alaserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) 二、代码分析 1. subscribe topic // sharp point clouds， 更新 cornerSharpBuf ros::Subscriber subCornerPo
rf2o_laser_odometry问题解决
m0_56357174的博客
02-15 844
rf2o_laser_odometry源码：GitHub - MAPIRlab/rf2o_laser_odometry: Estimation of 2D odometry based on planar laser scans. Useful for mobile robots with innacurate base odometry. For full description of the algorithm, please refer to: Planar Odometry from a Radia
移动机器人——里程计矫正
偷得浮生半日闲
11-22 2863
一、问题分析 机器人可以基于码盘数据和底盘运动学模型进行航迹推演，得到机器人的轨迹，但实际轨迹与推演轨迹存在误差，分析原因如下： 底盘实际尺寸与理论模型存在偏差，如轮子半径，两轮间距等； 底盘在运动过程中打滑或路面不平整导致偏差； 注意： 方向上的偏差较平移的偏差影响更大，会随着移动扩大误差。 码盘里程计受电控，底盘机械结构影响，在路况复杂和室外场景下，误差会较大。 解决方案 直接线性（此处使用该方法） 基于模型（可参考2d Laser 和 Odomter 内外参数标定工具原理及使用方法） 二、
LeGO-LOAM中地图配准的代码
05-11
pcl::transformPointCloud(*laserCloudCornerLast, *transformed_scan_ptr, laserOdometry); // Step 2: Add current scan to global map *laserCloudCornerFromMap += *transformed_scan_ptr; ...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值