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