LeGO-LOAM算法分析之mapOptmization
LeGO-LOAM将当前帧扫描到点云特征（线特征和面特征）与当前帧周围的点云地图进行匹配，进行位姿优化。
代码分析
从主函数开始分析，主函数分为2个主要的功能，一是启动单独的线程进行回环检测，二是在循环中执行主流程。
int main(int argc, char** argv)
{
ros::init(argc, argv, "lego_loam");
ROS_INFO("\033[1;32m---->\033[0m Map Optimization Started.");
mapOptimization MO;
// std::thread 构造函数，将MO作为参数传入构造的线程中使用
// 进行闭环检测与闭环的功能
std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
// 该线程中进行的工作是publishGlobalMap(),将数据发布到ros中，可视化
std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
ros::Rate rate(200);
while (ros::ok())
{
ros::spinOnce();
MO.run();
rate.sleep();
}
loopthread.join();
visualizeMapThread.join();
return 0;
}
主流程
run函数
void run(){
if (newLaserCloudCornerLast && std::abs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 &&
newLaserCloudSurfLast && std::abs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 &&
newLaserCloudOutlierLast && std::abs(timeLaserCloudOutlierLast - timeLaserOdometry) < 0.005 &&
newLaserOdometry)
{
newLaserCloudCornerLast = false; newLaserCloudSurfLast = false; newLaserCloudOutlierLast = false; newLaserOdometry = false;
std::lock_guard lock(mtx);
if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {
timeLastProcessing = timeLaserOdometry;
//转换到map坐标系下
transformAssociateToMap();
//提取周围的关键帧
extractSurroundingKeyFrames();
//下采样当前帧
downsampleCurrentScan();
// 当前扫描进行边缘优化，图优化以及进行LM优化的过程
scan2MapOptimization();
//保存关键帧和因子
saveKeyFramesAndFactor();
//校正位姿
correctPoses();
//发布坐标变换
publishTF();
//发布关键帧和因子
publishKeyPosesAndFrames();
//清除点云
clearCloud();
}
}
} 提取周围关键帧
如果使能了回环检测，则添加过去50个关键帧，如果没有使能回环检测，则添加离当前欧式距离最近的50个关键帧，然后拼接成点云。
void extractSurroundingKeyFrames(){
if (cloudKeyPoses3D->points.empty() == true)
return;
// loopClosureEnableFlag 这个变量另外只在loopthread这部分中有用到
if (loopClosureEnableFlag == true){
// recentCornerCloudKeyFrames保存的点云数量太少，则清空后重新塞入新的点云直至数量够
if (recentCornerCloudKeyFrames.size() < surroundingKeyframeSearchNum){
recentCornerCloudKeyFrames. clear();
recentSurfCloudKeyFrames. clear();
recentOutlierCloudKeyFrames.clear();
int numPoses = cloudKeyPoses3D->points.size();
for (int i = numPoses-1; i >= 0; --i){
// cloudKeyPoses3D的intensity中存的是索引值?
// 保存的索引值从1开始编号；
int thisKeyInd = (int)cloudKeyPoses3D->points[i].intensity;
PointTypePose thisTransformation = cloudKeyPoses6D->points[thisKeyInd];
updateTransformPointCloudSinCos(&thisTransformation);
// 依据上面得到的变换thisTransformation，对cornerCloudKeyFrames，surfCloudKeyFrames，surfCloudKeyFrames
// 进行坐标变换
recentCornerCloudKeyFrames. push_front(transformPointCloud(cornerCloudKeyFrames[thisKeyInd]));
recentSurfCloudKeyFrames. push_front(transformPointCloud(surfCloudKeyFrames[thisKeyInd]));
recentOutlierCloudKeyFrames.push_front(transformPointCloud(outlierCloudKeyFrames[thisKeyInd]));
if (recentCornerCloudKeyFrames.size() >= surroundingKeyframeSearchNum)
break;
}
}else{
// recentCornerCloudKeyFrames中点云保存的数量较多
// pop队列最前端的一个，再push后面一个
if (latestFrameID != cloudKeyPoses3D->points.size() - 1){
recentCornerCloudKeyFrames. pop_front();
recentSurfCloudKeyFrames. pop_front();
recentOutlierCloudKeyFrames.pop_front();
// 为什么要把recentCornerCloudKeyFrames最前面第一个元素弹出?
// (1个而不是好几个或者是全部)?
latestFrameID = cloudKeyPoses3D->points.size() - 1;
PointTypePose thisTransformation = cloudKeyPoses6D->points[latestFrameID];
updateTransformPointCloudSinCos(&thisTransformation);
recentCornerCloudKeyFrames. push_back(transformPointCloud(cornerCloudKeyFrames[latestFrameID]));
recentSurfCloudKeyFrames. push_back(transformPointCloud(surfCloudKeyFrames[latestFrameID]));
recentOutlierCloudKeyFrames.push_back(transformPointCloud(outlierCloudKeyFrames[latestFrameID]));
}
}
for (int i = 0; i < recentCornerCloudKeyFrames.size(); ++i){
// 两个pcl::PointXYZI相加?
// 注意这里把recentOutlierCloudKeyFrames也加入到了laserCloudSurfFromMap
*laserCloudCornerFromMap += *recentCornerCloudKeyFrames[i];
*laserCloudSurfFromMap += *recentSurfCloudKeyFrames[i];
*laserCloudSurfFromMap += *recentOutlierCloudKeyFrames[i];
}
}else{
// 下面这部分是没有闭环的代码
//
surroundingKeyPoses->clear();
surroundingKeyPosesDS->clear();
kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D);
// 进行半径surroundingKeyframeSearchRadius内的邻域搜索，
// currentRobotPosPoint：需要查询的点，
// pointSearchInd：搜索完的邻域点对应的索引
// pointSearchSqDis：搜索完的每个领域点点与传讯点之间的欧式距离
// 0：返回的邻域个数，为0表示返回全部的邻域点
kdtreeSurroundingKeyPoses->radiusSearch(currentRobotPosPoint, (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis, 0);
for (int i = 0; i < pointSearchInd.size(); ++i)
surroundingKeyPoses->points.push_back(cloudKeyPoses3D->points[pointSearchInd[i]]);
downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);
int numSurroundingPosesDS = surroundingKeyPosesDS->points.size();
for (int i = 0; i < surroundingExistingKeyPosesID.size(); ++i){
bool existingFlag = false;
for (int j = 0; j < numSurroundingPosesDS; ++j){
// 双重循环，不断对比surroundingExistingKeyPosesID[i]和surroundingKeyPosesDS的点的index
// 如果能够找到一样的，说明存在相同的关键点(因为surroundingKeyPosesDS从cloudKeyPoses3D中筛选而来)
if (surroundingExistingKeyPosesID[i] == (int)surroundingKeyPosesDS->points[j].intensity){
existingFlag = true;
break;
}
}
if (existingFlag == false){
// 如果surroundingExistingKeyPosesID[i]对比了一轮的已经存在的关键位姿的索引后（intensity保存的就是size()）
// 没有找到相同的关键点，那么把这个点从当前队列中删除
// 否则的话，existingFlag为true，该关键点就将它留在队列中
surroundingExistingKeyPosesID. erase(surroundingExistingKeyPosesID. begin() + i);
surroundingCornerCloudKeyFrames. erase(surroundingCornerCloudKeyFrames. begin() + i);
surroundingSurfCloudKeyFrames. erase(surroundingSurfCloudKeyFrames. begin() + i);
surroundingOutlierCloudKeyFrames.erase(surroundingOutlierCloudKeyFrames.begin() + i);
--i;
}
}
// 上一个两重for循环主要用于删除数据，此处的两重for循环用来添加数据
for (int i = 0; i < numSurroundingPosesDS; ++i) {
bool existingFlag = false;
for (auto iter = surroundingExistingKeyPosesID.begin(); iter != surroundingExistingKeyPosesID.end(); ++iter){
// *iter就是不同的cloudKeyPoses3D->points.size(),
// 把surroundingExistingKeyPosesID内没有对应的点放进一个队列里
// 这个队列专门存放周围存在的关键帧，但是和surroundingExistingKeyPosesID的点没有对应的，也就是新的点
if ((*iter) == (int)surroundingKeyPosesDS->points[i].intensity){
existingFlag = true;
break;
}
}
if (existingFlag == true){
continue;
}else{
int thisKeyInd = (int)surroundingKeyPosesDS->points[i].intensity;
PointTypePose thisTransformation = cloudKeyPoses6D->points[thisKeyInd];
updateTransformPointCloudSinCos(&thisTransformation);
surroundingExistingKeyPosesID. push_back(thisKeyInd);
surroundingCornerCloudKeyFrames. push_back(transformPointCloud(cornerCloudKeyFrames[thisKeyInd]));
surroundingSurfCloudKeyFrames. push_back(transformPointCloud(surfCloudKeyFrames[thisKeyInd]));
surroundingOutlierCloudKeyFrames.push_back(transformPointCloud(outlierCloudKeyFrames[thisKeyInd]));
}
}
for (int i = 0; i < surroundingExistingKeyPosesID.size(); ++i) {
*laserCloudCornerFromMap += *surroundingCornerCloudKeyFrames[i];
*laserCloudSurfFromMap += *surroundingSurfCloudKeyFrames[i];
*laserCloudSurfFromMap += *surroundingOutlierCloudKeyFrames[i];
}
}
// 进行两次下采样
// 最后的输出结果是laserCloudCornerFromMapDS和laserCloudSurfFromMapDS
downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->points.size();
downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->points.size();
}scan到map的优化
通过线面特征计算距离，使用LM优化（其实是高斯牛顿法）得到最优位姿变换
void scan2MapOptimization(){
// laserCloudCornerFromMapDSNum是extractSurroundingKeyFrames()函数最后降采样得到的coner点云数
// laserCloudSurfFromMapDSNum是extractSurroundingKeyFrames()函数降采样得到的surface点云数
if (laserCloudCornerFromMapDSNum > 10 && laserCloudSurfFromMapDSNum > 100) {
// laserCloudCornerFromMapDS和laserCloudSurfFromMapDS的来源有2个：
// 当有闭环时，来源是：recentCornerCloudKeyFrames，没有闭环时，来源surroundingCornerCloudKeyFrames
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);
for (int iterCount = 0; iterCount < 10; iterCount++) {
// 用for循环控制迭代次数，最多迭代10次
laserCloudOri->clear();
coeffSel->clear();
//线和面特征计算距离
cornerOptimization(iterCount);
surfOptimization(iterCount);
//LM优化
if (LMOptimization(iterCount) == true)
break;
}
// 迭代结束更新相关的转移矩阵
transformUpdate();
}
}
线特征优化
void cornerOptimization(int iterCount){
updatePointAssociateToMapSinCos();
for (int i = 0; i < laserCloudCornerLastDSNum; i++) {
pointOri = laserCloudCornerLastDS->points[i];
// 进行坐标变换,转换到全局坐标中去（世界坐标系）
// pointSel:表示选中的点，point select
// 输入是pointOri，输出是pointSel
pointAssociateToMap(&pointOri, &pointSel);
// 进行5邻域搜索，
// pointSel为需要搜索的点，
// pointSearchInd搜索完的邻域对应的索引
// pointSearchSqDis 邻域点与查询点之间的距离
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
// 只有当最远的那个邻域点的距离pointSearchSqDis[4]小于1m时才进行下面的计算
// 以下部分的计算是在计算点集的协方差矩阵，Zhang Ji的论文中有提到这部分
if (pointSearchSqDis[4] < 1.0) {
// 先求5个样本的平均值
float cx = 0, cy = 0, cz = 0;
for (int j = 0; j < 5; j++) {
cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
}
cx /= 5; cy /= 5; cz /= 5;
// 下面在求矩阵matA1=[ax,ay,az]^t*[ax,ay,az]
// 更准确地说应该是在求协方差matA1
float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
for (int j = 0; j < 5; j++) {
// ax代表的是x-cx,表示均值与每个实际值的差值，求取5个之后再次取平均，得到matA1
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(0, 0) = a11; matA1.at(0, 1) = a12; matA1.at(0, 2) = a13;
matA1.at(1, 0) = a12; matA1.at(1, 1) = a22; matA1.at(1, 2) = a23;
matA1.at(2, 0) = a13; matA1.at(2, 1) = a23; matA1.at(2, 2) = a33;
// 求正交阵的特征值和特征向量
// 特征值：matD1，特征向量：matV1中
cv::eigen(matA1, matD1, matV1);
// 边缘：与较大特征值相对应的特征向量代表边缘线的方向（一大两小，大方向）
// 以下这一大块是在计算点到边缘的距离，最后通过系数s来判断是否距离很近
// 如果距离很近就认为这个点在边缘上，需要放到laserCloudOri中
if (matD1.at(0, 0) > 3 * matD1.at(0, 1)) {
float x0 = pointSel.x;
float y0 = pointSel.y;
float z0 = pointSel.z;
float x1 = cx + 0.1 * matV1.at(0, 0);
float y1 = cy + 0.1 * matV1.at(0, 1);
float z1 = cz + 0.1 * matV1.at(0, 2);
float x2 = cx - 0.1 * matV1.at(0, 0);
float y2 = cy - 0.1 * matV1.at(0, 1);
float z2 = cz - 0.1 * matV1.at(0, 2);
// 这边是在求[(x0-x1),(y0-y1),(z0-z1)]与[(x0-x2),(y0-y2),(z0-z2)]叉乘得到的向量的模长
// 这个模长是由0.2*V1[0]和点[x0,y0,z0]构成的平行四边形的面积
// 因为[(x0-x1),(y0-y1),(z0-z1)]x[(x0-x2),(y0-y2),(z0-z2)]=[XXX,YYY,ZZZ],
// [XXX,YYY,ZZZ]=[(y0-y1)(z0-z2)-(y0-y2)(z0-z1),-(x0-x1)(z0-z2)+(x0-x2)(z0-z1),(x0-x1)(y0-y2)-(x0-x2)(y0-y1)]
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)));
// l12表示的是0.2*(||V1[0]||)
// 也就是平行四边形一条底的长度
float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
// 求叉乘结果[la',lb',lc']=[(x1-x2),(y1-y2),(z1-z2)]x[XXX,YYY,ZZZ]
// [la,lb,lc]=[la',lb',lc']/a012/l12
// LLL=[la,lb,lc]是0.2*V1[0]这条高上的单位法向量。||LLL||=1；
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;
// 计算点pointSel到直线的距离
// 这里需要特别说明的是ld2代表的是点pointSel到过点[cx,cy,cz]的方向向量直线的距离
float ld2 = a012 / l12;
// 如果在最理想的状态的话，ld2应该为0，表示点在直线上
// 最理想状态s=1；
float s = 1 - 0.9 * fabs(ld2);
// coeff代表系数的意思
// coff用于保存距离的方向向量
coeff.x = s * la;
coeff.y = s * lb;
coeff.z = s * lc;
// intensity本质上构成了一个核函数，ld2越接近于1，增长越慢
// intensity=(1-0.9*ld2)*ld2=ld2-0.9*ld2*ld2
coeff.intensity = s * ld2;
// 所以就应该认为这个点是边缘点
// s>0.1 也就是要求点到直线的距离ld2要小于1m
// s越大说明ld2越小(离边缘线越近)，这样就说明点pointOri在直线上
if (s > 0.1) {
laserCloudOri->push_back(pointOri);
coeffSel->push_back(coeff);
}
}
}
}
} 面特征优化
void surfOptimization(int iterCount){
updatePointAssociateToMapSinCos();
for (int i = 0; i < laserCloudSurfTotalLastDSNum; i++) {
pointOri = laserCloudSurfTotalLastDS->points[i];
pointAssociateToMap(&pointOri, &pointSel);
kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
if (pointSearchSqDis[4] < 1.0) {
for (int j = 0; j < 5; j++) {
matA0.at(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
matA0.at(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
matA0.at(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
}
// matB0是一个5x1的矩阵
// matB0 = cv::Mat (5, 1, CV_32F, cv::Scalar::all(-1));
// matX0是3x1的矩阵
// 求解方程matA0*matX0=matB0
// 公式其实是在求由matA0中的点构成的平面的法向量matX0
cv::solve(matA0, matB0, matX0, cv::DECOMP_QR);
// [pa,pb,pc,pd]=[matX0,pd]
// 正常情况下（见后面planeValid判断条件），应该是
// pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
// pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
// pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z = -1
// 所以pd设置为1
float pa = matX0.at(0, 0);
float pb = matX0.at(1, 0);
float pc = matX0.at(2, 0);
float pd = 1;
// 对[pa,pb,pc,pd]进行单位化
float ps = sqrt(pa * pa + pb * pb + pc * pc);
pa /= ps; pb /= ps; pc /= ps; pd /= ps;
// 求解后再次检查平面是否是有效平面
bool planeValid = true;
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;
// 后面部分相除求的是[pa,pb,pc,pd]与pointSel的夹角余弦值(两个sqrt，其实并不是余弦值)
// 这个夹角余弦值越小越好，越小证明所求的[pa,pb,pc,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;
// 判断是否是合格平面，是就加入laserCloudOri
if (s > 0.1) {
laserCloudOri->push_back(pointOri);
coeffSel->push_back(coeff);
}
}
}
}
} LM优化
// 这部分的代码是基于高斯牛顿法的优化，不是zhang ji论文中提到的基于L-M的优化方法
// 这部分的代码使用旋转矩阵对欧拉角求导，优化欧拉角，不是zhang ji论文中提到的使用angle-axis的优化
bool LMOptimization(int iterCount){
float srx = sin(transformTobeMapped[0]);
float crx = cos(transformTobeMapped[0]);
float sry = sin(transformTobeMapped[1]);
float cry = cos(transformTobeMapped[1]);
float srz = sin(transformTobeMapped[2]);
float crz = cos(transformTobeMapped[2]);
int laserCloudSelNum = laserCloudOri->points.size();
// laser cloud original 点云太少，就跳过这次循环
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));
for (int i = 0; i < laserCloudSelNum; i++) {
pointOri = laserCloudOri->points[i];
coeff = coeffSel->points[i];
// 求雅克比矩阵中的元素，距离d对roll角度的偏导量即d(d)/d(roll)
// 更详细的数学推导参看wykxwyc.github.io
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;
// 同上，求解的是对pitch的偏导量
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;
/*
在求点到直线的距离时，coeff表示的是如下内容
[la,lb,lc]表示的是点到直线的垂直连线方向，s是长度
coeff.x = s * la;
coeff.y = s * lb;
coeff.z = s * lc;
coeff.intensity = s * ld2;
在求点到平面的距离时，coeff表示的是
[pa,pb,pc]表示过外点的平面的法向量，s是线的长度
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
coeff.intensity = s * pd2;
*/
matA.at(i, 0) = arx;
matA.at(i, 1) = ary;
matA.at(i, 2) = arz;
// 这部分是雅克比矩阵中距离对平移的偏导
matA.at(i, 3) = coeff.x;
matA.at(i, 4) = coeff.y;
matA.at(i, 5) = coeff.z;
// 残差项
matB.at(i, 0) = -coeff.intensity;
}
// 将矩阵由matA转置生成matAt
// 先进行计算，以便于后边调用 cv::solve求解
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
// 利用高斯牛顿法进行求解，
// 高斯牛顿法的原型是J^(T)*J * delta(x) = -J*f(x)
// J是雅克比矩阵，这里是A，f(x)是优化目标，这里是-B(符号在给B赋值时候就放进去了)
// 通过QR分解的方式，求解matAtA*matX=matAtB，得到解matX
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
// iterCount==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矩阵求特征值和特征向量，
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(0, i) < eignThre[i]) {
for (int j = 0; j < 6; j++) {
matV2.at(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(0, 0);
transformTobeMapped[1] += matX.at(1, 0);
transformTobeMapped[2] += matX.at(2, 0);
transformTobeMapped[3] += matX.at(3, 0);
transformTobeMapped[4] += matX.at(4, 0);
transformTobeMapped[5] += matX.at(5, 0);
float deltaR = sqrt(
pow(pcl::rad2deg(matX.at(0, 0)), 2) +
pow(pcl::rad2deg(matX.at(1, 0)), 2) +
pow(pcl::rad2deg(matX.at(2, 0)), 2));
float deltaT = sqrt(
pow(matX.at(3, 0) * 100, 2) +
pow(matX.at(4, 0) * 100, 2) +
pow(matX.at(5, 0) * 100, 2));
// 旋转或者平移量足够小就停止这次迭代过程
if (deltaR < 0.05 && deltaT < 0.05) {
return true;
}
return false;
} 保存关键帧和因子
通过前后两帧进行优化，transformTobeMapped上一帧待优化的位姿，transformAftMapped优化后的位姿，保存优化后的位姿。
void saveKeyFramesAndFactor(){
currentRobotPosPoint.x = transformAftMapped[3];
currentRobotPosPoint.y = transformAftMapped[4];
currentRobotPosPoint.z = transformAftMapped[5];
bool saveThisKeyFrame = true;
if (sqrt((previousRobotPosPoint.x-currentRobotPosPoint.x)*(previousRobotPosPoint.x-currentRobotPosPoint.x)
+(previousRobotPosPoint.y-currentRobotPosPoint.y)*(previousRobotPosPoint.y-currentRobotPosPoint.y)
+(previousRobotPosPoint.z-currentRobotPosPoint.z)*(previousRobotPosPoint.z-currentRobotPosPoint.z)) < 0.3){
saveThisKeyFrame = false;
}
if (saveThisKeyFrame == false && !cloudKeyPoses3D->points.empty())
return;
previousRobotPosPoint = currentRobotPosPoint;
if (cloudKeyPoses3D->points.empty()){
// static Rot3 RzRyRx (double x, double y, double z),Rotations around Z, Y, then X axes
// RzRyRx依次按照z(transformTobeMapped[2])，y(transformTobeMapped[0])，x(transformTobeMapped[1])坐标轴旋转
// Point3 (double x, double y, double z) Construct from x(transformTobeMapped[5]), y(transformTobeMapped[3]), and z(transformTobeMapped[4]) coordinates.
// Pose3 (const Rot3 &R, const Point3 &t) Construct from R,t. 从旋转和平移构造姿态
// NonlinearFactorGraph增加一个PriorFactor因子
gtSAMgraph.add(PriorFactor(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),
Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])), priorNoise));
// initialEstimate的数据类型是Values,其实就是一个map，这里在0对应的值下面保存了一个Pose3
initialEstimate.insert(0, Pose3(Rot3::RzRyRx(transformTobeMapped[2], transformTobeMapped[0], transformTobeMapped[1]),
Point3(transformTobeMapped[5], transformTobeMapped[3], transformTobeMapped[4])));
for (int i = 0; i < 6; ++i)
transformLast[i] = transformTobeMapped[i];
}
else{
gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(transformLast[2], transformLast[0], transformLast[1]),
Point3(transformLast[5], transformLast[3], transformLast[4]));
gtsam::Pose3 poseTo = Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]),
Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4]));
// 构造函数原型:BetweenFactor (Key key1, Key key2, const VALUE &measured, const SharedNoiseModel &model)
gtSAMgraph.add(BetweenFactor(cloudKeyPoses3D->points.size()-1, cloudKeyPoses3D->points.size(), poseFrom.between(poseTo), odometryNoise));
initialEstimate.insert(cloudKeyPoses3D->points.size(), Pose3(Rot3::RzRyRx(transformAftMapped[2], transformAftMapped[0], transformAftMapped[1]),
Point3(transformAftMapped[5], transformAftMapped[3], transformAftMapped[4])));
}
// gtsam::ISAM2::update函数原型:
// ISAM2Result gtsam::ISAM2::update ( const NonlinearFactorGraph & newFactors = NonlinearFactorGraph(),
// const Values & newTheta = Values(),
// const std::vector< size_t > & removeFactorIndices = std::vector(),
// const boost::optional< FastMap< Key, int > > & constrainedKeys = boost::none,
// const boost::optional< FastList< Key > > & noRelinKeys = boost::none,
// const boost::optional< FastList< Key > > & extraReelimKeys = boost::none,
// bool force_relinearize = false )
// gtSAMgraph是新加到系统中的因子
// initialEstimate是加到系统中的新变量的初始点
isam->update(gtSAMgraph, initialEstimate);
// update 函数为什么需要调用两次？
isam->update();
// 删除内容?
gtSAMgraph.resize(0);
initialEstimate.clear();
PointType thisPose3D;
PointTypePose thisPose6D;
Pose3 latestEstimate;
// Compute an estimate from the incomplete linear delta computed during the last update.
isamCurrentEstimate = isam->calculateEstimate();
latestEstimate = isamCurrentEstimate.at(isamCurrentEstimate.size()-1);
thisPose3D.x = latestEstimate.translation().y();
thisPose3D.y = latestEstimate.translation().z();
thisPose3D.z = latestEstimate.translation().x();
thisPose3D.intensity = cloudKeyPoses3D->points.size();
cloudKeyPoses3D->push_back(thisPose3D);
thisPose6D.x = thisPose3D.x;
thisPose6D.y = thisPose3D.y;
thisPose6D.z = thisPose3D.z;
thisPose6D.intensity = thisPose3D.intensity;
thisPose6D.roll = latestEstimate.rotation().pitch();
thisPose6D.pitch = latestEstimate.rotation().yaw();
thisPose6D.yaw = latestEstimate.rotation().roll();
thisPose6D.time = timeLaserOdometry;
cloudKeyPoses6D->push_back(thisPose6D);
if (cloudKeyPoses3D->points.size() > 1){
transformAftMapped[0] = latestEstimate.rotation().pitch();
transformAftMapped[1] = latestEstimate.rotation().yaw();
transformAftMapped[2] = latestEstimate.rotation().roll();
transformAftMapped[3] = latestEstimate.translation().y();
transformAftMapped[4] = latestEstimate.translation().z();
transformAftMapped[5] = latestEstimate.translation().x();
for (int i = 0; i < 6; ++i){
transformLast[i] = transformAftMapped[i];
transformTobeMapped[i] = transformAftMapped[i];
}
}
pcl::PointCloud::Ptr thisCornerKeyFrame(new pcl::PointCloud());
pcl::PointCloud::Ptr thisSurfKeyFrame(new pcl::PointCloud());
pcl::PointCloud::Ptr thisOutlierKeyFrame(new pcl::PointCloud());
// PCL::copyPointCloud(const pcl::PCLPointCloud2 &cloud_in,pcl::PCLPointCloud2 &cloud_out )
pcl::copyPointCloud(*laserCloudCornerLastDS, *thisCornerKeyFrame);
pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame);
pcl::copyPointCloud(*laserCloudOutlierLastDS, *thisOutlierKeyFrame);
cornerCloudKeyFrames.push_back(thisCornerKeyFrame);
surfCloudKeyFrames.push_back(thisSurfKeyFrame);
outlierCloudKeyFrames.push_back(thisOutlierKeyFrame);
} 校正位姿
回环检测如果成功，会设置aLoopIsClosed为true，才会进行这一步。将isam优化后的位姿替换之前的位姿。
void correctPoses(){
if (aLoopIsClosed == true){
recentCornerCloudKeyFrames. clear();
recentSurfCloudKeyFrames. clear();
recentOutlierCloudKeyFrames.clear();
int numPoses = isamCurrentEstimate.size();
for (int i = 0; i < numPoses; ++i)
{
cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at(i).translation().y();
cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at(i).translation().z();
cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at(i).translation().x();
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(i).rotation().pitch();
cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at(i).rotation().yaw();
cloudKeyPoses6D->points[i].yaw = isamCurrentEstimate.at(i).rotation().roll();
}
aLoopIsClosed = false;
}
} 闭环检测
回环检测在loopClosureThread中进行，判断是否进入回环在detectLoopClosure中进行，判断条件是首尾之间的距离小于7米，并且时间相差30s以上。在 performLoopClosure()中检测到回环后进行ICP匹配，进行图优化。
void loopClosureThread(){
if (loopClosureEnableFlag == false)
return;
ros::Rate rate(1);
while (ros::ok()){
rate.sleep();
performLoopClosure();
}
}
bool detectLoopClosure(){
latestSurfKeyFrameCloud->clear();
nearHistorySurfKeyFrameCloud->clear();
nearHistorySurfKeyFrameCloudDS->clear();
// 资源分配时初始化
// 在互斥量被析构前不解锁
std::lock_guard lock(mtx);
std::vector pointSearchIndLoop;
std::vector pointSearchSqDisLoop;
kdtreeHistoryKeyPoses->setInputCloud(cloudKeyPoses3D);
// 进行半径historyKeyframeSearchRadius内的邻域搜索，
// currentRobotPosPoint：需要查询的点，
// pointSearchIndLoop：搜索完的邻域点对应的索引
// pointSearchSqDisLoop：搜索完的每个邻域点与当前点之间的欧式距离
// 0：返回的邻域个数，为0表示返回全部的邻域点
kdtreeHistoryKeyPoses->radiusSearch(currentRobotPosPoint, historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);
closestHistoryFrameID = -1;
for (int i = 0; i < pointSearchIndLoop.size(); ++i){
int id = pointSearchIndLoop[i];
// 两个时间差值大于30秒
if (abs(cloudKeyPoses6D->points[id].time - timeLaserOdometry) > 30.0){
closestHistoryFrameID = id;
break;
}
}
if (closestHistoryFrameID == -1){
// 找到的点和当前时间上没有超过30秒的
return false;
}
latestFrameIDLoopCloure = cloudKeyPoses3D->points.size() - 1;
// 点云的xyz坐标进行坐标系变换(分别绕xyz轴旋转)
*latestSurfKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[latestFrameIDLoopCloure], &cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
*latestSurfKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[latestFrameIDLoopCloure], &cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
// latestSurfKeyFrameCloud中存储的是下面公式计算后的index(intensity):
// thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;
// 滤掉latestSurfKeyFrameCloud中index<0的点??? index值会小于0?
pcl::PointCloud::Ptr hahaCloud(new pcl::PointCloud());
int cloudSize = latestSurfKeyFrameCloud->points.size();
for (int i = 0; i < cloudSize; ++i){
// intensity不小于0的点放进hahaCloud队列
// 初始化时intensity是-1，滤掉那些点
if ((int)latestSurfKeyFrameCloud->points[i].intensity >= 0){
hahaCloud->push_back(latestSurfKeyFrameCloud->points[i]);
}
}
latestSurfKeyFrameCloud->clear();
*latestSurfKeyFrameCloud = *hahaCloud;
// historyKeyframeSearchNum在utility.h中定义为25，前后25个点进行变换
for (int j = -historyKeyframeSearchNum; j <= historyKeyframeSearchNum; ++j){
if (closestHistoryFrameID + j < 0 || closestHistoryFrameID + j > latestFrameIDLoopCloure)
continue;
// 要求closestHistoryFrameID + j在0到cloudKeyPoses3D->points.size()-1之间,不能超过索引
*nearHistorySurfKeyFrameCloud += *transformPointCloud(cornerCloudKeyFrames[closestHistoryFrameID+j], &cloudKeyPoses6D->points[closestHistoryFrameID+j]);
*nearHistorySurfKeyFrameCloud += *transformPointCloud(surfCloudKeyFrames[closestHistoryFrameID+j], &cloudKeyPoses6D->points[closestHistoryFrameID+j]);
}
// 下采样滤波减少数据量
downSizeFilterHistoryKeyFrames.setInputCloud(nearHistorySurfKeyFrameCloud);
downSizeFilterHistoryKeyFrames.filter(*nearHistorySurfKeyFrameCloudDS);
if (pubHistoryKeyFrames.getNumSubscribers() != 0){
sensor_msgs::PointCloud2 cloudMsgTemp;
pcl::toROSMsg(*nearHistorySurfKeyFrameCloudDS, cloudMsgTemp);
cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);
cloudMsgTemp.header.frame_id = "/camera_init";
pubHistoryKeyFrames.publish(cloudMsgTemp);
}
return true;
}
void performLoopClosure(){
if (cloudKeyPoses3D->points.empty() == true)
return;
if (potentialLoopFlag == false){
if (detectLoopClosure() == true){
potentialLoopFlag = true;
timeSaveFirstCurrentScanForLoopClosure = timeLaserOdometry;
}
if (potentialLoopFlag == false)
return;
}
potentialLoopFlag = false;
pcl::IterativeClosestPoint icp;
icp.setMaxCorrespondenceDistance(100);
icp.setMaximumIterations(100);
icp.setTransformationEpsilon(1e-6);
icp.setEuclideanFitnessEpsilon(1e-6);
// 设置RANSAC运行次数
icp.setRANSACIterations(0);
icp.setInputSource(latestSurfKeyFrameCloud);
// 使用detectLoopClosure()函数中下采样刚刚更新nearHistorySurfKeyFrameCloudDS
icp.setInputTarget(nearHistorySurfKeyFrameCloudDS);
pcl::PointCloud::Ptr unused_result(new pcl::PointCloud());
// 进行icp点云对齐
icp.align(*unused_result);
// 为什么匹配分数高直接返回???分数高代表噪声太多
if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
return;
// 以下在点云icp收敛并且噪声量在一定范围内进行
if (pubIcpKeyFrames.getNumSubscribers() != 0){
pcl::PointCloud::Ptr closed_cloud(new pcl::PointCloud());
// icp.getFinalTransformation()的返回值是Eigen::Matrix
pcl::transformPointCloud (*latestSurfKeyFrameCloud, *closed_cloud, icp.getFinalTransformation());
sensor_msgs::PointCloud2 cloudMsgTemp;
pcl::toROSMsg(*closed_cloud, cloudMsgTemp);
cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);
cloudMsgTemp.header.frame_id = "/camera_init";
pubIcpKeyFrames.publish(cloudMsgTemp);
}
float x, y, z, roll, pitch, yaw;
Eigen::Affine3f correctionCameraFrame;
correctionCameraFrame = icp.getFinalTransformation();
// 得到平移和旋转的角度
pcl::getTranslationAndEulerAngles(correctionCameraFrame, x, y, z, roll, pitch, yaw);
Eigen::Affine3f correctionLidarFrame = pcl::getTransformation(z, x, y, yaw, roll, pitch);
Eigen::Affine3f tWrong = pclPointToAffine3fCameraToLidar(cloudKeyPoses6D->points[latestFrameIDLoopCloure]);
Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;
pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw);
gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
gtsam::Pose3 poseTo = pclPointTogtsamPose3(cloudKeyPoses6D->points[closestHistoryFrameID]);
gtsam::Vector Vector6(6);
float noiseScore = icp.getFitnessScore();
Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;
constraintNoise = noiseModel::Diagonal::Variances(Vector6);
std::lock_guard lock(mtx);
gtSAMgraph.add(BetweenFactor(latestFrameIDLoopCloure, closestHistoryFrameID, poseFrom.between(poseTo), constraintNoise));
isam->update(gtSAMgraph);
isam->update();
gtSAMgraph.resize(0);
aLoopIsClosed = true;
} 参考文章
LeGO-LOAM分析之建图（三） - 知乎