根据之前LOAM论文阅读那篇文章，进一步对ALOAM的代码进行阅读分析：
一、scanRegistration.cpp
对应launch里的节点：ascanRegistration
这部分代码用来筛选点云提取4种特征点：边缘点特征sharp、less_sharp，面特征flat、less_flat
首先main函数：
main函数主要订阅话题topic，当接收到一帧点云就调用一次回调函数laserCloudHandler处理接收到的点云。
//接收激光雷达信号
ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, laserCloudHandler);
//全部点云
pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100);
//角点
pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100);
//降采样角点
pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100);
//平面点
pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100);
//发布除了角点之外全部点 经过了滤波处理
pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100);
//去除点
pubRemovePoints = nh.advertise<sensor_msgs::PointCloud2>("/laser_remove_points", 100);几个提前定义的变量：scanPeriod:扫描周期，velodyne频率10Hz，周期0.1s；
const double scanPeriod = 0.1; //扫描周期，velodyne频率10Hz，周期0.1s
const int systemDelay = 0; //弃用前systemDelay帧初始数据
int systemInitCount = 0; //用于计数过了多少帧
bool systemInited = false; //超过systemDelay后，systemInited为true即初始化完成
int N_SCANS = 0; //激光雷达线数 初始化为0
float cloudCurvature[400000]; //单帧16线扫描的点云的曲率，400000为一帧点云中点的最大数量
int cloudSortInd[400000]; //曲率点对应的序号
int cloudNeighborPicked[400000]; //用于标记已处理点（点是否筛选过：0 未筛选过；1 筛选过）
int cloudLabel[400000]; //赋予点云特征标签，用于将点云特征分类 对于cloudLabel：赋予点云特征标签，把点云特征进行分类。2：代表曲率很大；1：代表曲率比较大；0：代表曲率比较小；-1：代表曲率很小。其中1包含2，0包含-1，0和1构成了点云全部的点。
定义一些发布点云话题的Publisher：
ros::Publisher pubLaserCloud; //整体点云
ros::Publisher pubCornerPointsSharp; //角点
ros::Publisher pubCornerPointsLessSharp; //降采样角点
ros::Publisher pubSurfPointsFlat; //面点
ros::Publisher pubSurfPointsLessFlat; //降采样面点
ros::Publisher pubRemovePoints; //剔除点
std::vector<ros::Publisher> pubEachScan; //ros形式的一线扫描定义了一个removeClosedPointCloud函数，作用是去除过远点。
void removeClosedPointCloud(const pcl::PointCloud<PointT> &cloud_in, //去除过远点
pcl::PointCloud<PointT> &cloud_out, float thres)
{
if (&cloud_in != &cloud_out) //统一header（时间戳）和size
{
cloud_out.header = cloud_in.header;
cloud_out.points.resize(cloud_in.points.size());
}
size_t j = 0;
//逐点距离比较
for (size_t i = 0; i < cloud_in.points.size(); ++i)
{//如果在这个范围内，跳出这次循环，下一个点进入循环
if (cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y + cloud_in.points[i].z * cloud_in.points[i].z < thres * thres)
continue;
cloud_out.points[j] = cloud_in.points[i]; //如果不在这个范围，正常执行
j++;
}
if (j != cloud_in.points.size()) //如果有点剔除，size发生改变
{
cloud_out.points.resize(j); //重新定义size
}
cloud_out.height = 1;
cloud_out.width = static_cast<uint32_t>(j);
cloud_out.is_dense = true;
}定义一个回调函数laserCloudHandler：作用是提取并发布4种特征点，laser_cloud_sharp、laser_cloud_less_sharp、laser_cloud_flat、laser_cloud_less_flat，方便之后的过程使用。
先计算当前帧点云的第一个point和第二个point的旋转角（两者差值约束在Pi-3Pi范围内）。
其中atan2(y,x):y,x是数字（y坐标和x坐标的比例），用于计算y/x的反正切的主值。范围为-pi到+pi，负号是因为激光雷达是顺时针旋转，而角度逆时针才为正。
int cloudSize = laserCloudIn.points.size(); //定义该次扫描的点数（计算点云点的数量）
//atan2的范围-pi到+pi，负号是因为激光雷达是顺时针旋转，而角度逆时针才为正
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x); //当前帧起始角
float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
laserCloudIn.points[cloudSize - 1].x) +
2 * M_PI; //当前帧终止角
//保证激光扫描的范围在pi-3pi
if (endOri - startOri > 3 * M_PI)
{
endOri -= 2 * M_PI;
}
else if (endOri - startOri < M_PI)
{
endOri += 2 * M_PI;
}
//printf("end Ori %f\n", endOri);遍历当前扫描帧的全部点，读取每个点的坐标，计算每个点的scanID和该点相对时间（旋转了多少，对应的时间）
//遍历当前扫描帧的全部点，读取每个点的坐标
for (int i = 0; i < cloudSize; i++)
{//坐标轴传化
point.x = laserCloudIn.points[i].x;
point.y = laserCloudIn.points[i].y;
point.z = laserCloudIn.points[i].z;
//计算垂直俯仰角
float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI; //16线激光雷达垂直视场为-15到15
int scanID = 0; //激光帧序号
if (N_SCANS == 16) //如果为16线激光，进入
{
//0.5为了四舍五入 int只能保留整数部分，angle的范围为-pi/2到pi/2，
// /2是每两个扫描之间差2度,angle范围为-15到15,带入求出scanID范围为0-15
scanID = int((angle + 15) / 2 + 0.5);
if (scanID > (N_SCANS - 1) || scanID < 0) //如果在16以上或者为负数，总点数-1结束本次循环
{
count--;
continue;
}
}
else if (N_SCANS == 32)
{
scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);
if (scanID > (N_SCANS - 1) || scanID < 0)
{
count--;
continue;
}
}
else if (N_SCANS == 64)
{
if (angle >= -8.83)
scanID = int((2 - angle) * 3.0 + 0.5);
else
scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);
// use [0 50] > 50 remove outlies
if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
{
count--;
continue;
}
}
else
{
printf("wrong scan number\n");
ROS_BREAK();
}
//printf("angle %f scanID %d \n", angle, scanID);
float ori = -atan2(point.y, point.x); //计算这个点的水平旋转角度
if (!halfPassed) //判断是否扫描一半，进行补偿，保证扫描终止起始扫描范围在pi-2pi
{
//如果没过半，选择起始位置进行差值计算，来补偿，确保-pi/2 < ori - startOri < 3*pi/2
if (ori < startOri - M_PI / 2)
{
ori += 2 * M_PI;
}
else if (ori > startOri + M_PI * 3 / 2)
{
ori -= 2 * M_PI;
}
if (ori - startOri > M_PI)
{
halfPassed = true;
}
}
else //如果过半，选择终止位置进行差值计算，来补偿，确保-3*pi/2 < ori - endOri < pi/2
{
ori += 2 * M_PI;
if (ori < endOri - M_PI * 3 / 2)
{
ori += 2 * M_PI;
}
else if (ori > endOri + M_PI / 2)
{
ori -= 2 * M_PI;
}
}
float relTime = (ori - startOri) / (endOri - startOri); //旋转的角度/整个周期角度，看旋转了多大
point.intensity = scanID + scanPeriod * relTime;//一个整数+一个小数，整数部分是线号（0-15），小数部分是相对时间（旋转了多少，对应的时间），
//scanPeriod为0.1，也就是一圈0.1s，小数部分不会超过0.1,完成了按照时间排序的需求
laserCloudScans[scanID].push_back(point); //把当前点，放入对应的scanid的线上
}计算每一个点的曲率
for (int i = 5; i < cloudSize - 5; i++)
{
float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;
float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;
float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;
cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ; //曲率大小
cloudSortInd[i] = i; //序号
cloudNeighborPicked[i] = 0; //没被选择过的点，设置为0，后续被选为特征点后，将被设置成1
cloudLabel[i] = 0; //曲率的分类
//Label 2：corner_sharp曲率大（角点）
//Label 1：corner_less_sharp, 包含Label 2（曲率稍微小的点，降采样角点）
//Label -1：surf_flat(平面点)
//Label 0：surf_less_flat， 包含Label -1，因为点太多，最后会降采样
}然后通过一个的for loop把每一个scan分为6个大块，将计算出的曲率进行大小比较，提取出四种特征点：
for (int i = 0; i < N_SCANS; i++) //按照scan的顺序，提取四种特征点
{
if( scanEndInd[i] - scanStartInd[i] < 6)//如果点数少于6个，则跳过，下一个扫描进来，确保将每个扫描进行6等分
continue;
pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
//将每个scan的曲率点分成6等份处理,确保周围都有点被选作特征点,或者说每两行都有
for (int j = 0; j < 6; j++)
{
int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6; //起始索引，六等分起点
int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;//结束索引，六等分终点，-1为了避免重复
TicToc t_tmp;
std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);//利用sort，按照其曲率大小从小到大排序
t_q_sort += t_tmp.toc();
//筛选每个分段，曲率很大和比较大的点
int largestPickedNum = 0;
for (int k = ep; k >= sp; k--) //倒序查找，曲率从小到大
{
int ind = cloudSortInd[k];
//如果没有被选择过，并且曲率较大
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > 0.1)
{
largestPickedNum++; //曲率大的点计数+1
if (largestPickedNum <= 2) //曲率大的前两个点，最多为两个
{
cloudLabel[ind] = 2; //label=2，定义为角点
cornerPointsSharp.push_back(laserCloud->points[ind]);
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
//曲率最大的前20个点定义为降采样角点，包括角点
else if (largestPickedNum <= 20)
{
cloudLabel[ind] = 1; //label=1，降采样角点
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
else
{
break;
}
cloudNeighborPicked[ind] = 1; //该点被选择处理过了，标志设为1
//将曲率比较大的点的前五个距离比较近的点筛选出去，防止特征点的聚集
for (int l = 1; l <= 5; l++)
{ //与当前点距离的平方小于等于0.05的点标记为选择过，避免特征点密集分布
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
//否则，距离的平方小于等于0.05,为比较近的点，标记为1,也就是选择处理过
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
}
}
//提取曲率比较小的点（平面点）
int smallestPickedNum = 0;
for (int k = sp; k <= ep; k++)//顺序查找，曲率从小到大
{
int ind = cloudSortInd[k];
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] < 0.1) //如果曲率比较小，并且没有被筛选过
{
cloudLabel[ind] = -1; //label=-1，为平面点
surfPointsFlat.push_back(laserCloud->points[ind]);
smallestPickedNum++;
if (smallestPickedNum >= 4) //选取曲率最小的前4个点
{
break;
}
cloudNeighborPicked[ind] = 1; //该点被选择处理过了，标志设为1
for (int l = 1; l <= 5; l++) //防止特征点聚集，将距离比较近的点筛选出去
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)//距离的平方小于等于0.05的被设置为1，也就是选择过，不再使用
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
}
}
//目前选出了曲率最大的前20个点认为是corner_less_sharp(包含corner_sharp)，选出了曲率最小的4个点认为是surf_flat
//将剩余的点（包括之前被排除的点）全部归入平面点中less flat类别中
for (int k = sp; k <= ep; k++)
{
if (cloudLabel[k] <= 0)
{
surfPointsLessFlatScan->push_back(laserCloud->points[k]);
//最初将所有点都标记为0了，见 cloudLabel[i] = 0;
}
}
}
//最后对该scan点云中提取的所有surf_less_flat特征点进行降采样（体素栅格滤波器），执行结果放入到scanDS
pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
pcl::VoxelGrid<PointType> downSizeFilter; //体素网格滤波
downSizeFilter.setInputCloud(surfPointsLessFlatScan); //设置需要降采样处理的点云
downSizeFilter.setLeafSize(0.2, 0.2, 0.2); //设置滤波时创建的体素体积
downSizeFilter.filter(surfPointsLessFlatScanDS); //处理结果放入DS
surfPointsLessFlat += surfPointsLessFlatScanDS; //每次汇总降采样处理之后的点云
}
printf("sort q time %f \n", t_q_sort); //输出分类排序的时间
printf("seperate points time %f \n", t_pts.toc()); //输出降采样的时间
最后进行点云格式转换，把提取出的四种特征点发布出去。
二、laserOdometry.cpp
launch文件中对应的节点名字：alaserOdometry。
里达里程计代码实现把上一步提取到的四种特征点进行特征匹配的过程。
首先定义了几个重要变量：
//扫描周期
constexpr double SCAN_PERIOD = 0.1;//constexpr:常量表达式。建议凡是「常量」语义的场景都使用 constexpr，只对「只读」语义使用 const
constexpr double DISTANCE_SQ_THRESHOLD = 25;//后面要进行距离比较的参数
constexpr double NEARBY_SCAN = 2.5;//找点进行匹配优化时的线数距离(13线-10线>2.5就break介样用)
int skipFrameNum = 5; //发给建图算法的频率5hzEigen::Quaterniond q_w_curr(1, 0, 0, 0);//旋转四元数q
Eigen::Vector3d t_w_curr(0, 0, 0);//位移t
-
//点云特征匹配时的待优化变量， 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};
//下面两个分别是优化变量para_q和para_t的映射：表示的是两个world坐标系下的位姿P之间的增量，例如，△P = P0.inverse() * P1
Eigen::Map<Eigen::Quaterniond> q_last_curr(para_q);//Eigen::Map可以理解为映射/引用
Eigen::Map<Eigen::Vector3d> t_last_curr(para_t);先接收来自scanRegistration的点云信息：
//上一帧边缘点（曲率较大的点）构成kd树
pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<pcl::PointXYZI>());
//上一帧平面点（曲率较小的点）构成kd树
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>());main函数前出现的几个函数：
TransformToStart：将当前帧Lidar坐标系下的点云变换到上一帧Lidar坐标系下
void TransformToStart(PointType const *const pi, PointType *const po)
{
//interpolation ratio
//如果点云没有去除畸变，用slerp差值的方式计算出每个点在fire时刻的位姿，然后进行TransformToStart的坐标变换，一方面实现了变换到上一帧Lidar坐标系下；
//另一方面也可以理解成点都将fire时刻统一到了开始时刻，即去除了畸变，完成了运动补偿
double s;
if (DISTORTION)
s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD; //SCAN_PERIOD=0.1，intensity的整数部分存放scanID，小数部分存放归一化后的fireID，int强转向下取整。因此s=fireID/0.1
else
s = 1.0;
//s = 1;
Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);//Eigen::Map<Eigen::Quaterniond> q_last_curr(para_q)，表示坐标变换的旋转增量
Eigen::Vector3d t_point_last = s * t_last_curr;//Eigen::Map<Eigen::Vector3d> t_last_curr(para_t)，表示坐标变换的位移增量
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;
}TransformToEnd：将输入点转为下一帧下的坐标
void TransformToEnd(PointType const *const pi, PointType *const po)
{
// undistort point first
pcl::PointXYZI un_point_tmp;
TransformToStart(pi, &un_point_tmp); //pi转到上一帧Lidar坐标系下，转为un_point_tmp
Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z);//un_point_tmp转为un_point向量
Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr);//Eigen::Map<Eigen::Vector3d> t_last_curr(para_t)
po->x = point_end.x();
po->y = point_end.y();
po->z = point_end.z();
//Remove distortion time info
po->intensity = int(pi->intensity);
}之后是五个Handler回调函数，这五个函数接收5个topic的回调函数，将消息缓存到对应的queue中，方便后续处理。接收的5个topic分别为sharp点云、less_sharp点云、flat点云、less_flat点云、全部点云。
//receive sharp pointcloud
void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsSharp2)
{
mBuf.lock();//互斥锁被锁定。线程申请该互斥锁，如果未能获得该互斥锁，则调用线程将阻塞(block)在该互斥锁上；如果成功获得该互诉锁，该线程一直拥有该互斥锁直到调用unlock解锁；
//如果该互斥锁已经被当前调用线程锁住，则产生死锁(deadlock)。
cornerSharpBuf.push(cornerPointsSharp2);//将corner_sharp点加入到cornerSharpBuf中
mBuf.unlock();//解锁，释放调用线程对该互斥锁的所有权。
}
//receive less_sharp pointcloud
void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsLessSharp2)
{
mBuf.lock();
cornerLessSharpBuf.push(cornerPointsLessSharp2);//将corner_less_sharp点加入到CornerLessSharpBuf中
mBuf.unlock();
}
//receive flat pointcloud
void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsFlat2)
{
mBuf.lock();
surfFlatBuf.push(surfPointsFlat2);//将surf_flat点加入到surfFlatBuf中
mBuf.unlock();
}
//receive less_flat pointcloud
void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsLessFlat2)
{
mBuf.lock();
surfLessFlatBuf.push(surfPointsLessFlat2);//将surf_less_flat点加入到surfLessFireBuf中
mBuf.unlock();
}
//receive all point cloud
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2)
{
mBuf.lock();
fullPointsBuf.push(laserCloudFullRes2);//将所有点laserCloud点加入到fullPointsBuf中
mBuf.unlock();
}
接下来为main函数：
同步消息过程：
if (!cornerSharpBuf.empty() && !cornerLessSharpBuf.empty() &&
!surfFlatBuf.empty() && !surfLessFlatBuf.empty() &&
!fullPointsBuf.empty())
{
//记录四种特征点云中的获取到的第一个点的时间戳
timeCornerPointsSharp = cornerSharpBuf.front()->header.stamp.toSec();
timeCornerPointsLessSharp = cornerLessSharpBuf.front()->header.stamp.toSec();//queue.front() 返回第一个元素
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();
cornerPointsSharp->clear();//清空cornerPointSharp中的所有点云
pcl::fromROSMsg(*cornerSharpBuf.front(), *cornerPointsSharp);//将ROS系统接收到的cornerSharpBuf中的首元素（一帧点云）转存到cornerPointsSharp
cornerSharpBuf.pop();//删除cornerSharpBuf中的首元素
cornerPointsLessSharp->clear();
pcl::fromROSMsg(*cornerLessSharpBuf.front(), *cornerPointsLessSharp);//将CornerLessSharpBuf中的首元素转存到CornerPointsLessSharp
cornerLessSharpBuf.pop();//删除CornerLessSharpBuf中的首元素
surfPointsFlat->clear();
pcl::fromROSMsg(*surfFlatBuf.front(), *surfPointsFlat);//将surfFlatBuf中的首元素转存到surfPointsFlat
surfFlatBuf.pop();//删除surfFlatBuf的首元素
surfPointsLessFlat->clear();//将surfLessFlatBuf中的首元素转存到surfPointsLessFlat
pcl::fromROSMsg(*surfLessFlatBuf.front(), *surfPointsLessFlat);
surfLessFlatBuf.pop();//删除surfLessFlatBuf的首元素
laserCloudFullRes->clear();
pcl::fromROSMsg(*fullPointsBuf.front(), *laserCloudFullRes);//将fullPointsBuf中的首元素转存到laserCloudFullRes
fullPointsBuf.pop();//删除fullPointsBuf的首元素
mBuf.unlock();
//这三句：.clear()、.fromROSmsg与.pop()都处在循环之中，每次循环都会清空A，将B首元素添加到A，再删除B的首元素，这样就实现了B的遍历并且保证每次A中只有一个元素接下来就是最核心的部分：帧间匹配
for (int i = 0; i < cornerPointsSharpNum; ++i)//对于每个corner_sharp点,注意是++i
{
TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);//将当前帧的corner_sharp特征点从当前帧的Lidar坐标系下变换到上一帧的Lidar坐标系下（作为kdTree的查询点）
kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);// kdtree中的点云是上一帧的corner_less_sharp，
//所以这是在上一帧的corner_less_sharp中寻找当前帧corner_sharp特征点O的最近邻点（记为A）
//pointSel：查询点；1：邻近个数；pointSearchInd：储存搜索到的近邻点的索引；pointSearchSqDis：储存查询点与对应近邻点中心距离平方
int closestPointInd = -1, minPointInd2 = -1;
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)//如果最近邻的corner特征点之间距离平方小于阈值(25)，则最近邻点A有效
{
closestPointInd = pointSearchInd[0];
int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity); //强转int向下取整，表示scanID
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;
//向上搜索，寻找另外一个最近邻的点（记为B1）， search in the direction of increasing scan line
for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j)
//laserCloudCornerLast 来自上一帧的corner_less_sharp特征点，由于提取特征点时是按照scan的顺序提取的，
//所以laserCloudCornerLast中的点也是按照scanID递增的顺序存放的
{
// if in the same scan line, continue
if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)//intensity整数部分存放的是scanID
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) //第二个最近邻点有效，更新点B1
{
// find nearer point
minPointSqDis2 = pointSqDis;
minPointInd2 = j;//找到一个像样的点B1，缩小阈值，继续搜索更近的点B来替换上一轮搜索的B，直到遍历所有的laserCloudCornerLast，即上一帧的corner_less_sharp点
}
}
}
// 向下搜索（scanID减小的方向），寻找点0的另外一个最近邻的点B2，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)//第二个最近邻点有效，更新点B
{
// find nearer point
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
}
if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid
{ // 即特征点0的两个最近邻点A和B都有效
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, //上一帧中搜索到的近邻点A
laserCloudCornerLast->points[closestPointInd].y,
laserCloudCornerLast->points[closestPointInd].z);
Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x, //上一帧中搜索到的近邻点B
laserCloudCornerLast->points[minPointInd2].y,
laserCloudCornerLast->points[minPointInd2].z);
double s; //运动补偿系数
if (DISTORTION)
s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD;//fireID/01
else
s = 1.0;
// 用0，A，B构造点到线的距离的残差，注意这三个点都是在上一帧的Lidar坐标系下，即，残差=点0到直线AB的距离
//具体计算方法在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++;
}
}
//下面说的点符号与上述相同
//与上面建立corner特征点之间的关联类似，寻找平面特征点的最近邻点ABC（只找3个最近邻点），即基于最近邻原理建立surf特征点之间的关联
// find correspondence for plane features
for (int i = 0; i < surfPointsFlatNum; ++i)
{
TransformToStart(&(surfPointsFlat->points[i]), &pointSel);//将当前帧的corner_sharp特征点从当前帧的Lidar坐标系下变换到上一帧的Lidar坐标系下（作为kdTree的查询点）
kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);// kdtree中的点云是上一帧的corner_less_sharp，所以这是在上一帧
//pointSel：查询点；1：邻近个数；pointSearchInd：储存搜索到的近邻点的索引；pointSearchSqDis：储存查询点与对应近邻点中心距离平方
int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)//如果最近邻的corner特征点之间距离平方小于阈值(25)，则找到的最近邻点A有效
{
closestPointInd = pointSearchInd[0];
// 向上搜索，寻找另外一个最近邻的点（记为B1）， get closest point's scan ID
int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);//强转int向下取整，表示scanID
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
if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;//找到的第2个最近邻有效，更新B，注意如果scanID准确的话，一般A与B的scanID相同
minPointInd2 = j;
}
// if in the higher scan line
else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
{
minPointSqDis3 = pointSqDis;//找到的第3个最近邻有效，更新点C，注意如果scanID准确的话，一般A与B的scanID相同，且与点C的scanID不同
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) //A、B、C都有效
{
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, //上一帧中的近邻点A
laserCloudSurfLast->points[closestPointInd].y,
laserCloudSurfLast->points[closestPointInd].z);
Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x, //上一帧中的近邻点B
laserCloudSurfLast->points[minPointInd2].y,
laserCloudSurfLast->points[minPointInd2].z);
Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x, //上一帧中的近邻点C
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;
//用点0，A，B，C构造点到面的距离的残差，注意这三个点都是在上一帧的Lidar坐标系下，即残差=点0到平面ABC的距离
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;
options.linear_solver_type = ceres::DENSE_QR; //QR分解
options.max_num_iterations = 4; //迭代数
options.minimizer_progress_to_stdout = false;
ceres::Solver::Summary summary;
//基于构造的所有残差项，求解最优的当前帧位姿与上一帧位姿的位姿增量：para_q和para_t
ceres::Solve(options, &problem, &summary);
printf("solver time %f ms \n", t_solver.toc());
}
printf("optimization twice time %f \n", t_opt.toc());
//经过2次点到线以及点到面的ICP点云配准之后，得到最优的位姿增量para_q和para_t，即，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;
}上面的过程经过两次点到线及点到面的ICP匹配，得到最优的位姿增量para_q和para_t，即，q_last_curr和t_last_curr。
然后就是发布 根据odometry计算出来的当前帧在世界坐标系的位姿、corner_less_sharp特征点、surf_less_flat特征点、滤波后的点云
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);用cornerPointsLessSharp和surfPointsLessFlat更新laserCloudCornerLast和laserlaserCloudSurfLast以及相应的kdtree，为下一次点云特征匹配提供target
//将cornerPointsLessSharp与laserCloudCornerLast交换,目的保存cornerPointsLessSharp的值下轮使用
pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
cornerPointsLessSharp = laserCloudCornerLast;
laserCloudCornerLast = laserCloudTemp;//将上一节点中获取到的less_sharp点云赋给laserCloudCornerLast，，这就是为啥//277行//第一帧不进行匹配，仅仅将CornerPointsLessSharp保存到laserCloudCornerLast，
//将surfPointLessFlat保存至laserCloudSurfLast，为下次匹配提供target
//将surfPointLessFlat与laserCloudSurfLast交换，目的保存surfPointsLessFlat的值下轮使用
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';
//使用上一帧的特征点构建kd-tree
kdtreeCornerLast->setInputCloud(laserCloudCornerLast);//所有的边沿点集合
kdtreeSurfLast->setInputCloud(laserCloudSurfLast);//所有的平面点集合每五帧发布一次话题给下一个建图过程：
if (frameCount % skipFrameNum == 0)//提取关键帧，间隔四帧提取一次，即每五帧发布一次话题用于建图
{
frameCount = 0;
//发布第一帧特征点云信息，ros::Time().fromSec（）实现将具体时间转换为时间戳
//将cornerPointsLessSharp和surfPointLessFlat点也即边沿点和平面点分别发送给laserMapping
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);
}三、laserMapping.cpp
launch文件中对应的节点名称：alaserMapping
作用：根据上一部分的过程提供的corner_less_sharp特征点和surf_less_flat特征点，把帧和定义的一种submap的点云特征进行匹配。
在匹配过程中引入了一种局部地图submap以及cube（即点云方块）这里参考了博客园的一篇博文
//点云方块集合最大数量（cube的总数量，21*21*11）
const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth; //4851
//记录submap中的有效cube的index，注意submap中cube的最大数量为 5 * 5 * 5 = 125
//lidar视域范围内(FOV)的点云集索引
int laserCloudValidInd[125];
//lidar周围的点云集索引
int laserCloudSurroundInd[125];首先定义了几个关键变量：
//点云特征匹配时的优化变量
double parameters[7] = {0, 0, 0, 1, 0, 0, 0};
//世界坐标系下某个点的四元数与位移
Eigen::Map<Eigen::Quaterniond> q_w_curr(parameters);
Eigen::Map<Eigen::Vector3d> t_w_curr(parameters + 4);
// wmap_T_odom * odom_T_curr = wmap_T_curr;
// transformation between odom's world and map's world frame
//世界坐标系下当前里程计坐标系的四元数与位移
Eigen::Quaterniond q_wmap_wodom(1, 0, 0, 0);
Eigen::Vector3d t_wmap_wodom(0, 0, 0);
//里程计坐标系下某点的四元数与位移
Eigen::Quaterniond q_wodom_curr(1, 0, 0, 0);
Eigen::Vector3d t_wodom_curr(0, 0, 0);
函数中坐标系有三个
1.雷达坐标系：雷达扫描时，某点会有一个位置point_curr
2.里程计坐标系：雷达相对于里程计有一个四元数和位移校正 q_wodom_curr+t_wodom_curr
3.世界坐标系：里程计坐标系相对世界坐标系有一个四元数和位移校正 q_wmap_wodom+t_wmap_wodom
所以，雷达坐标系到世界坐标系有一个四元数和位移校正 q_w_curr+t_w_curr
某点在世界坐标系下位置 point_w。
首先看main函数，main函数比较简单，就是接收上一步发布的topic，发布周围五帧点云集合，总点云地图，全部点云。
int main(int argc, char **argv)
{
ros::init(argc, argv, "laserMapping");
ros::NodeHandle nh;
float lineRes = 0;
float planeRes = 0;
//降采样
nh.param<float>("mapping_line_resolution", lineRes, 0.4);
nh.param<float>("mapping_plane_resolution", planeRes, 0.8);
printf("line resolution %f plane resolution %f \n", lineRes, planeRes);
downSizeFilterCorner.setLeafSize(lineRes, lineRes,lineRes);
downSizeFilterSurf.setLeafSize(planeRes, planeRes, planeRes);
//订阅角点，面点，里程计下当前帧的四元数与位移，全体点云
ros::Subscriber subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 100, laserCloudCornerLastHandler);
ros::Subscriber subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 100, laserCloudSurfLastHandler);
ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 100, laserOdometryHandler);
ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 100, laserCloudFullResHandler);
//发布周围五帧点云集合（降采样后的），总点云地图（降采样后的），全部点云，
//构图处理后的当前世界坐标系下雷达位姿，构图处理前的当前世界坐标系下雷达位姿，构图处理后的雷达全部位姿
pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surround", 100);
pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_map", 100);
pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_registered", 100);
pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>("/aft_mapped_to_init", 100);
pubOdomAftMappedHighFrec = nh.advertise<nav_msgs::Odometry>("/aft_mapped_to_init_high_frec", 100);
_pubC214_vicon_pose = nh.advertise<geometry_msgs::TransformStamped> ("/C214_vicon_pose",5);
_pubC214_visual_pose = nh.advertise<nav_msgs::Odometry> ("C214_visual_pose",5);
pubLaserAfterMappedPath = nh.advertise<nav_msgs::Path>("/aft_mapped_path", 100);
for (int i = 0; i < laserCloudNum; i++)
{
laserCloudCornerArray[i].reset(new pcl::PointCloud<PointType>());
laserCloudSurfArray[i].reset(new pcl::PointCloud<PointType>());
}
std::thread mapping_process{process};
ros::spin();
return 0;
}然后介绍main函数前的几个函数：
transformAssociateToMap函数：求世界坐标系下某个点的位姿（四元数和位移）
void transformAssociateToMap()
{
q_w_curr = q_wmap_wodom * q_wodom_curr;
t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
}transformUpdate函数：求世界坐标系下当前里程计坐标系的四元数与位移
用在最后，当Mapping的位姿w_curr计算完毕后，更新增量wmap_wodom，旨在为下一次执行transformAssociateToMap函数时做准备。即获取map到odom的变换。
void transformUpdate()
{
q_wmap_wodom = q_w_curr * q_wodom_curr.inverse(); //求逆
t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr;
}pointAssociateToMap函数X：把某点的坐标从lidar坐标系转换到世界坐标系下。
void (PointType const *const pi, PointType *const po)
{
Eigen::Vector3d point_curr(pi->x, pi->y, pi->z);
Eigen::Vector3d point_w = q_w_curr * point_curr + t_w_curr;
po->x = point_w.x();
po->y = point_w.y();
po->z = point_w.z();
po->intensity = pi->intensity;
//po->intensity = 1.0;
}pointAssociateTobeMapped函数：求雷达坐标系下的某点位置
void pointAssociateTobeMapped(PointType const *const pi, PointType *const po)
{
Eigen::Vector3d point_w(pi->x, pi->y, pi->z);
Eigen::Vector3d point_curr = q_w_curr.inverse() * (point_w - t_w_curr);
po->x = point_curr.x();
po->y = point_curr.y();
po->z = point_curr.z();
po->intensity = pi->intensity;
}laserOdometryHandler回调函数：接收之前publish的topic
void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudCornerLast2)
{
mBuf.lock();
cornerLastBuf.push(laserCloudCornerLast2);
mBuf.unlock();
}
void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudSurfLast2)
{
mBuf.lock();
surfLastBuf.push(laserCloudSurfLast2);
mBuf.unlock();
}
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2)
{
mBuf.lock();
fullResBuf.push(laserCloudFullRes2);
mBuf.unlock();
}
//receive odomtry
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &laserOdometry)
{
mBuf.lock();
odometryBuf.push(laserOdometry);
mBuf.unlock();
// high frequence publish
Eigen::Quaterniond q_wodom_curr;
Eigen::Vector3d t_wodom_curr;
q_wodom_curr.x() = laserOdometry->pose.pose.orientation.x;
q_wodom_curr.y() = laserOdometry->pose.pose.orientation.y;
q_wodom_curr.z() = laserOdometry->pose.pose.orientation.z;
q_wodom_curr.w() = laserOdometry->pose.pose.orientation.w;
t_wodom_curr.x() = laserOdometry->pose.pose.position.x;
t_wodom_curr.y() = laserOdometry->pose.pose.position.y;
t_wodom_curr.z() = laserOdometry->pose.pose.position.z;
// 为了保证LOAM整体的实时性，防止Mapping线程耗时>100ms导致丢帧，用上一次的增量wmap_wodom来更新
// Odometry的位姿，旨在用Mapping位姿的初始值（也可以理解为预测值）来实时输出，进而实现LOAM整体的实时性
Eigen::Quaterniond q_w_curr = q_wmap_wodom * q_wodom_curr;
Eigen::Vector3d t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "/camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
odomAftMapped.header.stamp = laserOdometry->header.stamp;
odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
odomAftMapped.pose.pose.orientation.y = q_w_curr.y();
odomAftMapped.pose.pose.orientation.z = q_w_curr.z();
odomAftMapped.pose.pose.orientation.w = q_w_curr.w();
odomAftMapped.pose.pose.position.x = t_w_curr.x();
odomAftMapped.pose.pose.position.y = t_w_curr.y();
odomAftMapped.pose.pose.position.z = t_w_curr.z();
pubOdomAftMappedHighFrec.publish(odomAftMapped);之后便是开始mapping过程的重要函数——process函数
首先是保证算法实时性的过程，保证消息是同步的，不同步的消息会被清空：
//数据全部接收
while (!cornerLastBuf.empty() && !surfLastBuf.empty() &&
!fullResBuf.empty() && !odometryBuf.empty())
{
mBuf.lock();
// odometryBuf只保留一个与cornerLastBuf.front()时间同步的最新消息
//如果里程计信息不为空，里程计时间戳小于角特征时间戳则取出里程计数据
while (!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
odometryBuf.pop();
//如果里程计信息为空跳出本次循环
if (odometryBuf.empty())
{
mBuf.unlock();
break;
}
//如果面特征信息不为空，面特征时间戳小于角特征时间戳则取出面特征数据
while (!surfLastBuf.empty() && surfLastBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
surfLastBuf.pop();
//如果面特征信息为空跳出本次循环
if (surfLastBuf.empty())
{
mBuf.unlock();
break;
}
//如果全部点信息不为空，全部点云时间戳小于角特征时间戳则取出全部点云信息
while (!fullResBuf.empty() && fullResBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
fullResBuf.pop();
//全部点云信息为空则跳出
if (fullResBuf.empty())
{
mBuf.unlock();
break;
}
//记录时间戳
timeLaserCloudCornerLast = cornerLastBuf.front()->header.stamp.toSec();
timeLaserCloudSurfLast = surfLastBuf.front()->header.stamp.toSec();
timeLaserCloudFullRes = fullResBuf.front()->header.stamp.toSec();
timeLaserOdometry = odometryBuf.front()->header.stamp.toSec();
//再次判定时间戳是否一致
if (timeLaserCloudCornerLast != timeLaserOdometry ||
timeLaserCloudSurfLast != timeLaserOdometry ||
timeLaserCloudFullRes != timeLaserOdometry)
{
printf("time corner %f surf %f full %f odom %f \n", timeLaserCloudCornerLast, timeLaserCloudSurfLast, timeLaserCloudFullRes, timeLaserOdometry);
printf("unsync messeage!");
mBuf.unlock();
break;
}
//清空上次角特征点云，并接收新的
laserCloudCornerLast->clear();
pcl::fromROSMsg(*cornerLastBuf.front(), *laserCloudCornerLast);
cornerLastBuf.pop();
//清空上次面特征点云，并接收新的
laserCloudSurfLast->clear();
pcl::fromROSMsg(*surfLastBuf.front(), *laserCloudSurfLast);
surfLastBuf.pop();
//清空上次全部点云，并接收新的
laserCloudFullRes->clear();
pcl::fromROSMsg(*fullResBuf.front(), *laserCloudFullRes);
fullResBuf.pop();
//接收里程计坐标系下的四元数和位移
q_wodom_curr.x() = odometryBuf.front()->pose.pose.orientation.x;
q_wodom_curr.y() = odometryBuf.front()->pose.pose.orientation.y;
q_wodom_curr.z() = odometryBuf.front()->pose.pose.orientation.z;
q_wodom_curr.w() = odometryBuf.front()->pose.pose.orientation.w;
t_wodom_curr.x() = odometryBuf.front()->pose.pose.position.x;
t_wodom_curr.y() = odometryBuf.front()->pose.pose.position.y;
t_wodom_curr.z() = odometryBuf.front()->pose.pose.position.z;
odometryBuf.pop();
//角特征不为空，堆入角特征，输出目前运行实时 清空cornerLastBuf的历史缓存，为了LOAM的整体实时性
while(!cornerLastBuf.empty())
{
cornerLastBuf.pop();
printf("drop lidar frame in mapping for real time performance \n");
}后面的过程在网上一些帖子上叫做地图维护过程，我的理解是：这一段刚开始是保证数组下标的值为正，所以加入了laserCloudCenWidth/Heigh/Depth偏移，同时保证扫描到的当前位置一直处在之前定义的submap的中心位置，包括后面的让坐标系平移的for loop代码。
TicToc t_shift;
//下面是计算当前帧t_w_curr IJK坐标，下面有关25，50的运算，等效于以50m为单位的缩放，因为LOAM用1维数组
//由于数组下标只能为正，所以加了laserCloudCenWidth/Heigh/Depth的偏移，
//来使得当前位置尽量位于submap的中心处，也就是使得上图中的五角星位置尽量处于所有格子的中心附近，
//偏移laserCloudCenWidth/Heigh/Depth会动态调整，来保证当前位置尽量位于submap的中心处。
//将当前激光雷达（视角）的位置作为中心点，添加一个laserCloudCenWidth的偏执使center为正
int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth;
int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight;
int centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth;
//由于int始终向0取整，所以t_w小于-25时，要修正其取整方向，使得所有取整方向一致
// 由于计算机求余是向零取整，为了不使（-50.0,50.0）求余后都向零偏移，当被求余数为负数时求余结果统一向左偏移一个单位，也即减一
if (t_w_curr.x() + 25.0 < 0)
centerCubeI--;
if (t_w_curr.y() + 25.0 < 0)
centerCubeJ--;
if (t_w_curr.z() + 25.0 < 0)
centerCubeK--;
//下面这6个while loop的作用：调整IJK坐标系（即调整cube的位置），使得当前位置在IJK坐标系的坐标范围为：
//3 < centerCubeI < 18， 3 < centerCubeJ < 8, 3 < centerCubeK < 18，防止后续向四周拓展cube时，index（即IJK坐标）成为负数。
//如果处于下边界，表明地图向负方向延伸的可能性比较大，则循环移位，将数组中心点向上边界调整一个单位
while (centerCubeI < 3)
{
for (int j = 0; j < laserCloudHeight; j++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int i = laserCloudWidth - 1; //指针赋值，保存最后一个指针位置（最大的i值）
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
//循环移位，I维度上依次后移
for (; i >= 1; i--) //在I方向cube[I]=cube[I-1],清空最后一个空出来的cube，实现IJK坐标系向I轴负方向移动一个cube的
// 效果，从相对运动的角度看是图中的五角星在IJK坐标系下向I轴正方向移动了一个cube
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
}
//将开始点赋值为最后一个点（把之前最大的i对应的点云赋值给最小的i）
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
//为下一帧mapping时作准备
centerCubeI++;
laserCloudCenWidth++;//不是加一行，因为laserCloudCenWidth代表的当前帧坐标在IJK坐标系下的偏移量，
//而不是submap的尺寸，这个偏移量是可变的，目的就是为了使得submap位于中心，submap的尺寸是固定的5*5*5
}
//如果处于上边界，表明地图向正方向延伸的可能性比较大，则循环移位，将数组中心点向下边界调整一个单位
while (centerCubeI >= laserCloudWidth - 3)
{
for (int j = 0; j < laserCloudHeight; j++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int i = 0;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; i < laserCloudWidth - 1; i++)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeI--;
laserCloudCenWidth--;
}
while (centerCubeJ < 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int j = laserCloudHeight - 1;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; j >= 1; j--)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight * k];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeJ++;
laserCloudCenHeight++;
}
while (centerCubeJ >= laserCloudHeight - 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int k = 0; k < laserCloudDepth; k++)
{
int j = 0;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; j < laserCloudHeight - 1; j++)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight * k];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeJ--;
laserCloudCenHeight--;
}
while (centerCubeK < 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int j = 0; j < laserCloudHeight; j++)
{
int k = laserCloudDepth - 1;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; k >= 1; k--)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k - 1)];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeK++;
laserCloudCenDepth++;
}
while (centerCubeK >= laserCloudDepth - 3)
{
for (int i = 0; i < laserCloudWidth; i++)
{
for (int j = 0; j < laserCloudHeight; j++)
{
int k = 0;
pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
for (; k < laserCloudDepth - 1; k++)
{
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * (k + 1)];
}
laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeCornerPointer;
laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
laserCloudCubeSurfPointer;
laserCloudCubeCornerPointer->clear();
laserCloudCubeSurfPointer->clear();
}
}
centerCubeK--;
laserCloudCenDepth--;
}然后得到局部地图的特征点云
//以当前帧为中心，扩展为（5，5，3）的小地图
//清空submap
int laserCloudValidNum = 0;
int laserCloudSurroundNum = 0;
/*向IJ坐标轴的正负方向各拓展2个cube，K坐标轴的正负方向各拓展1个cube，上图中五角星所在的蓝色cube就是当前位置
所处的cube，拓展的cube就是黄色的cube，这些cube就是submap的范围 submap的大小就是5,5,3 */
for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++)
{
for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++)
{
for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++)
{
if (i >= 0 && i < laserCloudWidth &&
j >= 0 && j < laserCloudHeight &&
k >= 0 && k < laserCloudDepth)//如果坐标合法
{
//记住视域范围内的cube索引，匹配用
laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
laserCloudValidNum++;
//记住附近所有cube的索引，显示用
laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
laserCloudSurroundNum++;
}
}
}
}
//将submap的点云叠加在一起
laserCloudCornerFromMap->clear();
laserCloudSurfFromMap->clear();
for (int i = 0; i < laserCloudValidNum; i++)
{ // 将有效index的cube中的点云叠加到一起组成submap的特征点云
*laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
*laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
}接下来就是匹配的ICP过程，分为点到线和点到面的匹配过程：
点到线：
//点到线
//降采样角点和面点，并统计降采样之后的数量
pcl::PointCloud<PointType>::Ptr laserCloudCornerStack(new pcl::PointCloud<PointType>());
downSizeFilterCorner.setInputCloud(laserCloudCornerLast);
downSizeFilterCorner.filter(*laserCloudCornerStack);
int laserCloudCornerStackNum = laserCloudCornerStack->points.size();
pcl::PointCloud<PointType>::Ptr laserCloudSurfStack(new pcl::PointCloud<PointType>());
downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
downSizeFilterSurf.filter(*laserCloudSurfStack);
int laserCloudSurfStackNum = laserCloudSurfStack->points.size();
printf("map prepare time %f ms\n", t_shift.toc());
// 输出(5,5,3)个cube中的点云的总数量
printf("map corner num %d surf num %d \n", laserCloudCornerFromMapNum, laserCloudSurfFromMapNum);
//如果点数较多，开始优化匹配的过程
if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 50)
{
TicToc t_opt;
TicToc t_tree;
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);//构建KD-tree
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);
printf("build tree time %f ms \n", t_tree.toc());
//优化两次，第二次在第一次得到的pose上进行
for (int iterCount = 0; iterCount < 2; iterCount++)
{
//ceres::LossFunction *loss_function = NULL;
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(parameters, 4, q_parameterization); //添加参数块
problem.AddParameterBlock(parameters + 4, 3);
TicToc t_data;
int corner_num = 0;
//遍历上一帧的所有Corner点
for (int i = 0; i < laserCloudCornerStackNum; i++)
{
pointOri = laserCloudCornerStack->points[i];
//double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;
/* 需要注意的是submap中的点云都是world坐标系，而当前帧的点云都是Lidar坐标系，所以
在搜寻最近邻点时，先用预测的Mapping位姿w_curr，将Lidar坐标系下的特征点变换到world坐标系下*/
pointAssociateToMap(&pointOri, &pointSel);
// 在submap的corner特征点（target）中，寻找距离当前帧corner特征点（source）最近的5个点
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
//如果五个点中最远的那个小于1米，则求解协方差矩阵
if (pointSearchSqDis[4] < 1.0)
{
std::vector<Eigen::Vector3d> nearCorners;
Eigen::Vector3d center(0, 0, 0);
for (int j = 0; j < 5; j++)
{
Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,
laserCloudCornerFromMap->points[pointSearchInd[j]].y,
laserCloudCornerFromMap->points[pointSearchInd[j]].z);
center = center + tmp;
nearCorners.push_back(tmp);
}
//计算这5个最近邻点的中心
center = center / 5.0;
//协方差矩阵
Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
for (int j = 0; j < 5; j++)
{
Eigen::Matrix<double, 3, 1> tmpZeroMean = nearCorners[j] - center;
covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
}
// 计算协方差矩阵的特征值和特征向量，用于判断这5个点是不是呈线状分布，此为PCA的原理
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);
// if is indeed line feature
// note Eigen library sort eigenvalues in increasing order
// 如果5个点呈线状分布，最大的特征值对应的特征向量就是该线的方向向量
Eigen::Vector3d unit_direction = saes.eigenvectors().col(2);
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
// 如果最大的特征值 >> 其他特征值，则5个点确实呈线状分布，否则认为直线“不够直”
if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])
{
Eigen::Vector3d point_on_line = center;
Eigen::Vector3d point_a, point_b;
// 从中心点沿着方向向量向两端移动0.1m，构造线上的两个点
point_a = 0.1 * unit_direction + point_on_line;
point_b = -0.1 * unit_direction + point_on_line;
// 然后残差函数的形式就跟Odometry一样了，残差距离即点到线的距离（具体计算方法在lidarFactor.cpp中）
ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
corner_num++;
}
}
/*
else if(pointSearchSqDis[4] < 0.01 * sqrtDis)
{
Eigen::Vector3d center(0, 0, 0);
for (int j = 0; j < 5; j++)
{
Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,
laserCloudCornerFromMap->points[pointSearchInd[j]].y,
laserCloudCornerFromMap->points[pointSearchInd[j]].z);
center = center + tmp;
}
center = center / 5.0;
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
}
*/
}
/* 上面的点到线的ICP过程就比Odometry中的要鲁棒和准确一些了（当然也就更耗时一些），
因为不是简单粗暴地搜最近的2个corner点作为target的线，而是PCA计算出最近邻的5个点的主方向作为线的方向，
而且还会check直线是不是“足够直”， */点到面：
//点到面的ICP过程
//根据法线判断是否为面特征
int surf_num = 0;
for (int i = 0; i < laserCloudSurfStackNum; i++)
{
pointOri = laserCloudSurfStack->points[i];
//double sqrtDis = pointOri.x * pointOri.x + pointOri.y * pointOri.y + pointOri.z * pointOri.z;
pointAssociateToMap(&pointOri, &pointSel);
kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
//求面的法向量就不是用的PCA了（虽然论文中说还是PCA），使用的是最小二乘拟合，是为了提效？不确定
//假设平面不通过原点，则平面的一般方程为Ax + By + Cz + 1 = 0，用这个假设可以少算一个参数，提效。
Eigen::Matrix<double, 5, 3> matA0;
Eigen::Matrix<double, 5, 1> matB0 = -1 * Eigen::Matrix<double, 5, 1>::Ones();
// 用上面的2个矩阵表示平面方程就是 matA0 * norm（A, B, C） = matB0，这是个超定方程组，因为数据个数超过未知数的个数
if (pointSearchSqDis[4] < 1.0)
{
for (int j = 0; j < 5; j++)
{
matA0(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;
matA0(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;
matA0(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;
//printf(" pts %f %f %f ", matA0(j, 0), matA0(j, 1), matA0(j, 2));
}
//求解这个最小二乘问题，可得平面的法向量
// find the norm of plane
Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0);
// Ax + By + Cz + 1 = 0，全部除以法向量的模长，方程依旧成立，而且使得法向量归一化了
double negative_OA_dot_norm = 1 / norm.norm();
norm.normalize();
// Here n(pa, pb, pc) is unit norm of plane
bool planeValid = true;
for (int j = 0; j < 5; j++)
{
// if OX * n > 0.2, then plane is not fit well
// 点(x0, y0, z0)到平面Ax + By + Cz + D = 0 的距离公式 = fabs(Ax0 + By0 + Cz0 + D) / sqrt(A^2 + B^2 + C^2)
if (fabs(norm(0) * laserCloudSurfFromMap->points[pointSearchInd[j]].x +
norm(1) * laserCloudSurfFromMap->points[pointSearchInd[j]].y +
norm(2) * laserCloudSurfFromMap->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2)
{
planeValid = false;//平面没有拟合好，平面“不够平”
break;
}
}
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
if (planeValid)
{ //构造点到面的距离残差项
ceres::CostFunction *cost_function = LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
surf_num++;
}
}
/*
else if(pointSearchSqDis[4] < 0.01 * sqrtDis)
{
Eigen::Vector3d center(0, 0, 0);
for (int j = 0; j < 5; j++)
{
Eigen::Vector3d tmp(laserCloudSurfFromMap->points[pointSearchInd[j]].x,
laserCloudSurfFromMap->points[pointSearchInd[j]].y,
laserCloudSurfFromMap->points[pointSearchInd[j]].z);
center = center + tmp;
}
center = center / 5.0;
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
ceres::CostFunction *cost_function = LidarDistanceFactor::Create(curr_point, center);
problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
}
*/
}
//printf("corner num %d used corner num %d \n", laserCloudCornerStackNum, corner_num);
//printf("surf num %d used surf num %d \n", laserCloudSurfStackNum, surf_num);
printf("mapping data assosiation time %f ms \n", t_data.toc());
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;
options.check_gradients = false;
options.gradient_check_relative_precision = 1e-4;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
printf("mapping solver time %f ms \n", t_solver.toc());
//printf("time %f \n", timeLaserOdometry);
//printf("corner factor num %d surf factor num %d\n", corner_num, surf_num);
//printf("result q %f %f %f %f result t %f %f %f\n", parameters[3], parameters[0], parameters[1], parameters[2],
// parameters[4], parameters[5], parameters[6]);
}
printf("mapping optimization time %f \n", t_opt.toc());
// -------------------至此，两次迭代完成了，得到w_curr，当前帧在map坐标系下的pose---------------------
}
else //submap的点数量不够
{
ROS_WARN("time Map corner and surf num are not enough");
}
/*完成ICP（迭代2次）的特征匹配后，用最后匹配计算出的优化变量w_curr，
更新增量wmap_wodom，为下一次mapping作准备*/最后是更新点云为下一次匹配作准备，并进行发布（每5帧发布一次）：
/*完成ICP（迭代2次）的特征匹配后，用最后匹配计算出的优化变量w_curr，
更新增量wmap_wodom，为下一次mapping作准备*/
transformUpdate(); //获取map到odom的变换Transform
TicToc t_add;
//下面两个for loop的作用：将当前帧的特征点云，逐点进行操作：转换到world坐标系并添加到对应位置的cube中
for (int i = 0; i < laserCloudCornerStackNum; i++)
{
//Lidar坐标系转到world坐标系
pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);
//计算本次的特征点的IJK坐标，进而确定添加到哪个cube中（按50的比例尺缩小）
int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;
if (pointSel.x + 25.0 < 0)
cubeI--;
if (pointSel.y + 25.0 < 0)
cubeJ--;
if (pointSel.z + 25.0 < 0)
cubeK--;
/* 只挑选-laserCloudCenWidth * 50.0 < point.x < laserCloudCenWidth * 50.0范围内的点，y和z同理
按照尺度放进不同的组，每个组的点数量各异 */
if (cubeI >= 0 && cubeI < laserCloudWidth &&
cubeJ >= 0 && cubeJ < laserCloudHeight &&
cubeK >= 0 && cubeK < laserCloudDepth)
{
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
laserCloudCornerArray[cubeInd]->push_back(pointSel);
}
}
//将surf points按距离（比例尺缩小）归入相应的立方体
for (int i = 0; i < laserCloudSurfStackNum; i++)
{
pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);
int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;
if (pointSel.x + 25.0 < 0)
cubeI--;
if (pointSel.y + 25.0 < 0)
cubeJ--;
if (pointSel.z + 25.0 < 0)
cubeK--;
if (cubeI >= 0 && cubeI < laserCloudWidth &&
cubeJ >= 0 && cubeJ < laserCloudHeight &&
cubeK >= 0 && cubeK < laserCloudDepth)
{
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
laserCloudSurfArray[cubeInd]->push_back(pointSel);
}
}
printf("add points time %f ms\n", t_add.toc());
TicToc t_filter;
// 对submap的cube点云进行降采样
// 因为新增加了点云，对之前已经存有点云的submap cube全部重新进行一次降采样
//优化：那如果没有新增加点，那就不需要重新进行一次降采样
for (int i = 0; i < laserCloudValidNum; i++) // 遍历submap的所有cube
{
int ind = laserCloudValidInd[i]; // submap中每一个cube的索引
// 判断当前的submap的cube id 是否在当前帧的索引的vector中
pcl::PointCloud<PointType>::Ptr tmpCorner(new pcl::PointCloud<PointType>());
downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);
downSizeFilterCorner.filter(*tmpCorner);
laserCloudCornerArray[ind] = tmpCorner;
pcl::PointCloud<PointType>::Ptr tmpSurf(new pcl::PointCloud<PointType>());
downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);
downSizeFilterSurf.filter(*tmpSurf);
laserCloudSurfArray[ind] = tmpSurf;
}
printf("filter time %f ms \n", t_filter.toc());
/*发布*/
// 每5帧发布一次submap (5,5,3)的cube大小
TicToc t_pub;
//publish surround map for every 5 frame
if (frameCount % 5 == 0)
{
laserCloudSurround->clear();
for (int i = 0; i < laserCloudSurroundNum; i++) //遍历submap
{
int ind = laserCloudSurroundInd[i];
*laserCloudSurround += *laserCloudCornerArray[ind];
*laserCloudSurround += *laserCloudSurfArray[ind];
}
sensor_msgs::PointCloud2 laserCloudSurround3;
pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);
laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudSurround3.header.frame_id = "/camera_init"; //world坐标系
pubLaserCloudSurround.publish(laserCloudSurround3);
}
// 每20帧发布IJK局部地图（降采样后的总点云地图）
// pub laserCloudMap for every 20 frames
if (frameCount % 20 == 0)
{
pcl::PointCloud<PointType> laserCloudMap;
for (int i = 0; i < 4851; i++) //遍历IJK坐标系
{
laserCloudMap += *laserCloudCornerArray[i];
laserCloudMap += *laserCloudSurfArray[i];
}
sensor_msgs::PointCloud2 laserCloudMsg;
pcl::toROSMsg(laserCloudMap, laserCloudMsg);
laserCloudMsg.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudMsg.header.frame_id = "world";
pubLaserCloudMap.publish(laserCloudMsg);
}
//全部点云转化到world坐标系，并发布
int laserCloudFullResNum = laserCloudFullRes->points.size();
for (int i = 0; i < laserCloudFullResNum; i++)
{
pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
}
sensor_msgs::PointCloud2 laserCloudFullRes3;
pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudFullRes3.header.frame_id = "/camera_init";
pubLaserCloudFullRes.publish(laserCloudFullRes3);
printf("mapping pub time %f ms \n", t_pub.toc());
printf("whole mapping time %f ms +++++\n", t_whole.toc());
nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "/camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);
odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
odomAftMapped.pose.pose.orientation.y = q_w_curr.y();
odomAftMapped.pose.pose.orientation.z = q_w_curr.z();
odomAftMapped.pose.pose.orientation.w = q_w_curr.w();
odomAftMapped.pose.pose.position.x = t_w_curr.x();
odomAftMapped.pose.pose.position.y = t_w_curr.y();
odomAftMapped.pose.pose.position.z = t_w_curr.z();
pubOdomAftMapped.publish(odomAftMapped);
geometry_msgs::PoseStamped laserAfterMappedPose;
laserAfterMappedPose.header = odomAftMapped.header;
laserAfterMappedPose.pose = odomAftMapped.pose.pose;
laserAfterMappedPath.header.stamp = odomAftMapped.header.stamp;
laserAfterMappedPath.header.frame_id = "world";
laserAfterMappedPath.poses.push_back(laserAfterMappedPose);
pubLaserAfterMappedPath.publish(laserAfterMappedPath);
//发布tf变换,(world)/camera_init--->(当前帧)/aft_mapped
static tf::TransformBroadcaster br;
tf::Transform transform;
tf::Quaternion q;
transform.setOrigin(tf::Vector3(t_w_curr(0),
t_w_curr(1),
t_w_curr(2)));
q.setW(q_w_curr.w());
q.setX(q_w_curr.x());
q.setY(q_w_curr.y());
q.setZ(q_w_curr.z());
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "/camera_init", "/aft_mapped"));
frameCount++;
}
//暂缓2ms
std::chrono::milliseconds dura(2);
std::this_thread::sleep_for(dura);
}
}最后是lidarFactor.hpp代码，在launch文件中没有自己对应的节点。
作用是计算点到线、点到面的距离残差项，包括点到线的残差距离计算和点到面的残差距离计算。
//点到线的残差距离计算
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_) {}
//括号运算符重载
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> 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())};
//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]}; //last到curr的旋转四元数
Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
// 考虑运动补偿（四元数插值），ktti点云已经补偿过所以可以忽略下面的对四元数slerp插值以及对平移的线性插值
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;
/*Odometry线程时，下面是将当前帧Lidar坐标系下的cp点变换到上一帧的Lidar坐标系下，
然后在上一帧的Lidar坐标系计算点到线的残差距离*/
/*Mapping线程时，下面是将当前帧Lidar坐标系下的cp点变换到world坐标系下，
然后在world坐标系下计算点到线的残差距离*/
lp = q_last_curr * cp + t_last_curr;
//点到线的计算
Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);
Eigen::Matrix<T, 3, 1> de = lpa - lpb;
/*最终的残差本来应该是residual[0] = nu.norm() / de.norm(); 为啥也分成3个，我也不知道，
从我试验的效果来看，确实是下面的残差函数形式，最后输出的pose精度会好一点点，这里需要注意的是，
所有的residual都不用加fabs，因为Ceres内部会对其求 平方 作为最终的残差项*/
residual[0] = nu.x() / de.norm();
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>(
// ^ ^ ^
// / / /
// 残差的维度 ____/ / /
// 优化变量q的维度 __/ /
// 优化变量t的维度 ___ /
new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));
}
Eigen::Vector3d curr_point, last_point_a, last_point_b;
double s;
};
// 计算Odometry线程中点到面的残差距离
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_)
{
// 点l、j、m就是搜索到的最近邻的3个点，下面就是计算出这三个点构成的平面ljm的法向量
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())};//cp
Eigen::Matrix<T, 3, 1> lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())}; //last point j
//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>(
// ^ ^ ^
// / / /
// 残差的维度 __ / / /
// 优化变量q的维度 _/ /
// 优化变量t的维度 __/
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;
};
//计算mapping线程中点到平面的残差距离
struct LidarPlaneNormFactor
{
LidarPlaneNormFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_,
double negative_OA_dot_norm_) : curr_point(curr_point_), plane_unit_norm(plane_unit_norm_),
negative_OA_dot_norm(negative_OA_dot_norm_) {}
template <typename T>
bool operator()(const T *q, const T *t, T *residual) const
{
Eigen::Quaternion<T> q_w_curr{q[3], q[0], q[1], q[2]};
Eigen::Matrix<T, 3, 1> t_w_curr{t[0], t[1], t[2]};
Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
Eigen::Matrix<T, 3, 1> point_w;
point_w = q_w_curr * cp + t_w_curr;
Eigen::Matrix<T, 3, 1> norm(T(plane_unit_norm.x()), T(plane_unit_norm.y()), T(plane_unit_norm.z()));
/*直接使用点到面的距离公式进行计算：点(x0, y0, z0)到平面Ax + By + Cz + D = 0 的距离 = fabs(A*x0 + B*y0 + C*z0 + D) / sqrt(A^2 + B^2 + C^2）
因为法向量（A, B, C）已经归一化了，所以距离公式可以简写为：距离 = fabs(A*x0 + B*y0 + C*z0 + D) ，即：*/
residual[0] = norm.dot(point_w) + T(negative_OA_dot_norm);
return true;
}
static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d plane_unit_norm_,
const double negative_OA_dot_norm_)
{
return (new ceres::AutoDiffCostFunction<
LidarPlaneNormFactor, 1, 4, 3>(
new LidarPlaneNormFactor(curr_point_, plane_unit_norm_, negative_OA_dot_norm_)));
}
Eigen::Vector3d curr_point;
Eigen::Vector3d plane_unit_norm;
double negative_OA_dot_norm;
};
struct LidarDistanceFactor
{
LidarDistanceFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d closed_point_)
: curr_point(curr_point_), closed_point(closed_point_){}
template <typename T>
bool operator()(const T *q, const T *t, T *residual) const
{
Eigen::Quaternion<T> q_w_curr{q[3], q[0], q[1], q[2]};
Eigen::Matrix<T, 3, 1> t_w_curr{t[0], t[1], t[2]};
Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
Eigen::Matrix<T, 3, 1> point_w;
point_w = q_w_curr * cp + t_w_curr;
residual[0] = point_w.x() - T(closed_point.x());
residual[1] = point_w.y() - T(closed_point.y());
residual[2] = point_w.z() - T(closed_point.z());
return true;
}
static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d closed_point_)
{
return (new ceres::AutoDiffCostFunction<
LidarDistanceFactor, 3, 4, 3>(
new LidarDistanceFactor(curr_point_, closed_point_)));
}
Eigen::Vector3d curr_point;
Eigen::Vector3d closed_point;
};
2594
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
