自动驾驶系列(4)ALOAM源码解析-laserOdometry.cpp

1原理

利用激光雷达数据进行机器人的位姿估计。具体来说,它通过订阅激光雷达的不同类型点云话题,获取尖锐角点、不尖锐角点、平坦平面和不平坦平面等信息。然后,对这些点云数据进行处理和关联,利用 Ceres 库进行优化求解,估计机器人的位姿变换。最后,将位姿信息发布到 ROS 话题中,供其他节点使用。

  • 点云数据处理:代码首先对订阅到的点云数据进行处理,包括去畸变、坐标变换等操作。
  • 特征提取:从点云中提取尖锐角点和平坦平面等特征,并建立相应的 Kd 树用于快速搜索最近邻点。
  • 对应关系建立:通过遍历特征点,利用 Kd 树搜索最近邻点,并根据距离和扫描线信息建立对应关系。
  • 位姿优化:使用 Ceres 库构建优化问题,将对应关系作为约束,通过最小化误差函数来求解机器人的位姿变换。
  • 位姿发布:将优化得到的位姿信息发布到 ROS 话题中

2 实现

2.1去畸变函数

先转换到初始点,依据匀速模型。
再转换到结束点,迭代优化。

// 对激光雷达点进行去畸变
void TransformToStart(PointType const *const pi, PointType *const po)
{
    // 插值比例
    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;
}

// 将所有激光雷达点变换到下一帧的开始
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();

    // 去除畸变时间信息
    po->intensity = int(pi->intensity);
}


2.2主函数

功能:

  • main 函数:
  • 初始化 ROS 节点。
  • 订阅激光雷达点云话题。
  • 创建 ROS 发布者,发布激光里程计、激光路径和处理后的点云数据。
  • 在主循环中,处理接收到的点云数据。
  • 如果系统尚未初始化,则进行初始化操作。
  • 否则,进行点云数据的关联和优化。
  • 发布激光里程计和路径。
  • 转换和发布处理后的点云数据。
  • 统计时间并输出警告信息。
    (1)订阅和发布的话题
    // 订阅尖锐角点话题
    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);

    // 发布全分辨率激光云话题
    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);

(2)点云数据的关联和优化
利用Ceres库(一个非线性优化器)解决基于激光雷达数据的形状配准问题。首先,它初始化了一些必要的变量和对象,然后进入一个循环,针对边缘点和平面点分别进行配准。同时在每个循环中,它首先找到当前点在上一帧激光雷达数据中的 nearest neighbor,然后根据扫描ID的差异来搜索更近或更远的点。

// 寻找平面特征的对应关系
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)
    {
        closestPointInd = pointSearchInd[0];

        // 获取最近点的扫描 ID
        int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);
        double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;

        // 在扫描线增加的方向搜索
        for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j)
        {
            // 如果不在附近扫描,结束循环
            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);

            // 如果在同一或更低扫描线,更新 minPointSqDis2
            if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)
            {
                minPointSqDis2 = pointSqDis;
                minPointInd2 = j;
            }
            // 如果在更高扫描线,更新 minPointSqDis3
            else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
            {
                minPointSqDis3 = pointSqDis;
                minPointInd3 = j;
            }
        }

        // 在扫描线减少的方向搜索
        for (int j = closestPointInd - 1; j >= 0; --j)
        {
            // 如果不在附近扫描,结束循环
            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);

            // 如果在同一或更高扫描线,更新 minPointSqDis2
            if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2)
            {
                minPointSqDis2 = pointSqDis;
                minPointInd2 = j;
            }
            // 如果在更低扫描线,更新 minPointSqDis3
            else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3)
            {
                minPointSqDis3 = pointSqDis;
                minPointInd3 = j;
            }
        }

        if (minPointInd2 >= 0 && minPointInd3 >= 0)
        {
            Eigen::Vector3d curr_point(surfPointsFlat->points[i].x,
                                        surfPointsFlat->points[i].y,
                                        surfPointsFlat->points[i].z);
            Eigen::Vector3d last_point_a(laserCloudSurfLast->points[closestPointInd].x,
                                            laserCloudSurfLast->points[closestPointInd].y,
                                            laserCloudSurfLast->points[closestPointInd].z);
            Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x,
                                            laserCloudSurfLast->points[minPointInd2].y,
                                            laserCloudSurfLast->points[minPointInd2].z);
            Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x,
                                            laserCloudSurfLast->points[minPointInd3].y,
                                            laserCloudSurfLast->points[minPointInd3].z);

            double s;
            if (DISTORTION)
                s = (surfPointsFlat->points[i].intensity - int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD;
            else
                s = 1.0;
            ceres::CostFunction *cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s);
            problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
            plane_correspondence++;
        }
    }
}
// 优化两次
for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)
{
    // 重置对应点数量
    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;
    // 寻找角点特征的对应关系
    for (int i = 0; i < cornerPointsSharpNum; ++i)
    {
        TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);
        kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);

        int closestPointInd = -1, minPointInd2 = -1;
        if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
        {
            closestPointInd = pointSearchInd[0];
            int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);

            double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;
            // 搜索在扫描线增加的方向
            for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j)
            {
                // 如果在同一扫描线,继续
                if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)
                    continue;

                // 如果不在附近扫描,结束循环
                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)
                {
                    // 找到更近的点
                    minPointSqDis2 = pointSqDis;
                    minPointInd2 = j;
                }
            }

            // 搜索在扫描线减少的方向
            for (int j = closestPointInd - 1; j >= 0; --j)
            {
                // 如果在同一扫描线,继续
                if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID)
                    continue;

                // 如果不在附近扫描,结束循环
                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)
                {
                    // 找到更近的点
                    minPointSqDis2 = pointSqDis;
                    minPointInd2 = j;
                }
            }
        }

        // 找到角点特征的对应关系
        if (minPointInd2 >= 0) // 两个最近点都有效
        {
            Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x,
                                       cornerPointsSharp->points[i].y,
                                       cornerPointsSharp->points[i].z);
            Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x,
                                           laserCloudCornerLast->points[closestPointInd].y,
                                           laserCloudCornerLast->points[closestPointInd].z);
            Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x,
                                           laserCloudCornerLast->points[minPointInd2].y,
                                           laserCloudCornerLast->points[minPointInd2].z);

            double s;
            if (DISTORTION)
                s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD;
            else
                s = 1.0;
            ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
            problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
            corner_correspondence++;
        }
    }

    // 寻找平面特征的对应关系
    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)
        {
            closestPointInd = pointSearchInd[0];

            // 获取最近点的扫描 ID
            int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);
            double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;

            // 搜索在扫描线增加的方向
            for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j)
            {
                // 如果不在附近扫描,结束循环
                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);

                // 如果在同一或更低扫描线,更新 minPointSqDis2
                if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)
                {
                    minPointSqDis2 = pointSqDis;
                    minPointInd2 = j;
                }
                // 如果在更高扫描线,更新 minPointSqDis3
                else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
                {
                    minPointSqDis3 = pointSqDis;
                    minPointInd3 = j;
                }
            }

            // 搜索在扫描线减少的方向
            for (int j = closestPointInd - 1; j >= 0; --j)
            {
                // 如果不在附近扫描,结束循环
                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
  • 5
    点赞
  • 4
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
(messenger-libusb.cpp:42) control_transfer返回了错误,索引: 768,错误。 这个错误是指在messenger-libusb.cpp文件中的第42行,执行了一个名为control_transfer的函数,但该函数返回了错误。错误信息显示索引为768,但具体的错误原因并没有提供。 要解决这个问题,我们可以进行以下几步: 1. 检查索引是否超出了范围。索引通常用于引用某种数据或操作对象,如果超过了其有效范围,就会导致错误。检查代码中对该索引的使用,并确保其在合理的范围内。 2. 检查传递给control_transfer函数的参数是否正确。函数可能需要特定的参数或特定的格式。查看函数的文档或源代码,确保传递给它的参数是正确的。 3. 检查是否需要更新或更改相关的库文件。错误可能是由于库文件中的bug或不兼容性引起的。查看最新版本的库文件,并尝试更新或更改它们来修复错误。 4. 使用调试工具进行调试。通过使用调试器或日志记录器等工具,可以跟踪代码的执行过程,以便更容易发现错误所在。可以逐步执行代码并观察变量的值,以找出导致错误的原因。 5. 寻求帮助。如果以上方法都没有解决问题,可以向开发人员或相关论坛或社区寻求帮助。提供具体的错误信息和相关的代码片段,以便他们更好地理解问题并提供解决方案。 总之,错误(messenger-libusb.cpp:42) control_transfer返回了错误,索引: 768,错误表明在执行control_transfer函数时出现了问题。我们需要检查索引范围、参数正确性、库文件更新、调试等方面,以找出并解决问题。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值