本文记录第二个函数laserOdometry.cpp，这一部分代码主要实现了高频的激光里程计。
理论部分可参考：
https://zhuanlan.zhihu.com/p/400014744
https://zhuanlan.zhihu.com/p/400014744下面对源码进行分析
一、主函数
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
int main(int argc, char **argv)
{
// 初始化ROS节点
ros::init(argc, argv, "laserOdometry");
ros::NodeHandle nh;
// 参数设置：每处理多少帧数据执行一次映射
int skipFrameNum;
nh.param<int>("mapping_skip_frame", skipFrameNum, 2);
//skipFrameNum: 这是一个在代码中定义的变量，用于存储从参数服务器获取到的值。如果找到了参数，存到skipFrameNum 中。
// 这是一个默认值。如果在参数服务器中找不到 "mapping_skip_frame" 参数，skipFrameNum 将被赋值为 2。
// 打印映射频率信息
printf("Mapping %d Hz \n", 10 / skipFrameNum);
// 订阅锐利角点的点云
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);
// 定义一个用于存储激光路径的变量
nav_msgs::Path laserPath;
// 帧计数器
int frameCount = 0;
// 设置循环频率
ros::Rate rate(100);
// ROS主循环
while (ros::ok())
{
// 处理回调函数
ros::spinOnce();
// 每帧处理逻辑...
// 等待循环周期结束
rate.sleep();
}
return 0;
}
这部分代码主要就是从scanRegistration中订阅其最后发布的5个话题进行回调函数，然后在while循环里使用每帧处理逻辑处理数据，这里提一下ros::spinOnce() ，其适用于需要执行额外任务的复杂节点。这些任务可能包括数据处理、决策制定、发布消息等。我们在别的代码里看到的ros::spin() 通常用于简单的ROS节点，这些节点的主要任务是监听并响应消息。
1.1回调函数
// 处理锐角点云消息回调函数
void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsSharp2)
{
// 上锁以确保对缓冲区的安全访问
mBuf.lock();
// 将接收到的锐角点云消息存储到相应的缓冲区中
cornerSharpBuf.push(cornerPointsSharp2);
// 解锁
mBuf.unlock();
}
//... 1.2ros::spinOnce()函数
ros::spinOnce()通常在while里，回调函数执行即返回，然后执行一次后续代码。
ros::Rate rate(100); // 设置为100Hz的频率,理想情况
while (ros::ok()) {
ros::spinOnce(); // 处理所有当前等待的消息回调
// 执行其他任务
// ...
rate.sleep(); // 等待，以保持设定的循环频率
}
二、进行前后帧匹配与位姿估计
目的是求出两帧之间的位姿变化
2.1数据检查和准备
while (ros::ok())
{
// 在ROS节点运行期间循环执行以下代码
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();
}
// 上锁以同步访问
mBuf.lock();
// 清空并从ROS消息转换为PCL点云
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();
先检查点云是否为空，然后检查时间戳是否对齐，最后是同步将ros格式转换为pcl
2.2寻找角点约束
// initializing
//作用是跳过第一帧
if (!systemInited)
{
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)
{
corner_correspondence = 0;
plane_correspondence = 0;
//ceres::LossFunction *loss_function = NULL;
//定义HUBER核函数减小外点权重，阈值0.1，残差大于阈值减小权重
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
//LocalParameterization自定义加法
ceres::LocalParameterization *q_parameterization =
new ceres::EigenQuaternionParameterization();
ceres::Problem::Options problem_options;
//定义CERES问题
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
//寻找角点约束
for (int i = 0; i < cornerPointsSharpNum; ++i)
{
//进行运动补偿
TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);
//调用KDTREE接口寻找这帧角点转换到上一帧后距离最近的角点
kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1;
//距离小于阈值有效
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
{
//取出ID
closestPointInd = pointSearchInd[0];
//取出线数
int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);
//DISTANCE_SQ_THRESHOLD = 25
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;
// 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
//线离得太远的也pass掉
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;
}
}
// 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;
}
}
}
//如果是有效点
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构建约束，调用lidarFactor.hpp里的类计算残差
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++;
}
}
ceres的构建和使用可以查阅官方文档学习Automatic Derivatives — Ceres Solver 使用他的自动求导接口需要自行构建一个类，写在src里的lidarFactor.hpp里了
这部分我注释写的比较细，总体流程大概是：
初始化检查：首先，代码检查系统是否已初始化。如果这是处理的第一帧数据（
systemInited为 false），系统将设置为已初始化状态，并输出一条初始化完成的消息。对于后续的帧，程序会继续执行特征匹配和位姿优化的步骤。准备数据：获取当前帧的锐角点（cornerPointsSharp）和平面点（surfPointsFlat）的数量。这些点是从原始点云数据中提取的特征点。
设置优化问题：为了优化激光雷达的位姿，使用 Ceres Solver 创建一个优化问题。设置 Huber Loss 函数以减少异常值（outliers）的影响，并应用四元数参数化（
EigenQuaternionParameterization）来处理旋转。角点特征匹配：对于每个锐角点，通过运动补偿将其转换到一个起始位置，然后使用 k-d树（
kdtreeCornerLast）在上一帧的数据中找到最近的角点。搜索邻近点：对于找到的每个近邻角点，搜索其在扫描线上相邻的其他角点，以形成角点对。搜索方向包括扫描线的增加方向和减少方向，以确保找到合适的匹配点。
构建优化问题：对于每一对有效的角点匹配，根据当前点和上一帧中的两个角点，创建一个代表边缘特征的残差块（通过
LidarEdgeFactor）。该残差块随后被添加到优化问题中。执行优化：以上步骤为每个角点特征执行两次。通过 Ceres Solver 对所有收集的残差块进行优化，以精细调整激光雷达的位姿估计。
2.3 寻找面点约束
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);
// 根据点的扫描线ID和距离选择合适的点
if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2) {
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
} 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);
// 根据点的扫描线ID和距离选择合适的点
if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2) {
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
} else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3) {
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
// 构建ceres优化问题
if (minPointInd2 >= 0 && minPointInd3 >= 0) { // 确保找到了有效的最近点
// 将当前点和找到的三个最近点转换为Eigen的Vector3d格式
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优化问题的代价函数，用于优化当前点与三个最近点形成的平面的关系
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("coner_correspondance %d, plane_correspondence %d \n", corner_correspondence, 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;
//调用求解器求解
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;
}
这里和角点做法基本相同，只不过选取的点多了一个，计算的变成了是点到面的距离。最后使用求解器求解，迭代4次
2.4发布里程计与点云、更新
TicToc t_pub;
// 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);
//发给rvzi
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);
if(0)
{...}
//更新当前帧为上一帧
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';
//加入KDTREE为下一帧循环作准备
kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
kdtreeSurfLast->setInputCloud(laserCloudSurfLast);
//进行一定的降频后发给后段
if (frameCount % skipFrameNum == 0)
{
frameCount = 0;
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());
//超过100ms,输出提示
if(t_whole.toc() > 100)
ROS_WARN("odometry process over 100ms");
frameCount++;
}
rate.sleep();
}
return 0;
}
三、点云去畸变函数
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;
}
// transform all lidar points to the start of the next frame
void TransformToEnd(PointType const *const pi, PointType *const po)
{
// undistort point first
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);
}这两个函数通常用于校正由于传感器运动引起的点云数据畸变。TransformToStart 将点云数据变换到扫描开始时的参考帧，而 TransformToEnd 则将数据变换到扫描结束时的参考帧。在使用KITTI数据集时并没有调用。
扫一扫
2941
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
