TransformToStart（变换到起始点后的位置，去畸变）
// undistort lidar point
// 将当前帧点云转换到上一帧，根据运动模型对点云去畸变
//（TransformToStart：将当前帧Lidar坐标系下的点云变换到上一帧Lidar坐标系下（也就是当前帧的初始位姿，起始位姿，所以函数名是TransformToStart），
// 因为kitti点云已经去除了畸变，所以不再考虑运动补偿。（如果点云没有去除畸变，用slerp差值的方式计算出每个点在fire时刻的位姿，
// 然后进行TransformToStart的坐标变换，一方面实现了变换到上一帧Lidar坐标系下；
// 另一方面也可以理解成点都将fire时刻统一到了开始时刻，即去除了畸变，完成了运动补偿）
// 功能函数的名字 : TransformToStart
// 形参 传入的指针 pi是输入点的点云地址 po是转换后的输出点的点云地址
void TransformToStart(PointType const *const pi, PointType *const po)
{
//interpolation ratio
double s;
if (DISTORTION)
// 相减得到 pi 相对于该扫描线起始点的时间差*扫描周期，
// 除以扫描周期得到相对于该扫描线起始点的时间差
s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;
else
s = 1.0;
//s = 1;
// 线性插值得到当前点相对于该帧起始点的旋转，插值范围为：[I, q_last_curr]
// s = 0 得到 I,表示相对于起始点没有旋转，s = 1 表示至结束点，旋转就是 last to current 的旋转
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;
}
laserOdometry主函数
laserOdometry 节点的主要作用是作为 a loam 的里程计对激光雷达的位姿进行快速的估计，大致过程为：
- 从 scanRegistraion 中接收预处理后的点云以及各个特征点点云，并将其存放在相应的缓冲队列中
- 每一次从各个缓冲队列中获取最早的一帧点云以及相应的特征点
- 对每一个特征点（平面点/角点），在上一帧中找到相应的特征点及其构成的点，对角点来说是两个点构成一个角点特征；对平面点来说是三个点构成一个平面
- 将当前分析的特征点以及找到的匹配特征点作为观测值加入 ceres 问题中构建残差
- 对所有观测值进行优化
- 更新世界坐标系下的位姿以及发布更新后的点云地图
int main(int argc, char **argv)
{
// 初始化里程计节点
ros::init(argc, argv, "laserOdometry");
ros::NodeHandle nh;
// 获取参数，表示建图间隔的帧数，默认为 2，意思是每三帧建一次图
nh.param<int>("mapping_skip_frame", skipFrameNum, 2);
printf("Mapping %d Hz \n", 10 / skipFrameNum);
// 初始化预处理后的点云以及各个特征点的 subscriber
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);
// 接收scanRegistration过滤掉空值和距离原点过近的点云数据
ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100, laserCloudFullResHandler);
// 初始化 publishers，分别发布上一帧的角点以及平面点，还有转发全分辨率的原始点云（按需转换至点云末尾时刻）
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;
// 100ms 执行一次，频率为 10 Hz
ros::Rate rate(100);
while (ros::ok())
{
// 只触发一次回调，所以每次都要调用一次；等待回调函数执行完毕，执行一次后续代码
// ROS的主循环中，程序需要不断调用ros::spin() 或 ros::spinOnce()
// 两者区别在于前者调用后不会再返回，也就是你的主程序到这儿就不往下执行了，而后者在调用后还可以继续执行之后的程序。
// 这里需要特别强调一下，如果程序写了相关的消息订阅函数，那千万千万千万不要忘了在相应位置加上ros::spin()或者ros::spinOnce()函数，
// 不然永远都得不到另一边发出的数据或消息的
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出现丢包会导致这种情况
ROS_BREAK();
}
// 对 buffer 加锁防止其他线程（subscriber）在对 buffer 进行操作
mBuf.lock();
// 提取最旧一帧的特征点并转为 pcl 点云格式
cornerPointsSharp->clear();
pcl::fromROSMsg(*cornerSharpBuf.front(), *cornerPointsSharp);// 消息类型转换为点云
cornerSharpBuf.pop();//pop()：删除 queue 中的第一个元素
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)
{
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 使用流程
1、创建优化问题与损失核函数
ceres::Problem problem;
ceres::LossFunction *loss_function; // 损失核函数
loss_function = new ceres::HuberLoss(1.0);
loss_function = new ceres::CauchyLoss(1.0); // 柯西核函数
2、在创建的problem中添加优化问题的优化变量 problem.AddParameterBlock()，该函数常用的重载有两个
void AddParameterBlock(double* values, int size);
void AddParameterBlock(double* values,
int size,
LocalParameterization* local_parameterization);
*/
//ceres::LossFunction *loss_function = NULL;
// 定义一下ceres的核函数，使用Huber核函数来减少外点的影响
// 定义ceres的Huber损失函数0.1代表残差大于0.1的点,则权重降低(2√s - 1,s > 0.1).小于0.1则认为正常,不做特殊的处理
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
// 由于旋转不满足一般意义的加法，因此这里使用ceres自带的local param
// 例如当采用四元数表示位姿时，由于四元数本身的约束（模长为1），实际的自由度为3而非4。
// 此时，若直接传递四元数进行优化，冗余的维数会带来计算资源的浪费，需要使用Ceres预先定义的 QuaternionParameterization 对优化参数进行重构;
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;// 计算寻找关联点的时间
// 构建一个非线性优化问题：以点i与直线lj的距离为代价函数，以位姿变换T(四元数表示旋转+位移t)为优化变量。
for (int i = 0; i < cornerPointsSharpNum; ++i)
{
// 将点的位姿转换至这一帧开始的时间点(去畸变，同一帧下方便计算)
TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);
// 在上一帧的角点中找到离当前点最近的一个点，参数分别为nearestKSearch（基准点，取几个离基本点最近的点，K个点的下标，K个点相对于基准点的距离）
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);
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;
// 从选择的最近的点往后遍历（扫描线 id 增加的方向）
// 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（相邻只包含前后的相邻2条，第三条开始就不包括在内，退出循环）
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;
}
}
}
// 保证 minPointInd2 有效的同时保证了 closestPointInd 有效
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;
/*
将点 a, b, p 以及畸变因子构建 loss 函数跟旋转和平移参数构建残差块传入 ceres 问题中
- 计算残差的方法和论文基本一致：
- 按需对点 p 进行去畸变处理，这里去畸变的做法是假定在一帧点云中激光雷达做匀速运动，因此对旋转和平移进行线性插值即可，
对于更多的去畸变方法可以参考：[激光雷达测量模型及运动畸变去除](https://xiaotaoguo.com/p/lidar-model-distortion-removal/)，
如果使用的是类似 Kitti 数据集之类的已经校正过的点云则不要去畸变
- 计算 pa 和 pb 的叉乘，表示三角形 abp 的面积，然后除以 ab 即可得到距离
所有特征点的残差块加入后优化即可。
*/
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++;
}
}
// 对当前帧的平面点一一进行分析
// find correspondence for plane features
for (int i = 0; i < surfPointsFlatNum; ++i)
{
// 将当前平面点位姿转换至当前帧起始时间
TransformToStart(&(surfPointsFlat->points[i]), &pointSel);
// 在上一帧的平面点中找到距离当前点最近的点
kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
// 重置上一帧中找到的三个构成平面的点的索引
// 其中， minPointInd2 用来存储在选择的点之后（扫描线 ID 更大）的点
// minPointInd3 存储在选择的点之前（扫描线 ID 更小）的点
int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
{
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(其实只能为同一根扫描线，后面的点不可能是之前扫描线的，记录为2)
if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
// if in the higher scan line（如果是后面的非同一条线的最近点，记录为3）
else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
{
minPointSqDis3 = pointSqDis;
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;
}
}
// 保证了 minPointInd2 和 minPointInd3 有效的同时也保证了 closestPointInd 有效
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;
// 将当前帧的平面点 p，以及上一帧构成平面的点 a,b,c 还有畸变因子 s 加入优化器中计算残差
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());
// 如果总的符合要求的点太少，就输出一下
if ((corner_correspondence + plane_correspondence) < 10)
{
printf("less correspondence! *************************************************\n");
}
// 对位姿进行优化使得残差最小
// 调用ceres求解器求解，设定求解器类型，最大迭代次数，不输出过程信息，优化报告存入summary
TicToc t_solver;
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;// 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());// 循环做了两遍，经过两次LM优化消耗的时间
// 使用优化后的 q_last_curr， t_last_curr 更新激光雷达位姿
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 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消息是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);
/*
这里有一点不太清楚的地方是作者将 `TransformToEnd` 这一步将当前帧点云转换到结束时刻的位姿的部分注释掉了，即默认是不进行这一操作的，
但是论文里应该是有这一操作的，因为在我们对特征点匹配的时候，我们是将特征点转换到当前帧起始时间，即上一帧的结束时间的。
能想到的一个解释是对于 Kitti 等数据集中，其发布的点云经过校正后本来就是对应当前帧结束时刻的位姿，但是需要看数据集确定。
*/
// transform corner features and plane features to the scan end point
// why don't run this process?
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]);
}
}
// 用当前的角点和平面点点云更新上一帧信息（将全部筛选出来的边缘点和平面点变为下一步的数据，而之前的距离筛选出来的数据已经优化反应到里程计中了）
// 位姿估计完毕之后，当前边线点和平面点就变成了上一帧的边线点和平面点，把索引和数量都转移过去
// 利用临时变量laserCloudTemp交换
// 用cornerPointsLessSharp和surfPointsLessFlat更新laserCloudCornerLast和laserCloudSurfLast以及相应的kdtree，为下一次点云特征匹配提供target
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';
kdtreeCornerLast->setInputCloud(laserCloudCornerLast);//设置kdtreeCornerLast输入点云为上一帧的less_sharp
kdtreeSurfLast->setInputCloud(laserCloudSurfLast);//设置 kdtreeSurfLast输入点云为上一帧的less_flat
// 按给定频率发布当前的特征点点云以及全分辨率点云
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());
if(t_whole.toc() > 100)
ROS_WARN("odometry process over 100ms");
frameCount++;
}
rate.sleep();
}
return 0;
}
非线性优化
T
(
k
+
1
,
i
)
L
T_{(k+1,i)}^L
T(k+1,i)L是在这一圈中某个时刻的位姿变换，
T
k
+
1
L
T_{k+1}^L
Tk+1L是结束时刻的位姿变换，意思就是假设旋转和平移都是匀速的，所以模型可以简化成下面这样，时间和位姿变换是同时变化的（对应的是函数TransformToStart）
假设 X ( k + 1 , i ) L X_{(k+1,i)}^L X(k+1,i)L为投影之前的i点坐标， X ~ ( k + 1 , i ) L \widetilde{X}_{(k+1,i)}^L X (k+1,i)L为投影到起始点 t k + 1 t_{k+1} tk+1之后的i点坐标，则两者之间存在如下关系。
接着求点线距和点面距就可以简化成一个公式（
T
(
k
+
1
,
i
)
L
T^L_{(k+1,i)}
T(k+1,i)L为
T
k
+
1
L
T^L_{k+1}
Tk+1L的一个因变量，公式中）
解得下面公式
采用LM迭代求解该非线性优化问题，求得最优解d。
在这里我理解的T就是取了一个位姿变化使得这一个子区域中的2个边缘点和4个平面点前后两帧变化最小，综合考虑这6个点的结果。这个最小的d就代表最理想的匹配情况。
464
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
