最近研究了一下loam的代码。几件了下面的几篇文章。
https://zhuanlan.zhihu.com/p/29719106
https://blog.csdn.net/nksjc/article/details/76401092
由于便于以后自己查看。所以再次把一些较难理解的算法的个人的见解写在了下面，方便以后自己的查看。
由于本人查看代码时间用的不多，所以应当有理解失当的地方，如果有，请发现的朋友批评指正。
由于涉及到一些公式的推到，所以在此将使用图片进行上传。
难点1对于odometry的如下代码：
int pointSelNum = laserCloudOri->points.size();
if (pointSelNum < 10) {
continue;
}
cv::Mat matA(pointSelNum, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matAt(6, pointSelNum, CV_32F, cv::Scalar::all(0));
cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matB(pointSelNum, 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 < pointSelNum; i++) {
pointOri = laserCloudOri->points[i]; // 当前时刻点坐标
coeff = coeffSel->points[i]; // 该点所对应的偏导数
float s = 1;
float srx = sin(s * transform[0]);
float crx = cos(s * transform[0]);
float sry = sin(s * transform[1]);
float cry = cos(s * transform[1]);
float srz = sin(s * transform[2]);
float crz = cos(s * transform[2]);
float tx = s * transform[3];
float ty = s * transform[4];
float tz = s * transform[5];
float arx = (-s*crx*sry*srz*pointOri.x + s*crx*crz*sry*pointOri.y + s*srx*sry*pointOri.z
+ s*tx*crx*sry*srz - s*ty*crx*crz*sry - s*tz*srx*sry) * coeff.x
+ (s*srx*srz*pointOri.x - s*crz*srx*pointOri.y + s*crx*pointOri.z
+ s*ty*crz*srx - s*tz*crx - s*tx*srx*srz) * coeff.y
+ (s*crx*cry*srz*pointOri.x - s*crx*cry*crz*pointOri.y - s*cry*srx*pointOri.z
+ s*tz*cry*srx + s*ty*crx*cry*crz - s*tx*crx*cry*srz) * coeff.z;
float ary = ((-s*crz*sry - s*cry*srx*srz)*pointOri.x
+ (s*cry*crz*srx - s*sry*srz)*pointOri.y - s*crx*cry*pointOri.z
+ tx*(s*crz*sry + s*cry*srx*srz) + ty*(s*sry*srz - s*cry*crz*srx)
+ s*tz*crx*cry) * coeff.x
+ ((s*cry*crz - s*srx*sry*srz)*pointOri.x
+ (s*cry*srz + s*crz*srx*sry)*pointOri.y - s*crx*sry*pointOri.z
+ s*tz*crx*sry - ty*(s*cry*srz + s*crz*srx*sry)
- tx*(s*cry*crz - s*srx*sry*srz)) * coeff.z;
float arz = ((-s*cry*srz - s*crz*srx*sry)*pointOri.x + (s*cry*crz - s*srx*sry*srz)*pointOri.y
+ tx*(s*cry*srz + s*crz*srx*sry) - ty*(s*cry*crz - s*srx*sry*srz)) * coeff.x
+ (-s*crx*crz*pointOri.x - s*crx*srz*pointOri.y
+ s*ty*crx*srz + s*tx*crx*crz) * coeff.y
+ ((s*cry*crz*srx - s*sry*srz)*pointOri.x + (s*crz*sry + s*cry*srx*srz)*pointOri.y
+ tx*(s*sry*srz - s*cry*crz*srx) - ty*(s*crz*sry + s*cry*srx*srz)) * coeff.z;
float atx = -s*(cry*crz - srx*sry*srz) * coeff.x + s*crx*srz * coeff.y
- s*(crz*sry + cry*srx*srz) * coeff.z;
float aty = -s*(cry*srz + crz*srx*sry) * coeff.x - s*crx*crz * coeff.y
- s*(sry*srz - cry*crz*srx) * coeff.z;
float atz = s*crx*sry * coeff.x - s*srx * coeff.y - s*crx*cry * coeff.z;
float d2 = coeff.intensity;
matA.at<float>(i, 0) = arx;
matA.at<float>(i, 1) = ary;
matA.at<float>(i, 2) = arz;
matA.at<float>(i, 3) = atx;
matA.at<float>(i, 4) = aty;
matA.at<float>(i, 5) = atz;
matB.at<float>(i, 0) = -0.05 * d2;
}
// 最小二乘计算(QR分解法)
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
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));
cv::eigen(matAtA, matE, matV);
matV.copyTo(matV2);
isDegenerate = false;
float eignThre[6] = {10, 10, 10, 10, 10, 10};
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;
}
transform[0] += matX.at<float>(0, 0);
transform[1] += matX.at<float>(1, 0);
transform[2] += matX.at<float>(2, 0);
transform[3] += matX.at<float>(3, 0);
transform[4] += matX.at<float>(4, 0);
transform[5] += matX.at<float>(5, 0);
for(int i=0; i<6; i++){
if(isnan(transform[i]))
transform[i]=0;
}
float deltaR = sqrt(
pow(rad2deg(matX.at<float>(0, 0)), 2) +
pow(rad2deg(matX.at<float>(1, 0)), 2) +
pow(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.1 && deltaT < 0.1) {
break;
}
/*************************************************/首先对于理论方面，上面的代码是基于论文原文中的公式4-12
首先对于理论进行解释：
对于公式4 
就是一个时间的线性插值，注意顺序是从k+1时刻开始的
对于公式6—8就是罗德里格斯公式的内容
首先对于公式5是经典的利用旋转矩阵和平移向量实现位姿变换的经典公式，由于旋转矩阵的是9个变量和平移向量的3的变量还是直接使用标准的齐次的变换矩阵（4*4）进行计算显然需要浪费大量的计算资源，因此通过罗德里格斯变换将旋转矩阵使用旋转向量表示，这样就可以使用3+3共6个参数表示旋转和平移了。
这里需要注意，此时只属于理论依据和代码实现是不一样的。个人理解此处的w应当时[cosx,cosy,cosz](即方向余弦)，这样满足了罗德里格斯转换的条件，求得w的反对称矩阵
，
就是绕旋转轴转的角度值。这样我们就只需要求6个变量就可以的即T（6）。
由于这里点数众多自然就涉及到了我们经常使用的优化问题。在这里使用的是levenberg-marquardt算法进行的非线性最优化。
理论依据：
对于公式9-11就是指明了我们的目标函数。12就是lm最终的迭代公式。
现在开始进行代码的具体分析：首先
下面的代码已经求除了下一时刻的数据中的特征点对上一时刻的数据中的相应的线特征或者是面特征的距离。并且求出了距离对于当前时刻特征点的偏导数coeff，代码如下：
tripod1 = _lastCornerCloud->points[_pointSearchCornerInd1[i]];
tripod2 = _lastCornerCloud->points[_pointSearchCornerInd2[i]];
float x0 = pointSel.x;
float y0 = pointSel.y;
float z0 = pointSel.z;
float x1 = tripod1.x;
float y1 = tripod1.y;
float z1 = tripod1.z;
float x2 = tripod2.x;
float y2 = tripod2.y;
float z2 = tripod2.z;
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)));
float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
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;
// TODO: Why writing to a variable that's never read?
pointProj = pointSel;
pointProj.x -= la * ld2;
pointProj.y -= lb * ld2;
pointProj.z -= lc * ld2;
float s = 1;
if (iterCount >= 5) {
s = 1 - 1.8f * fabs(ld2);
}
coeff.x = s * la;
coeff.y = s * lb;
coeff.z = s * lc;
coeff.intensity = s * ld2;
if (s > 0.1 && ld2 != 0) {
_laserCloudOri->push_back(_cornerPointsSharp->points[i]);
_coeffSel->push_back(coeff);
}
}
}
for (int i = 0; i < surfPointsFlatNum; i++) {
transformToStart(_surfPointsFlat->points[i], pointSel);
if (iterCount % 5 == 0) {
_lastSurfaceKDTree.nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
if (pointSearchSqDis[0] < 25) {
closestPointInd = pointSearchInd[0];
int closestPointScan = int(_lastSurfaceCloud->points[closestPointInd].intensity);
float pointSqDis, minPointSqDis2 = 25, minPointSqDis3 = 25;
for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++) {
if (int(_lastSurfaceCloud->points[j].intensity) > closestPointScan + 2.5) {
break;
}
pointSqDis = calcSquaredDiff(_lastSurfaceCloud->points[j], pointSel);
if (int(_lastSurfaceCloud->points[j].intensity) <= closestPointScan) {
if (pointSqDis < minPointSqDis2) {
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
} else {
if (pointSqDis < minPointSqDis3) {
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
}
for (int j = closestPointInd - 1; j >= 0; j--) {
if (int(_lastSurfaceCloud->points[j].intensity) < closestPointScan - 2.5) {
break;
}
pointSqDis = calcSquaredDiff(_lastSurfaceCloud->points[j], pointSel);
if (int(_lastSurfaceCloud->points[j].intensity) >= closestPointScan) {
if (pointSqDis < minPointSqDis2) {
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
} else {
if (pointSqDis < minPointSqDis3) {
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
}
}
_pointSearchSurfInd1[i] = closestPointInd;
_pointSearchSurfInd2[i] = minPointInd2;
_pointSearchSurfInd3[i] = minPointInd3;
}
if (_pointSearchSurfInd2[i] >= 0 && _pointSearchSurfInd3[i] >= 0) {
tripod1 = _lastSurfaceCloud->points[_pointSearchSurfInd1[i]];
tripod2 = _lastSurfaceCloud->points[_pointSearchSurfInd2[i]];
tripod3 = _lastSurfaceCloud->points[_pointSearchSurfInd3[i]];
float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z)
- (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z);
float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x)
- (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x);
float pc = (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y)
- (tripod3.x - tripod1.x) * (tripod2.y - tripod1.y);
float pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z);
float ps = sqrt(pa * pa + pb * pb + pc * pc);
pa /= ps;
pb /= ps;
pc /= ps;
pd /= ps;
float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
// TODO: Why writing to a variable that's never read? Maybe it should be used afterwards?
pointProj = pointSel;
pointProj.x -= pa * pd2;
pointProj.y -= pb * pd2;
pointProj.z -= pc * pd2;
float s = 1;
if (iterCount >= 5) {
s = 1 - 1.8f * fabs(pd2) / sqrt(calcPointDistance(pointSel));
}
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
coeff.intensity = s * pd2;
if (s > 0.1 && pd2 != 0) {
_laserCloudOri->push_back(_surfPointsFlat->points[i]);
_coeffSel->push_back(coeff);
}
对于上面的距离求导和求偏导数如果不想自己看代码可以看前面我提到的两篇文章，里面有必究详尽的推导。
在获取了偏导数后，就可以进行我们前面提到的第一段代码的解释了。
首先由于我们已经获取了距离对当前时刻点的x，y，z的偏导数。所以我们只需要将求得的偏导数再对我们的T（6）中的6个变量（tx,ty,tz,rx,ry,rz）进行求偏导，然后对应相乘即可。
对于当前时刻的x对rx求偏导，需要特别注意一点，此处与前面的理论推导的不同，次数的rx，ry，rz就是欧拉角roll，pitch，yaw。而不是直接依据罗德里格斯公式算出的那个三维向量。在次数仅对coeff.x对rx的偏导进行求，其他的原理是一模一样的。
根据上面同样可获得y与z
对于下面的构建lm迭代函数部分，使用了一个技巧。就是如下：
上式的At为A的转置矩阵，乘以它视为了确保At*A后是可逆的。后面的transform将每次迭代求得的步长累计起来。然后这个更新了的transform，在下一次迭代求亚隔壁矩阵的时候进行使用的。
这样进行迭代就可以求出符合要求的位姿变换tx，ty，tz，rx，ry，rz了。
这里需要特别注意的一点是，我们算出的变化都是针对于，t+1时刻而言的。因此还需要进行坐标系的变化将其转化为真正的全局坐标系才行。这部分请自己看源码。
难点二分析（mapping）：
还是像上面一样先上代码：
上面涉及到的坐标系的转换是将当前的计算得到的点云转换 
，然后将测量得到的点坐标转换到世界坐标系下。
坐标的转换过程为：map-》（bl转换）-》以前的odom的转换-》（al转换）-》上一次转换了一个位姿差值后的位姿—》（bc变换）-》当前转化后的位姿
注意代码上的符号的表示，sbcx，sblx，salx等的意思。个人根据代码逻辑推测，b表示before，a表示after，c表示current，l表示last。这样已解释整个转换代码就清晰了。
这里面有一个数学转化如下：
设空间有一坐标系其直接转化为map和根据上面的转换过程转换过去的过程分别如下：
下面的表示次序有些问题，但是思路就是这样的，由于太麻烦，在发现转换次序又问题后，也暂时没有修改以后如果有时间再修改吧。
由于对于直接转换到map有：y在对应点（x0，y0，z0）对应的z0的系数为srx，这样就可以求出srx了。同样的道理可以求出srxcrx、crycrx等，这样就是程序里面的公式了。
由于时间仓促，文章一定有很多不足，恳请各位看官批评指正。
wxw 2018.05.06 SH
687
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
