2021SC@SDUSC
（八）lvi-sam源代码阅读6
lidar_odometry
mapOptmization.cpp
laserCloudInfoHandler
订阅激光帧点云信息
接上篇：
4 scan2MapOptimization()
scan-to-map优化当前帧位姿
void scan2MapOptimization()
{
// 如果没有关键帧点云数据
if (cloudKeyPoses3D->points.empty())
return;
// 关键点数量大于阈值，边为10，面为100
/*
nh.param<int>(PROJECT_NAME + "/edgeFeatureMinValidNum", edgeFeatureMinValidNum, 10);
nh.param<int>(PROJECT_NAME + "/surfFeatureMinValidNum", surfFeatureMinValidNum, 100);
*/
//当前帧
if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum)
{
//输入为局部 map 点云 （第二步中得到前之前关键帧集合）
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);
// 迭代30次
for (int iterCount = 0; iterCount < 30; iterCount++)
{
// 每次迭代清空特征点集合
// 当前帧与局部map匹配上了的角点、平面点，加入同一集合：后面是对应点的参数
laserCloudOri->clear();
coeffSel->clear();
// 当前激光帧角点寻找局部map匹配点
cornerOptimization();
// 当前激光帧平面点寻找局部map匹配点
surfOptimization();
// 提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合
combineOptimizationCoeffs();
//scan-to-map优化
//对匹配特征点计算雅可比矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped
if (LMOptimization(iterCount) == true)
break;
}
transformUpdate();
} else {
ROS_WARN("Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum, laserCloudSurfLastDSNum);
}
}
// 当前激光帧角点寻找局部map匹配点
void cornerOptimization()
{
// 仿射变换，更新当前位姿与地图间位姿变换
updatePointAssociateToMap();
#pragma omp parallel for num_threads(numberOfCores)
// 遍历点云，构建点到直线的约束
for (int i = 0; i < laserCloudCornerLastDSNum; i++)
{
PointType pointOri, pointSel, coeff;
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
// 角点（坐标还是lidar系）
pointOri = laserCloudCornerLastDS->points[i];
// 将点从 lidar 坐标系变换到 map 坐标系
pointAssociateToMap(&pointOri, &pointSel);
// 在局部角点 map 中查找当前角点相邻的 5 个角点
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
//存储矩阵
//https://blog.csdn.net/u012058778/article/details/90764430
cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));
// 只有最近的点都在一定阈值内（1米）才进行计算 说明得到的结果是升序排列的
if (pointSearchSqDis[4] < 1.0) {
float cx = 0, cy = 0, cz = 0;
for (int j = 0; j < 5; j++) {
// 计算 5 个点的均值坐标，记为中心点
cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
}
cx /= 5; cy /= 5; cz /= 5;
// 根据均值计算协方差 （得到一个协方差矩阵：）
float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
for (int j = 0; j < 5; j++) {
// 计算点与中心点之间的距离
float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;
a11 += ax * ax; a12 += ax * ay; a13 += ax * az;
a22 += ay * ay; a23 += ay * az;
a33 += az * az;
}
a11 /= 5; a12 /= 5; a13 /= 5; a22 /= 5; a23 /= 5; a33 /= 5;
// 构建协方差矩阵
matA1.at<float>(0, 0) = a11; matA1.at<float>(0, 1) = a12; matA1.at<float>(0, 2) = a13;
matA1.at<float>(1, 0) = a12; matA1.at<float>(1, 1) = a22; matA1.at<float>(1, 2) = a23;
matA1.at<float>(2, 0) = a13; matA1.at<float>(2, 1) = a23; matA1.at<float>(2, 2) = a33;
//https://blog.csdn.net/weixin_37835423/article/details/111587379
// 特征值分解，求协方差矩阵的特征值和特征向量, 特征值：matD1，特征向量：matV1
cv::eigen(matA1, matD1, matV1);
// 如果最大的特征值相比次大特征值，大很多，认为构成了线，角点是合格的
//两个疑问点：1 为啥能这么判断这些点在同一直线上；2 为啥这些点在同一直线上能判定角点满足条件
//描述子？？？
//2 对于这个我自己的理解：对于当前需要被匹配的角点a，在之前选出的角点集合中，选出距离最近的5个角点
//距离都<1米，保证距离足够近；在一条直线上，可能跟线性有关？？？（可能跟某种特征子有关系）
if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {
// 当前帧角点坐标（map 系下）
float x0 = pointSel.x;
float y0 = pointSel.y;
float z0 = pointSel.z;
// 局部 map 对应中心角点，沿着特征向量（直线方向）方向，前后各取一个点
float x1 = cx + 0.1 * matV1.at<float>(0, 0);
float y1 = cy + 0.1 * matV1.at<float>(0, 1);
float z1 = cz + 0.1 * matV1.at<float>(0, 2);
float x2 = cx - 0.1 * matV1.at<float>(0, 0);
float y2 = cy - 0.1 * matV1.at<float>(0, 1);
float z2 = cz - 0.1 * matV1.at<float>(0, 2);
// area_012，也就是三个点组成的三角形面积*2，叉积的模|axb|=a*b*sin(theta)
float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
+ ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
+ ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));
// line_12，底边边长
float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
// 两次叉积，得到点到直线的垂线段单位向量，x 分量，下面同理
float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
+ (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;
float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
- (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
+ (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
// 三角形的高，也就是点到直线距离
float ld2 = a012 / l12;
// 下面涉及到一个鲁棒核函数，作者简单地设计了这个核函数
// 距离越大，s 越小，使用距离惩罚因子（权重）
float s = 1 - 0.9 * fabs(ld2);
coeff.x = s * la;
coeff.y = s * lb;
coeff.z = s * lc;
coeff.intensity = s * ld2;
//距离越小，这个因子的影响就越小（距离越大，加权后的距离小的越大）
if (s > 0.1) {
//距离<1
当前激光帧角点，加入匹配集合中
laserCloudOriCornerVec[i] = pointOri;
// 角点的参数
coeffSelCornerVec[i] = coeff;
laserCloudOriCornerFlag[i] = true;
}
}
}
}
}
// 当前激光帧平面点寻找局部map匹配点
void surfOptimization()
{
updatePointAssociateToMap();
#pragma omp parallel for num_threads(numberOfCores)
for (int i = 0; i < laserCloudSurfLastDSNum; i++)
{
PointType pointOri, pointSel, coeff;
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
pointOri = laserCloudSurfLastDS->points[i];
pointAssociateToMap(&pointOri, &pointSel);
kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
Eigen::Matrix<float, 5, 3> matA0;
Eigen::Matrix<float, 5, 1> matB0;
Eigen::Vector3f matX0;
matA0.setZero();
matB0.fill(-1);
matX0.setZero();
if (pointSearchSqDis[4] < 1.0) {
for (int j = 0; j < 5; j++) {
// 那么将这五个点存入 matA
matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
}
// Ax = B，根据这五个点求解平面方程，进行 QR 分解，获得平面方程解
// 假设平面方程为ax+by+cz+1=0，这里就是求方程的系数abc，d=1
matX0 = matA0.colPivHouseholderQr().solve(matB0);
// 平面方程的系数，也是法向量的分量
float pa = matX0(0, 0);
float pb = matX0(1, 0);
float pc = matX0(2, 0);
float pd = 1;
// 将 matX 归一化，也就是单位法向量
float ps = sqrt(pa * pa + pb * pb + pc * pc);
pa /= ps; pb /= ps; pc /= ps; pd /= ps;
bool planeValid = true;
// 检查平面是否合格，如果5个点中有点到平面的距离超过0.2m，那么认为这些点太分散了，不构成平面
for (int j = 0; j < 5; j++) {
if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2) {
planeValid = false;
break;
}
}
// 平面合格
if (planeValid) {
// 当前激光帧点到平面距离
float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x
+ pointSel.y * pointSel.y + pointSel.z * pointSel.z));
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
coeff.intensity = s * pd2;
if (s > 0.1) {
laserCloudOriSurfVec[i] = pointOri;
coeffSelSurfVec[i] = coeff;
laserCloudOriSurfFlag[i] = true;
}
}
}
}
}
// 提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合
void combineOptimizationCoeffs()
{
// combine corner coeffs
// 遍历当前帧角点集合，提取出与局部map匹配上了的角点
for (int i = 0; i < laserCloudCornerLastDSNum; ++i){
if (laserCloudOriCornerFlag[i] == true){
laserCloudOri->push_back(laserCloudOriCornerVec[i]);
coeffSel->push_back(coeffSelCornerVec[i]);
}
}
// combine surf coeffs
// 遍历当前帧平面点集合，提取出与局部map匹配上了的平面点
for (int i = 0; i < laserCloudSurfLastDSNum; ++i){
if (laserCloudOriSurfFlag[i] == true){
]p->push_back(laserCloudOriSurfVec[i]);
coeffSel->push_back(coeffSelSurfVec[i]);
}
}
// reset flag for next iteration
// 清空标记
std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
}
// scan-to-map优化，也就是优化 argmin_x ||Ax-b||^2
/**
* scan-to-map优化
* 对匹配特征点计算雅可比矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformtobemapped
* 公式推到：todo
*/
bool LMOptimization(int iterCount)
{
// This optimization is from the original loam_velodyne, need to cope with coordinate transformation 这个优化来自于原来的loam_velodyne，需要处理坐标变换
// lidar <- camera --- camera <- lidar
// x = z --- x = y
// y = x --- y = z
// z = y --- z = x
// roll = yaw --- roll = pitch
// pitch = roll --- pitch = yaw
// yaw = pitch --- yaw = roll
// lidar -> camera
// 雷达到相机变换的三轴方向的正弦和余弦
float srx = sin(transformTobeMapped[1]);
float crx = cos(transformTobeMapped[1]);
float sry = sin(transformTobeMapped[2]);
float cry = cos(transformTobeMapped[2]);
float srz = sin(transformTobeMapped[0]);
float crz = cos(transformTobeMapped[0]);
int laserCloudSelNum = laserCloudOri->size();
// 当前帧匹配特征点数太少
if (laserCloudSelNum < 50) {
return false;
}
cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));
cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));
PointType pointOri, coeff;
//楼下是一堆看不懂的数学公式（等着看完了loam再说
// 遍历匹配特征点，构建Jacobian矩阵
for (int i = 0; i < laserCloudSelNum; i++) {
// lidar -> camera
pointOri.x = laserCloudOri->points[i].y;
pointOri.y = laserCloudOri->points[i].z;
pointOri.z = laserCloudOri->points[i].x;
// lidar -> camera
// coeff 为点到直线/平面距离
coeff.x = coeffSel->points[i].y;
coeff.y = coeffSel->points[i].z;
coeff.z = coeffSel->points[i].x;
coeff.intensity = coeffSel->points[i].intensity;
// in camera
//求导？？？
float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x
+ (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y
+ (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;
float ary = ((cry*srx*srz - crz*sry)*pointOri.x
+ (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x
+ ((-cry*crz - srx*sry*srz)*pointOri.x
+ (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;
float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x
+ (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y
+ ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;
// lidar -> camera
matA.at<float>(i, 0) = arz;
matA.at<float>(i, 1) = arx;
matA.at<float>(i, 2) = ary;
matA.at<float>(i, 3) = coeff.z;
matA.at<float>(i, 4) = coeff.x;
matA.at<float>(i, 5) = coeff.y;
// 点到直线距离、平面距离，作为观测值
matB.at<float>(i, 0) = -coeff.intensity;
}
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
// J^T·J·delta_x = -J^T·f 高斯牛顿
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
// 首次迭代，检查近似 Hessian 矩阵（J^T·J）是否退化，或者称为奇异，行列式值=0
if (iterCount == 0)
{
cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));
// 首次迭代，检查近似 Hessian 矩阵（J^T·J）是否退化，或者称为奇异，行列式值=0
cv::eigen(matAtA, matE, matV);
matV.copyTo(matV2);
isDegenerate = false;
float eignThre[6] = {
100, 100, 100, 100, 100, 100};
for (int i = 5; i >= 0; i--) {
if (matE.at<float>(0, i) < eignThre[i]) {
for (int j = 0; j < 6; j++) {
matV2.at<float>(i, j) = 0;
}
isDegenerate = true;
} else {
break;
}
}
matP = matV.inv() * matV2;
}
if (isDegenerate) {
cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
matX.copyTo(matX2);
matX = matP * matX2;
}
transformTobeMapped[0] += matX.at<float>(0, 0);
transformTobeMapped[1] += matX.at<float>(1, 0);
transformTobeMapped[2] += matX.at<float>(2, 0);
transformTobeMapped[3] += matX.at<float>(3, 0);
transformTobeMapped[4] += matX.at<float>(4, 0);
transformTobeMapped[5] += matX.at<float>(5, 0);
float deltaR = sqrt(
pow(pcl::rad2deg(matX.at<float>(0, 0)), 2) +
pow(pcl::rad2deg(matX.at<float>(1, 0)), 2) +
pow(pcl::rad2deg(matX.at<float>(2, 0)), 2));
float deltaT = sqrt(
pow(matX.at<float>(3, 0) * 100, 2) +
pow(matX.at<float>(4, 0) * 100, 2) +
pow(matX.at<float>(5, 0) * 100, 2));
if (deltaR < 0.05 && deltaT < 0.05) {
return true; // converged
}
return false; // keep optimizing
}
// 用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合，更新当前帧位姿的roll、pitch，约束z坐标
void transformUpdate()
{
if (cloudInfo.imuAvailable == true)
{
// 俯仰角小于1.4
if (std::abs(cloudInfo.imuPitchInit) < 1.4)
{
double imuWeight = 0.01;
tf::Quaternion imuQuaternion;
tf::Quaternion transformQuaternion;
double rollMid, pitchMid, yawMid;
// slerp roll
// roll 角求加权均值，用 scan-to-map 优化得到的位姿与 imu 原始 RPY 数据，进行加权平均
transformQuaternion.setRPY(transformTobeMapped[0], 0, 0);
imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);
//slerp函数：利用求面差值得到介于两个已知旋转之间的近似值
tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
transformTobeMapped[0] = rollMid;
// slerp pitch
transformQuaternion.setRPY(0, transformTobeMapped[1], 0);
imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
transformTobeMapped[1] = pitchMid;
}
}
// 更新当前帧位姿的roll, pitch, z坐标
// 因为是小车，roll、pitch是相对稳定的，不会有很大变动，一定程度上可以信赖imu的数据，z是进行高度约束
/*
nh.param<float>(PROJECT_NAME + "/z_tollerance", z_tollerance, FLT_MAX);
nh.param<float>(PROJECT_NAME + "/rotation_tollerance", rotation_tollerance, FLT_MAX);
*/
transformTobeMapped[0] = constraintTransformation(transformTobeMapped[0], rotation_tollerance);
transformTobeMapped[1] = constraintTransformation(transformTobeMapped[1], rotation_tollerance);
transformTobeMapped[5] = constraintTransformation(transformTobeMapped[5], z_tollerance);
}
float constraintTransformation(float value, float limit)
{
if (value < -limit)
value = -limit;
if (value > limit)
value = limit;
return value;
}
5 saveKeyFramesAndFactor()
因子图优化，更新所有关键帧位姿
void saveKeyFramesAndFactor()
{
// 计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧
if (saveFrame() == false)
return;
// odom factor
// 激光里程计因子
addOdomFactor();
// gps factor
// addGPSFactor();
// loop factor
// 闭环因子
addLoopFactor();
//、、、、、楼下这些跟imu里的一样
// update iSAM
// 执行优化
//initialEstimate在addOdomFactor中被初始化
isam->update(gtSAMgraph, initialEstimate);
isam->update();
// update之后要清空一下保存的因子图，注：历史数据不会清掉，ISAM保存起来了///
gtSAMgraph.resize(0);
initialEstimate.clear();
//save key poses
PointType thisPose3D;
PointTypePose thisPose6D;
Pose3 latestEstimate;
// 优化结果
isamCurrentEstimate = isam->calculateEstimate();
latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1);
// cout << "****************************************************" << endl;
// isamCurrentEstimate.print("Current estimate: ");
// cloudKeyPoses3D 加入当前帧位姿
thisPose3D.x = latestEstimate.translation().x();
thisPose3D.y = latestEstimate.translation().y();
thisPose3D.z = latestEstimate.translation().z();
thisPose3D.intensity = cloudKeyPoses3D->size(); // this can be used as index
cloudKeyPoses3D->push_back(thisPose3D);
// cloudKeyPoses6D 加入当前帧位姿
//只有6D中有时间戳
thisPose6D.x = thisPose3D.x;
thisPose6D.y = thisPose3D.y;
thisPose6D.z = thisPose3D.z;
thisPose6D.intensity = thisPose3D.intensity ; // this can be used as index
thisPose6D.roll = latestEstimate.rotation().roll();
thisPose6D.pitch = latestEstimate.rotation().pitch();
thisPose6D.yaw = latestEstimate.rotation().yaw();
thisPose6D.time = timeLaserInfoCur;
cloudKeyPoses6D->push_back(thisPose6D);
// cout << "****************************************************" << endl;
// cout << "Pose covariance:" << endl;
// cout << isam->marginalCovariance(isamCurrentEstimate.size()-1) << endl << endl;
// 位姿协方差
poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size()-1);
// save updated transform
// transformTobeMapped更新当前帧位姿
transformTobeMapped[0] = latestEstimate.rotation().roll();
transformTobeMapped[1] = latestEstimate.rotation().pitch();
transformTobeMapped[2] = latestEstimate.rotation().yaw();
transformTobeMapped[3] = latestEstimate.translation().x();
transformTobeMapped[4] = latestEstimate.translation().y();
transformTobeMapped[5] = latestEstimate.translation().z();
// save all the received edge and surf points
// 当前帧激光角点、平面点，降采样集合
pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>());
pcl::copyPointCloud(*laserCloudCornerLastDS, *thisCornerKeyFrame);
pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame);
// save key frame cloud
// 保存特征点降采样集合
cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
surfCloudKeyFrames.push_back(thisSurfKeyFrame);
// save path for visualization
// 更新里程计轨迹，也就是将 thisPose6D 的位姿加入到 "/lidar/mapping/path" 中去
updatePath(thisPose6D);
}
// 激光里程计因子
void addOdomFactor()
{
// 检查是否有关键帧点
if (cloudKeyPoses3D->points.empty())
{
// 第一帧初始化先验因子
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*meter
gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise));
// 变量节点设置初始值
initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped));
}else{
// 添加激光里程计因子
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back());
gtsam::Pose3 poseTo = trans2gtsamPose(transformTobeMapped);
// 添加关键帧最近的两个位姿
// 参数：前一帧id，当前帧id，前一帧与当前帧的位姿变换（作为观测值），噪声协方差
gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise));
// 变量节点设置初始值
//下标
initialEstimate.insert(cloudKeyPoses3D->size(), poseTo);
// if (isDegenerate)
// {
// adding VINS constraints is deleted as benefits are not obvious, disable for now
// gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), vinsPoseFrom.between(vinsPoseTo), odometryNoise));
// }
}
}
// 闭环因子
void addLoopFactor()
{
if (loopIndexQueue.empty())
return;
for (size_t i = 0; i < loopIndexQueue.size(); ++i)
{
// 添加相邻两个回环位姿
int indexFrom = loopIndexQueue[i].first;
int indexTo = loopIndexQueue[i].second;
gtsam::Pose3 poseBetween = loopPoseQueue[i];
gtsam::noiseModel::Diagonal::shared_ptr noiseBetween = loopNoiseQueue[i];
//添加到因子图中
gtSAMgraph.add(BetweenFactor<Pose3>(indexFrom, indexTo, poseBetween, noiseBetween));
}
// 清空所有的回环数据队列
loopIndexQueue.clear();
loopPoseQueue.clear();
loopNoiseQueue.clear();
aLoopIsClosed = true;
}
// 更新里程计轨迹，也就是将 thisPose6D 的位姿加入到 "/lidar/mapping/path" 中去
// 更新路径（位置坐标 + 旋转四元数）
void updatePath(const PointTypePose& pose_in)
{
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header.stamp = ros::Time().fromSec(pose_in.time);
pose_stamped.header.frame_id = "odom";
pose_stamped.pose.position.x = pose_in.x;
pose_stamped.pose.position.y = pose_in.y;
pose_stamped.pose.position.z = pose_in.z;
tf::Quaternion q = tf::createQuaternionFromRPY(pose_in.roll, pose_in.pitch, pose_in.yaw);
pose_stamped.pose.orientation.x = q.x();
pose_stamped.pose.orientation.y = q.y();
pose_stamped.pose.orientation.z = q.z();
pose_stamped.pose.orientation.w = q.w();
globalPath.poses.push_back(pose_stamped);
}
6 correctPoses()
更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹
void correctPoses()
{
if (cloudKeyPoses3D->points.empty())
return;
if (aLoopIsClosed == true)
{
// clear path
// 清空里程计轨迹
globalPath.poses.clear();
// update key poses
int numPoses = isamCurrentEstimate.size();
// 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿
for (int i = 0; i < numPoses; ++i)
{
cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().x();
cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().y();
cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().z();
cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x;
cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y;
cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z;
cloudKeyPoses6D->points[i].roll = isamCurrentEstimate.at<Pose3>(i).rotation().roll();
cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<Pose3>(i).rotation().pitch();
cloudKeyPoses6D->points[i].yaw = isamCurrentEstimate.at<Pose3>(i).rotation().yaw();
//将修正好的位姿逐一再添加会globalPath中
updatePath(cloudKeyPoses6D->points[i]);
}
aLoopIsClosed = false;
// ID for reseting IMU pre-integration
++imuPreintegrationResetId;
}
}
7 publishOdometry()
发布激光里程计
void publishOdometry()
{
// Publish odometry for ROS// 发布激光里程计，odom等价map
nav_msgs::Odometry laserOdometryROS;
laserOdometryROS.header.stamp = timeLaserInfoStamp;
laserOdometryROS.header.frame_id = "odom";
laserOdometryROS.child_frame_id = "odom_mapping";
laserOdometryROS.pose.pose.position.x = transformTobeMapped[3];
laserOdometryROS.pose.pose.position.y = transformTobeMapped[4];
laserOdometryROS.pose.pose.position.z = transformTobeMapped[5];
laserOdometryROS.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
laserOdometryROS.pose.covariance[0] = double(imuPreintegrationResetId);
pubOdomAftMappedROS.publish(laserOdometryROS);
// Publish TF
// 发布TF，odom->lidar
static tf::TransformBroadcaster br;
tf::Transform t_odom_to_lidar = tf::Transform(tf::createQuaternionFromRPY(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]),
tf::Vector3(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5]));
tf::StampedTransform trans_odom_to_lidar = tf::StampedTransform(t_odom_to_lidar, timeLaserInfoStamp, "odom", "lidar_link");
br.sendTransform(trans_odom_to_lidar);
}
8 publishFrames()
发布里程计、点云、轨迹
void publishFrames()
{
if (cloudKeyPoses3D->points.empty())
return;
// publish key poses
// 发布历史关键帧位姿集合
publishCloud(&pubKeyPoses, cloudKeyPoses6D, timeLaserInfoStamp, "odom");
// Publish surrounding key frames
// 发布局部map的降采样特征点集合
publishCloud(&pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, "odom");
// publish registered key frame
// 发布历史帧（累加的）的角点、平面点降采样集合
if (pubRecentKeyFrame.getNumSubscribers() != 0)
{
pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
//transformPointCloud 作用是返回输入点云乘以输入的 transform
*cloudOut += *transformPointCloud(laserCloudCornerLastDS, &thisPose6D);
*cloudOut += *transformPointCloud(laserCloudSurfLastDS, &thisPose6D);
publishCloud(&pubRecentKeyFrame, cloudOut, timeLaserInfoStamp, "odom");
}
// publish registered high-res raw cloud 发布配准的高分辨率原始云
if (pubCloudRegisteredRaw.getNumSubscribers() != 0)
{
pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
pcl::fromROSMsg(cloudInfo.cloud_deskewed, *cloudOut);
PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped);
*cloudOut = *transformPointCloud(cloudOut, &thisPose6D);
publishCloud(&pubCloudRegisteredRaw, cloudOut, timeLaserInfoStamp, "odom");
}
// publish path 发布路径
if (pubPath.getNumSubscribers() != 0)
{
globalPath.header.stamp = timeLaserInfoStamp;
globalPath.header.frame_id = "odom";
pubPath.publish(globalPath);
}
}