A-loam学习笔记——laserOdometry 激光里程计
理论部分（待更新）（5月31日更新）
发布的主要包括分好类的点云以及里程计的位姿信息和路径的位置信息。
代码部分
#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_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <eigen3/Eigen/Dense>
#include <mutex>
#include <queue>
#include "aloam_velodyne/common.h"
#include "aloam_velodyne/tic_toc.h"
#include "lidarFactor.hpp"
#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;
bool systemInited = false;
double timeCornerPointsSharp = 0;
double timeCornerPointsLessSharp = 0;
double timeSurfPointsFlat = 0;
double timeSurfPointsLessFlat = 0;
double timeLaserCloudFullRes = 0;
pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<pcl::PointXYZI>());
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;
// Transformation from current frame to world frame
//当前帧在世界坐标系下的坐标
Eigen::Quaterniond q_w_curr(1, 0, 0, 0);
Eigen::Vector3d t_w_curr(0, 0, 0);
// q_curr_last(x, y, z, w), t_curr_last
//点云特征匹配时的优化变量
double para_q[4] = {0, 0, 0, 1};
double para_t[3] = {0, 0, 0};
Eigen::Map<Eigen::Quaterniond> q_last_curr(para_q);
Eigen::Map<Eigen::Vector3d> t_last_curr(para_t);
//存放数据的队列
//只能访问 queue<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;
// undistort lidar point
//去畸变
//其中根据运动模型对点云进行了去畸变
void TransformToStart(PointType const *const pi, PointType *const po)
{
//interpolation ratio 插值率
double s;
//如果有畸变
if (DISTORTION)
//pi->intensity - int(pi->intensity))这部分为旋转角占总角度的比值 * 0.1
//SCAN_PERIOD = 0.1
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);
}
// 之后的5个Handler函数为接受上游5个topic的回调函数，作用是将消息缓存到对应的queue中，以便后续处理。
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();
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "laserOdometry");
ros::NodeHandle nh;
nh.param<int>("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;
//循环频率位100 ros::spin = 循环 + spinOnce(可以控制接受速度) 控制回调函数接受速度
ros::Rate rate(100);
while (ros::ok())
{
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();
}
// 5个点云的时间同步
//循环系统，pop出第一个元素，front访问第一个元素给转化成pcl格式传回来
mBuf.lock();
cornerPointsSharp->clear();
//转化成pcl形式的数据
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
//第一帧不进行匹配，仅仅将cornerPointsLessSharp保存至laserCloudCornerLast
//将surfPointsLessFlat保存至laserCloudSurfLast
//为下次匹配提供target
//为啥从第二帧开始， 因为要把第一帧的信息输入到kdtree中，让后面的去匹配阿！！！！！不然kdtree报错
if (!systemInited)
{
systemInited = true;
std::cout << "Initialization finished \n";
}
else // 第二帧开始
{
int cornerPointsSharpNum = cornerPointsSharp->points.size();
int surfPointsFlatNum = surfPointsFlat->points.size();
//计时
TicToc t_opt;
点到线以及点到面的ICP，迭代2次
for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)
{
// 角点的误差项数量
corner_correspondence = 0;
//平面点误差项的数量
plane_correspondence = 0;
//ceres::LossFunction *loss_function = NULL;
//ceres建立误差函数
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)
{
// 将当前帧的corner_sharp特征点O_cur，从当前帧的Lidar坐标系下变换到上一帧的Lidar坐标系下
//（记为点O，注意与前面的点O_cur不同），
//以利于寻找corner特征点的correspondence
TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);
//在上一幀的降采样角点中搜索与 0 点最近邻点 即为 A
kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1;
//如果最近点的corner特征点之间距离的平方小于阈值，则A点有效
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
{
//保存索引
closestPointInd = pointSearchInd[0];
//保存最近点的id（线数）
int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;
// search in the direction of increasing scan line
//寻找点0的另外一个最近邻点（记为点B）
//顺着index的方向查找
//遍历所有点选择距离pointSeld的最近的点（除去上面的A点）
for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j)
{
// if in the same scan line, continue
//如果查找的线数小于最近点的id那么直接跳过
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;
//计算与pointSel的距离
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
//在另一个线数方向上进行搜索最近点 B
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;
}
}
}
//如果最小点id大于0 的两个最近点 A B 均有效 开始的默认值是-1
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++;
}
}
// find correspondence for plane features
//寻找平面特征点
for (int i = 0; i < surfPointsFlatNum; ++i)
{
//转化到初始坐标系，顺便去了畸变
TransformToStart(&(surfPointsFlat->points[i]), &pointSel);
//在kdtree中搜索目标特征点最近的那个点
kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
//在kdtree中搜索的那个点到目标点的距离如果小于一定的阈值，则把他定为点A
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
{
closestPointInd = pointSearchInd[0];
// get closest point's scan ID
//得到最近点的 id
int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);
//开始寻找另外两个点 B C
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;
//计算近点与pointSel的距离
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
//如果线数小于最近点线数，并且距离更近，那就用该点替换最近点 B
if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
// if in the higher scan line
//如果线数大于最近点线数，并且距离更近，那就用该点替换最近点 C
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;
}
}
//如果B 和 C 点符合要求则进行提取坐标
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++;
}
}
}
//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");
}
//配置优化器，开始优化
TicToc t_solver;
ceres::Solver::Options options;
//QR法
options.linear_solver_type = ceres::DENSE_QR;
//最大迭代次数
options.max_num_iterations = 4;
// 输出到cout
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;
//发布odom和path
// 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 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]);
}
}
//更新配准的source
//把当前帧赋值给上一帧
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);
//每5帧执行一次发布
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;
}
1046
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
