一、A_loam的主要过程
采用提取点云的面特征(曲率小)、线特征(曲率大)，构建点-线约束、点-面约束，采用ceres非线性优化进行匹配. 粗匹配(快，Scan2Scan)+精匹配(慢，Scan2Map)模式。
1）scanRegistration.cpp
完成两件事：点云重组（计算ring以及分类每条线束的点、每个点扫描时间）和特征寻找（每一帧点云寻找16x6x(4+2)个特征，16线束、6个方向、每个方向4个极大面特征和2个极大线特征）；
2）laserOdometry.cpp
scan2scan的粗匹配进行帧间递推，完成里程计的位姿计算（当前帧的极大特征与上一帧的次极大特征匹配，次极大特征的集合包含了极大特征集合）；
3）laserMapping.cpp
scan2map的精匹配，当前帧特征在submap特征中最近邻寻找，找到后，采用类似PCA（求点云的协方差矩阵的特征值和特征向量）or 构建超定方程（QR分解）去检验submap里面的特征，是否满足的线方程 or 平面方程，满足后，用当前帧的特征点和求解的方程去构建优化问题（laserOdom的估计值会在这里提供初值）。
二、代码理解
A_loam的laserOdom估计出来的位姿结果是比较一般的，所以后面有作者发布了F_LOAM, 直接去除了该节点。下面的mapping部分最关键的部分，加上自己的理解的注释，直接看建图线程：std::thread mapping_process{process}：
if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 50) // submap的特征点足够，则进行优化问题的构建
{
TicToc t_opt;
TicToc t_tree;
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap); // 输入submap特征
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);
printf("build tree time %f ms \n", t_tree.toc());
for (int iterCount = 0; iterCount < 2; iterCount++) // 构建最小二乘，类似laserodom， 迭代次数2
{
//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); // parameters为7个元素的数组,前4个姿态,后3个位移
problem.AddParameterBlock(parameters+4, 3);
TicToc t_data;
int corner_num = 0;
/**
* 如果是边线点，则以这五个点的均值点为中心，以5个点的主方向向量（类似于PCA方法）为方向，作一条直线，令该边线点与直线距离最短，构建非线性优化问题;
* 如果是平面点，则寻找五个点的法方向（反向的PCA方法），令这个平面点在法方向上与五个近邻点的距离最小，构建非线性优化问题。
*/
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;
pointAssociateToMap(&pointOri, &pointSel); // 该点坐标转到map下，开始寻找关联
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); // 最近邻5个点
if (pointSearchSqDis[4] < 1.0)
{
std::vector<Eigen::Vector3d> nearCorners; // 存储搜索出的5个特征点
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);
}
center = center / 5.0; // 五个点的均值
Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero(); // ! 构建5个最近邻点的协方差矩阵
for (int j = 0; j < 5; j++)
{
Eigen::Matrix<double, 3, 1> tmpZeroMean = nearCorners[j] - center;
covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
}
// 对协方差矩阵进行特征值分解
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);
// if is indeed line feature
// note Eigen library sort eigenvalues in increasing order
// ! PCA的原理：计算协方差矩阵的特征值和特征向量，用于判断这5个点是不是呈线状分布
// ! 如果5个点呈线状分布，最大的特征值对应的特征向量就是该线的方向向量
// eigenvectors分解的特征向量组成的矩阵, eigenvalues特征值(内部应该是按照升序排列的)
Eigen::Vector3d unit_direction = saes.eigenvectors().col(2); // 如果5个点呈线状分布，最大的特征值对应的特征向量就是该线的方向向量
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1]) // 如果最大的特征值 远大于>> 其他特征值，则5个点确实呈线状分布，否则认为直线“不够直”
{
Eigen::Vector3d point_on_line = center; // 中心点
Eigen::Vector3d point_a, point_b;
// 根据拟合出来的线特征方向，以平均点为中心构建两个虚拟点，代替一条直线
point_a = 0.1 * unit_direction + point_on_line;
point_b = -0.1 * unit_direction + point_on_line;
// 构建约束
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);
}
*/
}
int surf_num = 0;
for (int i = 0; i < laserCloudSurfStackNum; i++) // ! 【2】面特征约束构建
{
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);
Eigen::Matrix<double, 5, 3> matA0;
Eigen::Matrix<double, 5, 1> matB0 = -1 * Eigen::Matrix<double, 5, 1>::Ones();
// 构建平面方程Ax + By +Cz + 1 = 0
// 通过构建一个超定方程来求解这个平面方程
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接口求解该方程，解就是这个平面的法向量
// ! matA0 * norm = matB0, matB0是0矩阵, 那么求得的norm即为法向量
Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0);
double negative_OA_dot_norm = 1 / norm.norm(); // 平方开方,2范数
norm.normalize(); // 后面校验的时候norm用的归一化去做的,所以平面方程的常数项1也要归一化
// 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 的距离
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); // 最终优化输出 parameters[7]
}
*/
}
//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());
// ! ceres求解
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());
}上面的点到线的ICP过程就比Odometry中的要鲁棒和准确一些了（当然也就更耗时一些），因为不是简单粗暴地搜上一帧的2个corner点作为target的线，而是PCA计算出最近邻的5个点的主方向作为线的方向。
上面的点到面的ICP过程同样比Odometry中的要鲁棒和准确一些了（当然也就更耗时一些），因为不是简单粗暴地搜最近的3个surf点作为target的平面点，而是对最近邻的5个点进行基于最小二乘的平面拟合.（构建超定方程组QR分解）。
另外，我觉得重要的，就是lidarFactor.hpp的残差构建，原理如下：
点线残差（我理解了好久=_=，太蠢了）：
点-面约束残差就是点的面的距离，目标点 点积面的法向即可，那个比较好理解。
还有一点，就是比较新颖的，是源码中对submap的管理并不是基于时序的滑动窗口方式，而是空间范围划分方式，这就是为什么最后的frame对应的submap会包含早期的frames。
Last，我看过的觉得很详细的分析：LOAM笔记及A-LOAM源码阅读 - WellP.C - 博客园
感谢！！！
1424
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
