目录
一、代码注释
1.1 去畸变函数
原理：
按照匀速运动假设，找到了一个衡量畸变的参数s，就是前面算出来的点的时间，保存在point.intensity的小数部分。也就是说，时间越晚，运动的时刻相对开始时刻越晚，产生的畸变越严重。在去畸变的时候，位移向量可以直接乘这个衡量参数s进行校正，而姿态四元数表征旋转，需要用slerp球面线性插值。
思考：
先转换到初始点，因为可以依据匀速模型。
再转换到结束点，因为这个变化的位姿需要迭代优化。
t_last_curr表示current—>last的平移变化。
//（TransformToStart：将当前帧Lidar坐标系下的点云变换到上一帧Lidar坐标系下
void TransformToStart(PointType const *const pi, PointType *const po)
{
//interpolation ratio
double s; //插值比，根据时间，在这一帧里占的比率
if (DISTORTION) //判断是否失真
// intensity 实数部分存的是 scan上点的 id 虚数部分存的这一点相对这一帧起始点的时间差
// intensity的整体减去实数部分,就是时间差,那么除以周期,也就是时间占比了
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); //slerp函数(球面线性插值)
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);
}
1.2 回调函数
这个节点的回调函数功能简单，都是接收点云，并添加到队列中。添加前加锁，添加后解锁。
lock()和unlock()都来源于头文件mutex类。
// 操作都是送去各自的队列中，加了线程锁以避免线程数据冲突
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();
}
1.3主函数
主循环while (ros::ok())是主要内容，具体代码注释如下：
int main(int argc, char **argv)
{
ros::init(argc, argv, "laserOdometry");
ros::NodeHandle nh;
nh.param<int>("mapping_skip_frame", skipFrameNum, 2); //设定里程计的帧率，由launch设定，默认值为 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::Subscriber subRemovePoints = nh.subscribe<sensor_msgs::PointCloud2>("/laser_remove_points", 100, laserCloudFullResHandler);
//创建publisher
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); // 循环频率
//主函数的主循环
//主要是帧间位姿估计的过程，目标是希望找到位姿变换T，使得第k帧点云左乘T得到第k+1帧点云，或者说左乘T得到k+1帧点云的误差最小
while (ros::ok())
{
ros::spinOnce(); // 调用队列中第1个数据作为callback函数的参数
// 首先确保订阅的五个消息都有，有一个队列为空都不行
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();
}
// 分别将五个点云消息取出来，同时转成pcl的点云格式
mBuf.lock(); //数据多个线程使用，这里先进行加锁，避免线程冲突
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(); //数据取出来后进行解锁
TicToc t_whole; //计算整个激光雷达里程计的时间
// initializing
//主要用来跳过第一帧数据，直接作为第二帧的上一帧
if (!systemInited)
{
systemInited = true;
std::cout << "Initialization finished \n";
}
else //第二帧开始，特征点匹配、位姿估计
{
// 取出比较突出的特征点 数量，极大角点（2） 极小平面点（-1）
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;
// 定义一下ceres的核函数，使用Huber核函数来减少外点的影响，即去除outliers
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
// 由于旋转不满足一般意义的加法，因此这里使用ceres自带的local param
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特征点之间的关联
//构建一个非线性优化问题：以点i与直线lj的距离为代价函数，以位姿变换T(四元数表示旋转+位移t)为优化变量
for (int i = 0; i < cornerPointsSharpNum; ++i) //先进行角点的匹配
{
//对角点运动补偿去畸变
TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);
// 在上一帧所有 次极大边线点 构成的kdtree中寻找距离当前帧最近的一个点
kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1;
//如果最近邻的corner特征点之间距离平方小于阈值，则最近邻点有效
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
{
closestPointInd = pointSearchInd[0]; // 目标点对应的最近距离点的索引取出来
// 找到其所在线束id，线束信息是intensity的整数部分
int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;
// search in the direction of increasing scan line 沿扫描线增加的方向搜索
//在刚刚角点 id附近扫描线分别继续寻找最邻近点，目的是找到最近的角点，由于其按照线束进行排序，所以就是向上找
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;
//既不是同一根scan的，也不是很远的，只留下近的
// 计算和当前找到的角点之间的距离的 平方
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;
//代价函数CostFunction
// 先求出代价函数
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 查找平面特征的对应关系
//构建一个非线性优化问题：以点i与平面lmj的距离为代价函数，以位姿变换T(四元数表示旋转+t)为优化变量。
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];
// get closest point's scan ID 找到最近点的扫描线（scan）的ID
int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;
//额外再寻找两个点，要求一个点和最近点同一个scan，另一个是不同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;
}
}
// 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;
}
}
// 如果特征点面点，和它附近的三个点，一个点是角点的找法
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;
// 构建点到面的约束，构建cere的非线性优化问题。
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);
//输出寻找关联点消耗的时间 t_data
printf("data association time %f ms \n", t_data.toc());
// 如果总的线约束和面约束太少，就打印一下
if ((corner_correspondence + plane_correspondence) < 10)
{
printf("less correspondence! *************************************************\n");
}
// 调用ceres 优化求解 ，DENSE_QR分解方法，最大迭代次数4，不输出过程信息，优化报告存入summary
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()); //两次LM优化消耗的时间t_opt
// 这里的w_curr 实际上是 w_last，即上一刻的位姿
// 更新帧间匹配的结果，得到lidar odom位姿
//在现有t_w_curr和q_w_curr的基础上，修正t_last_curr和q_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 发布lidar里程计结果
// 创建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);
// 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
//将角点特征和平面特征变换到扫描终点
//去畸变，没有调用 1和0的没变换
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]);
}
}
// 位姿估计完毕之后，当前角点和平面点就变成了上一帧的边线点和平面点，把索引和数量都转移成last
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设置当前帧，用来下一帧lidar odom使用
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()); //输出 发布运行时间 t_pub
printf("whole laserOdometry time %f ms \n \n", t_whole.toc()); //输出 全过程运行时间 t_whole
if(t_whole.toc() > 100)
ROS_WARN("odometry process over 100ms");
frameCount++;
}
rate.sleep();
}
return 0;
}
1.4 Ceres优化
ceres使用流程：
-
创建优化问题与损失核函数
ceres::Problem problem; ceres::LossFunction *loss_function; // 损失核函数 loss_function = new ceres::HuberLoss(1.0); loss_function = new ceres::CauchyLoss(1.0); // 柯西核函数 -
在创建的problem中添加优化问题的优化变量 problem.AddParameterBlock()，该函数常用的重载有两个
void AddParameterBlock(double* values, int size); void AddParameterBlock(double* values, int size, LocalParameterization* local_parameterization);
在lidarFactor.cpp里，laserOdometry.cpp主要用到两个ceres求解，分别是边缘约束和面约束。
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_) {} //s当前点的时间戳占比
template <typename T>
bool operator()(const T *q, const T *t, T *residual) const
{
// 将特征点的坐标xyz进行数据格式转换，double数组转成eigen的数据结构
Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};
Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};
// 将初始位姿数据格式进行转换，double数组转成eigen的数据结构
//Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};
Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
// 计算的是上一帧到当前帧的位姿变换，因此根据匀速模型，计算该点对应的位姿
q_last_curr = q_identity.slerp(T(s), q_last_curr);
Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};
Eigen::Matrix<T, 3, 1> lp;
// 把当前点根据当前计算的帧间位姿变换到上一帧的坐标系
lp = q_last_curr * cp + t_last_curr;
//都是向量，用点的位姿构成了向量
Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb); // cross()函数表示叉乘运算，求得平行四边形面积
Eigen::Matrix<T, 3, 1> de = lpa - lpb; //点 a，b的连线是底边
// 残差的模是该点到底边的垂线长度，012三个的数构成残差向量
//residual = nu.norm() / de.norm 可以用一个值表示，但是却构建成了三维向量
residual[0] = nu.x() / de.norm(); //norm()函数表示向量2-范数，模
residual[1] = nu.y() / de.norm();
residual[2] = nu.z() / de.norm();
return true;
}
static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,
const Eigen::Vector3d last_point_b_, const double s_)
{
return (new ceres::AutoDiffCostFunction<
LidarEdgeFactor, 3, 4, 3>( //这里的第1个3是指的三维残差,然后的4是四元数的参数块维度,之后的3是平移向量的参数块维度.
new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));
}
Eigen::Vector3d curr_point, last_point_a, last_point_b;
double s;
};
struct LidarPlaneFactor //平面约束
{
LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,
Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_)
: curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_),
last_point_m(last_point_m_), s(s_)
{
// 求出平面单位法向量
ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);
ljm_norm.normalize(); //法向量单位化
}
template <typename T>
bool operator()(const T *q, const T *t, T *residual) const
{
Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
Eigen::Matrix<T, 3, 1> lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())};
//Eigen::Matrix<T, 3, 1> lpl{T(last_point_l.x()), T(last_point_l.y()), T(last_point_l.z())};
//Eigen::Matrix<T, 3, 1> lpm{T(last_point_m.x()), T(last_point_m.y()), T(last_point_m.z())};
Eigen::Matrix<T, 3, 1> ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())};
//Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};
Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
// 根据时间戳进行插值
q_last_curr = q_identity.slerp(T(s), q_last_curr);
Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};
Eigen::Matrix<T, 3, 1> lp;
lp = q_last_curr * cp + t_last_curr;
// 点到平面的距离
residual[0] = (lp - lpj).dot(ljm);
return true;
}
static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_,
const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_,
const double s_)
{
return (new ceres::AutoDiffCostFunction<
LidarPlaneFactor, 1, 4, 3>( //第1个1是指的一维残差,然后的4是四元数的参数块维度,之后的3是平移向量的参数块维度.
new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_)));
}
Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;
Eigen::Vector3d ljm_norm;
double s;
};
二、重要内容解析
2.1 spinOnce()和spin()区别
作用：
- 当spinOnce()被调用时，调用队列中第1个数据作为callback函数的参数；等到下次spinOnce()被调用时，队列中第2个数据被使用，以此类推。这会有一个问题。因为队列的长度是有限的，如果发布器发送数据的速度太快，spinOnce函数调用的频率太少，就会导致队列溢出。
- spin()，一旦进入spin函数，它就不会返回了，相当于它在自己的函数里面死循环了。只要回调队列里面有数据在，它就会马上去执行callback函数。如果没有的话，它就会阻塞，不会占用CPU。意思是不需要循环while，只要队列里有数据就会一直执行。
区别：
- spin() 在调用后不会再返回，也就是你的主程序到这儿就不往下执行了；spinOnce()在调用后还可以继续执行之后的程序。
- 这就决定了scanRegistration的回调函数长，主函数短；而laserOdometry的回调函数短，主函数长。当然这跟实现的功能有关。
参考链接: ros::spin() 和 ros::spinOnce()机制作用和区别
2.2 pop()和push()区别
pop()取出队列最后一个元素，并删除，可以减少内存占用。
参考链接: C++ push_back()和back() 、pop()、push()、emplace() 和 emplace_back()
2.3 残差residual理解
理想情况下，解算出的位姿q，t可以对应于每一对cornerpoint和sufrpoint，但是现实情况下，不能解算出一个变换矩阵使得两帧点云能够完全对应，也就是说，在lidar发射点的光源稳定的情况下，无法找到一个完美变换，使得当前所有点云能和刚才扫描的点云进行完全对应。所以，计算点到线和点到面的距离作为优化计算的residual合情合理，通过两次迭代以达到最优。
理解：
对于cornerpoint，加上它匹配到的两个点，三点构成一个平面，降到二维进行想象和理解。两个向量的叉乘的模表示所构成的平行四边形的面积，说三角形面积的话正好是平行四边形的一半。
那么，residual=nu.norm() / de.norm()，也就是cornerpoint到底边的高，代码里把residual按照xyz三个维度分解构成了一个三维向量，本人认为有些没必要，一个数就可以解决，而且surfpoint高维也就用的一个数。
参考链接: 点云模型的训练，三维空间基础，移动最小二乘法应用、Hole Filling、点云平滑重建滤波，位姿估计
2.4 四元数球面线性插值Slerp
参考链接: 球面线性插值方法（Slerp）推导
参考链接: 四元数姿态平滑推导
2.5 尚存问题
1、pcl::KdTreeFLANN <pcl::PointXYZ>I::Ptr
2、Eigen::Map<Eigen::Quaterniond>
3、Ceres自己使用
470
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
