LOAM 系列(3): laserOdometrynode 分析

已于 2022-04-05 19:40:54 修改
阅读量568 收藏 2
点赞数 2
分类专栏： # lidar_slam 文章标签： slam
于 2021-11-23 22:46:37 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_40072161/article/details/121504652
版权

一、对应源文件

add_executable(alaserOdometry src/laserOdometry.cpp)
target_link_libraries(alaserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES})

二、原理

1. 点云符号表示

 时间段内接收到的点云：t_k-t_{k+1} ：P_k

投影到终止时刻 Lidar 坐标系：\bar{P_k}

投影到初始时刻 Lidar 坐标系：\tilde{P_k}（用当前估计的 pose）

边缘点云：\varepsilon _k

平面点云：H_k

2. 特征关联

2.1 线特征

输入：

        点云集合 \tilde{\varepsilon }_{k+1}，从中取一个点 i

        匹配点：点云集合 \bar{P}_k

步骤：

        step1：从 \bar{P}_k 中找到 i的最近邻点 j 

        step2：从 j 的相邻扫描束中找 i 的最近邻点 l  （因为直线在一个扫描束上只可能有一个点云）

        step3：检查 j, l 的平滑度

 输出：

        i <----> (j, l)

2.2 平面特征

 输入：

        点云集合 \tilde{H}_{k+1}，从中取一个点 i

        匹配点：点云集合 \bar{P}_k

步骤：

        step1：从 \bar{P}_k 中找到 i的最近邻点 j 

        step2：从 j 的同一个扫描束中找 i 的最近邻点 l  ， 从 j 的相邻扫描束中找 i 的最近邻点 m

        step3：检查 j,l,m 的平滑度

 输出：

        i <----> (j, l, m)

2.3 图示

3. 误差项构造

3.1 线特征

i\in \tilde{\varepsilon }_{k+1}

(j, l) \in \bar{P}_k

点到直线距离

3.2 平面特征

i\in \tilde{H }_{k+1}

(j, l, m) \in \bar{P}_k

点到平面距离

4. 运动补偿

 4.1 问题引入

（1）在一个 sweep 期间，不同时刻 Lidar 坐标系不一样，需要线性插值，来估计不同时刻的位姿

（2）待求解 delta pose 是一个 sweep 周期 的 R, T，而每组观测来源于不同时刻，所以需要引入运动补偿，将观测统一变换至扫描起始时刻 Lidar 坐标系下

4.2 公式推导

时间周期：[t_{k+1}, t], delta pose: T^L_{k+1}

                    T^L_{k+1}=[t_x, t_y, t_z, \theta_x, \theta_y, \theta_z]^T

这儿用旋转矢量对旋转部分进行建模

中间时刻 pose: 

                     T^L_{k+1, i}=\frac{t_i-t_{k+1}}{t-t_{k+1}}T^L_{k+1}， t_i \in [t_{k+1}, t]

t_i 时刻点的坐标  X^L_{k+1, i} 变换至 sweep 起始时刻  \tilde{X}^L_{k+1, i}

                     X^L_{k+1, i}=R \tilde{X}^L_{k+1, i} + T^L_{k+1, i}(1:3)

                   

                

                  

 5. 结合运动补偿的误差构造

               

               

              

Levenberg-Marquardt method

            

  6. 算法流程

输入：

        上一个周期的点云：\bar{P}_k

        当前周期实时累加的点云：P_{k+1}

        上次迭代求解的 [t_{k+1}, t] 之间 delta pose: T^L_{k+1}

输出：

        最新计算的 delta pose : T^L_{k+1}

返回：

        If the algorithm reaches the end of a sweep, P_{k+1} is reprojected to time stamp t_{k+2} using the estimated motion during the sweep. Otherwise, only the transform T^L_{k+1} is returned for the next round of recursion

 三、ROS interface

1.1 subscribe topic

// sharp point clouds， 更新  cornerSharpBuf
ros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100, laserCloudSharpHandler);
// less_sharp point clouds，更新 cornerLessSharpBuf
ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100, laserCloudLessSharpHandler);
// flat surface，更新 surfFlatBuf
ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100, laserCloudFlatHandler);
// less flat surface，更新 surfLessFlatBuf
ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100, laserCloudLessFlatHandler);
// all point cloud，更新 fullPointsBuf
ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100, laserCloudFullResHandler);

1.2. publish topic

// last corner pts
ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 100);
// last flat pts
ros::Publisher pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 100);
// all pts
ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 100);
// odom
ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry>("/laser_odom_to_init", 100);
// path
ros::Publisher pubLaserPath = nh.advertise<nav_msgs::Path>("/laser_odom_path", 100);

1.3. 缓存队列

// sharp corners
cornerSharpBuf.push(cornerPointsSharp2);
// less sharp corners
cornerLessSharpBuf.push(cornerPointsLessSharp2);
// flat surface
surfFlatBuf.push(surfPointsFlat2);
// less flat surface
surfLessFlatBuf.push(surfPointsLessFlat2);
// all pointclouds
fullPointsBuf.push(laserCloudFullRes2);

四、算法实现

1. 点云预处理

// 算法输入：
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 laserCloudFullRes(new pcl::PointCloud<PointType>());

// 1. 时间戳同步检查
......
// 2. 转换为 pcl 点云
......

2. factor 构造

src/lidarFactor.hpp

2.1 LidarEdgeFactor

todo

2.2 LidarPlaneFactor

todo

3. 两次优化迭代

3.1 构造 problem

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);
// 状态变量：
// double para_q[4] = {0, 0, 0, 1};
// double para_t[3] = {0, 0, 0};
problem.AddParameterBlock(para_q, 4, q_parameterization);
problem.AddParameterBlock(para_t, 3);

3.2 运动补偿

变换至起始帧下

TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);

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;
    }
 
    // R_start_i
    Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);
    // t_start_i
    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;
}

关于如何从 intensity 中获取相对时间：

LOAM 系列(2): ascanRegistration node 分析_scofield_ly的博客-CSDN博客

3.3 线特征

(1). 输入点  i ，运动补偿去畸变变换至起始帧下

TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);

(2). kdtree 搜索找到最近点  j

kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);

closestPointInd = pointSearchInd[0];
// 并获取对应扫描线束号
int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);

(3). 查找 j 扫描线束相邻线束的最近邻点 l

(closestPointScanID, closestPointScanID + NEARBY_SCAN)

// search in the direction of increasing scan line
for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j) {
    // if in the same scan line, continue
    if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID) {
        continue;
    }
    // if not in nearby scans, end the loop
    if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN)) {
        break;
    }
    // 点云距离
    double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * 
                        (laserCloudCornerLast->points[j].x - pointSel.x) +
                        (laserCloudCornerLast->points[j].y - pointSel.y) * 
                        (laserCloudCornerLast->points[j].y - pointSel.y) + 
                        (laserCloudCornerLast->points[j].z - pointSel.z) * 
                        (laserCloudCornerLast->points[j].z - pointSel.z);

   if (pointSqDis < minPointSqDis2) {
        // find nearer point
        minPointSqDis2 = pointSqDis;
        minPointInd2 = j;
    }
}

(closestPointScanID, closestPointScanID - NEARBY_SCAN)

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

(4). 添加残差

if (minPointInd2 >= 0)  { // both closestPointInd and minPointInd2 is valid
    Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x,
                               cornerPointsSharp->points[i].y,
                               cornerPointsSharp->points[i].z);
    Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x,
                                 laserCloudCornerLast->points[closestPointInd].y,
                                 laserCloudCornerLast->points[closestPointInd].z);
    Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x,
                                 laserCloudCornerLast->points[minPointInd2].y,
                                 laserCloudCornerLast->points[minPointInd2].z);

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

3.4. 面特征

(1). 输入点  i ，运动补偿去畸变变换至起始帧下

同线特征

(2). kdtree 搜索找到最近点  j

 同线特征，只不过 kd tree 用的是 kdtreeSurfLast

 (3). 查找 j 相同线束的最近邻点 l，以及相邻线束的最近邻点 m

 (closestPointScanID, closestPointScanID + NEARBY_SCAN)

// search in the direction of increasing scan line
for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j) {
    // if not in nearby scans, end the loop
    if (int(laserCloudSurfLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
        break;

   double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
                       (laserCloudSurfLast->points[j].x - pointSel.x) +
                       (laserCloudSurfLast->points[j].y - pointSel.y) *
                       (laserCloudSurfLast->points[j].y - pointSel.y) +
                       (laserCloudSurfLast->points[j].z - pointSel.z) *
                       (laserCloudSurfLast->points[j].z - pointSel.z);

    // if in the same or lower scan line
    if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2) {
        minPointSqDis2 = pointSqDis;
        minPointInd2 = j;
    }
    // if in the higher scan line
    else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3) {
        minPointSqDis3 = pointSqDis;
        minPointInd3 = j;
    }
}

 (closestPointScanID, closestPointScanID - NEARBY_SCAN)

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

(4). 添加残差

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

3.5. ceres 求解

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

4. 依据帧间位姿更新当前帧位姿

t_w_curr = t_w_curr + q_w_curr * t_last_curr;
q_w_curr = q_w_curr * q_last_curr;

5. publish odometry && path

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

6. 重新初始化 kdtree

kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
kdtreeSurfLast->setInputCloud(laserCloudSurfLast);

五、参考

LOAM-SLAM原理深度解析 - 知乎

scofield_ly
关注
  • 2
    点赞
  • 踩
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
rf2o_laser_odometry问题解决
m0_56357174的博客
02-15 818
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
srf_laser_odometry:基于对称距离流和多扫描对齐的鲁棒平面里程计
05-30
srf_laser_odometry 基于对称距离流和多扫描对齐的鲁棒平面里程计 该 pkgs 提供了基于平面激光扫描的 2D 里程计的快速可靠估计。 SRF 是的延续， 是一种快速而精确的方法，可通过连续距离扫描来估计激光雷达的平面运动。 SRF 提出了一种使用激光扫描仪估计平面运动的密集方法。 从两次扫描之间的几何一致性的对称表示开始，我们得出精确的范围流量约束，并根据扫描仪的刚性运动来表达扫描观测值的运动。 与将传入扫描与前一个或最后一个选择的键扫描对齐的现有技术相比，我们提出了一种组合且有效的公式，以在每次迭代时联合对齐所有这三个扫描。 这种新公式保留了基于键扫描的策略的优点，但对键扫描的次优选择和移动对象的存在更加稳健。 这种方法的一个广泛的评估，提出在静态和动态环境模拟和真实数据。 结果表明，在所有进行的实验中，我们的方法都比现有方法快一个数量级，并且准确性大大提高。 运
LOAM laserOdom
zc的博客
01-16 122
LOAM laserOdom
laserOdometry
qq_37497304的博客
03-24 267
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 doub
map odom base_link和laser坐标系
BoriseLee
11-19 1897
转载：http://blog.sina.com.cn/s/blog_8a2281f70102xwc0.html 1. 前提：        本文讨论的是在已有地图的情况下，进行导航的情形。   2. 坐标系出现的原因：        为了实现机器人在环境中的自主定位而设定的。要让机器人实现自主定位，机器人就需要有一个先验的地图，可以经过前期使用slam_gmapping功能实现。   ...
KITTI数据、LOAM系列代码
05-19
KITTI数据以及LOAM系列代码（轨迹评估改），需配合我的相关博客应用
SC-Lego-LOAM：LiDAR SLAM：扫描上下文+ LeGO-LOAM
01-28
SC-Lego-LOAM：LiDAR SLAM：扫描上下文+ LeGO-LOAM
LIO-SAM-DetailedNote:LIO-SAM源码详细注释，3D SLAM融合激光、IMU、GPS
05-08
关于LIO-SAM的论文解读，网上已经有很多文章啦，同系列的LOAM、A-LOAM、LEGO-LOAM等，在网上都可以找到相关的解读文章。所以本文旨在对源代码进行阅读学习，积累一些工程上的经验。这里记录下来，希望可以帮到有需要...
matlab注销代码-A-LOAM:A-LOAM
05-25
可以在A-LOAM/analysis/ntuviral_aloam文件夹中找到A-LOAM/analysis/ntuviral_aloam 。 只需运行MATLAB脚本checkall_parallel.m ，结果将被打印出来。 如果您在工作中使用NTU VIRAL数据集，请使用数据集处的BibTex...
SF_loam:语义融合壤土
03-31
Sfusion_loam 语义融合壤土。
A-LOAM(后端2)地图中的线面特征提取及优化问题构建-算法流程+代码注释
zysss_的博客
08-15 1857
在后端的当前帧和地图匹配的时候，我们就需要从地图⾥寻找线特征和⾯特征的约束对，此时，由于没有了线束信息，我们就需要采取额外的操作来判断其是否符合线特征和⾯特征的给定约束。注意这里的A、B、C不是之前的ABC，A=A/D···，下式就是上式整体除以D，下式也就是三个未知数，五个⽅程（也是找5个点），写成矩阵的形式就是⼀个5×3⼤⼩的矩阵，求出结果之后，我们还需要对结果进⾏校验，来观察其是否符合平⾯约束，具体就是分别求出5个点到求出平⾯的距离，如果太远，则说明该平⾯拟合不成功。，这里利用平面方程构建约束。...
LOAM中Ceres的使用分析
啥也不懂的研究生也淦SLAM的博客
10-14 1364
LOAM中Ceres的使用分析，包括aloam和floam
aloam中的ceres-solver
yoga_wyj的博客
11-27 2034
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.
KD-Tree原理详解
惊鸿一博
02-24 7413
目录 1、简介 2、原理 2.1、数据结构 2.2、构建kd-tree 例子 2.3、最近邻检索 3、常用函数 4、参考文献 kd-tree是在进行激光点云编程中经常使用的一个工具，我们平时使用的时候可能就是使用其中的一小部分的内容，并未对其进行很深的思考。那么kd-tree的原理到底是什么呢？我们又经常使用哪些常用函数呢？ 本片文章将对kd-tree进行细致的整理，方便以后的使用和阅读。 1、简介 kd-tree简称k维树，是一种空间划分的数据结构。常被用于高维空间中的搜索，比如范
A-loam学习笔记——laserOdometry 激光里程计
weixin_51326570的博客
04-20 1005
a-loam学习笔记——laserOdometry 激光里程计 理论部分（待更新） 代码部分 先看主函数在看回调函数哦 #include <cmath> #include <nav_msgs/Odometry.h> #include <nav_msgs/Path.h> #include <geometry_msgs/PoseStamped.h> #include <pcl/point_cloud.h> #include <pcl/point_
激光SLAM第二期学习笔记
weixin_39458342的博客
02-21 1335
第二章传感器数据处理 第1节 里程计运动模型及标定 根据相对位置求绝对位姿： 若为二维点，[x, y, 1] = [R, t; 0, 1]*[del_x, del_y, 1]，这里的xy表示的点是没有方向的 若为带方向的三维点，上述公式是没法用的，不能用向量而应该用位姿矩阵来表示T = T0*delta_T; 记住：带方向的位姿，一定先转化成T再进行运算 [R, t; 0,...
A loam odometry中的数据关联(最近临搜索 KDtree）
weixin_44916682的博客
10-13 197
1
LOAM_velodyne学习（二）
热门推荐
沧海一升的博客
07-05 1万+
LaserOdometry 这一模块（节点）主要功能是：进行点云数据配准，完成运动估计 利用ScanRegistration中提取到的特征点，建立相邻时间点云数据之间的关联，由此推断lidar的运动。我们依旧从主函数开始： int main(int argc, char** argv) { ros::init(argc, argv, "laserOdometry"); ros::N...
A-LOAM线/面特征雅可比解析求导
dushib的博客
01-04 1884
雅可比矩阵推导 线特征： 核心思想：点到直线距离最小。通过平行四边形面积除以对角线长度 面特征 核心思想：使得点到面距离最短 线/面特征雅可比矩阵解析求导实现 需要修改的代码为 include/lidar_localization/models/loam/aloam_factor.hpp 源代码利用直接求导的方式定义面特征与线特征自动求导结构体 struct LidarEdgeFactor struct LidarPlaneFactor...
LeGO-LOAM原理分析
最新发布
06-16
LEGO-LOAM（Lidar-based Elastic Graph Optimization for Monocular SLAM）是一种结合了激光雷达和单目相机的SLAM（同时定位和建图）方法，由Li et al. 在2013年提出。它将两者的优点相结合，提高了SLAM系统的鲁棒性...

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值