本文主要讲的是特征提取中线面特征匹配以及优化的代码理解，因为上次代码看麻了·，这次静下心来又重新看了一遍。
不想听我废话的，直接去最后参考链接部分自己去理解。
面特征匹配（findCorrespondingSurfFeatures）
找三个距离i点最近的三个点就不讲了，说明白了就是算一下距离，其中closestPointInd是通过kd树找的，另外两个都是根据到closestPointInd的最小距离找到的。
pointSearchSurfInd1[i] = closestPointInd;
pointSearchSurfInd2[i] = minPointInd2;
pointSearchSurfInd3[i] = minPointInd3;
算法先把三个点复制一下：
if (pointSearchSurfInd2[i] >= 0 && pointSearchSurfInd3[i] >= 0) {
tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]];
tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]];
tripod3 = laserCloudSurfLast->points[pointSearchSurfInd3[i]];
为了后文便于讲解，我将tripod1，tripod2，tripod3分别记为j,l,m三个点。
首先我们要计算的是点i到j,i,m三点构成的平面的距离。下面要计算的就是这个。
首先复习下数学知识：
向量的点乘与叉乘，向量的叉乘的模长表示的是以两个向量为边长的平行四边形的面积,
a×b=|a||b|sin<a,b>
a·b=|a||b|cos<a,b>
如下图：
向量ji和向量jm的的叉乘表示的是地面四边形的面积，而叉乘出来的向量是与底面垂直的（符合右手准则）。叉乘向量与向量ji的点乘的数值就是立方体的体积，拿得到的体积除以底面面积就可以轻松得到点i到平面的距离。
而三维向量叉乘与点乘的公式如下：
（a1,a2,a3）×（b1,b2,b3）=（a2b3-a3b2 , a3b1-a1b3 , a1b2-a2b1）
（a1,a2,a3）·（b1,b2,b3）=（a1b1 , a2b2 , a3b3）
回到论文
点到面的距离公式：
i,j,l,m坐标分别记为（x0,y0,z0）（x1,y1,z1）（x2,y2,z2）（x3,y3,z3）
在面匹配中主要关注的会是 d对 rx,rz,ty 的偏导(因为是2步lm求解参数),以 rx 为例:
在面匹配过程中计算出三项中的第一项偏导，
坐标带入可得：
所以有：
有了表达式就很容易计算三个偏导：
了解了数学原理来解释下代码
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);
tripod1，tripod2，tripod3分别记为j,l,m三个点
所以tripod2-tripod1表示jl向量，tripod3-tripod1表示jm向量。
这时候就会发现pa,pb,pc就是三个偏导
接着看后面的代码：
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;
// 距离没有取绝对值
// 两个向量的点乘，分母除以ps中已经除掉了，
// 加pd原因:intSelpo与tripod1构成的线段需要相减
float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
可以发现ps表示的是面积，那么pd是什么呢，别着急往下接着看，把pd2展开看看：
pd2
= pa * pointSel.x + pb * pointSel.y + pc * pointSel.z - pd1/ps
= (pa1 * pointSel.x + pb1 * pointSel.y + pc1 * pointSel.z)/ps - pd1/ps
= (pa1 * pointSel.x + pb1 * pointSel.y + pc1 * pointSel.z)/ps -(pa1 * tripod1.x + pb1 * tripod1.y + pc1 * tripod1.z)/ps
= (pa1*(pointSel.x - tripod1.x) + pb1*(pointSel.y - tripod1.y) + pc1*(pointSel.z - tripod1.z))/ps
公式中pa1，pb1，pc1表示最开始没单位化的pa，pb，pc。
pointSel - tripod1表示的是什么，不就是ji向量吗，所以这不就是叉乘向量与ji向量点乘得到体积，然后除以面积ps得到的点到面的距离。
所以pd2表示的就是点到面的距离。
在后面讲就是一些加权值影响，coeffSel保存这些平面特征，laserCloudOri保存点i的原始点云数据：
float s = 1;
if (iterCount >= 5) {
// /加上影响因子
s = 1 - 1.8 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x
+ pointSel.y * pointSel.y + pointSel.z * pointSel.z));
}
if (s > 0.1 && pd2 != 0) {
// [x,y,z]是整个平面的单位法量
// intensity是平面外一点到该平面的距离
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
coeff.intensity = s * pd2;
// 未经变换的点放入laserCloudOri队列，距离，法向量值放入coeffSel
laserCloudOri->push_back(surfPointsFlat->points[i]);
coeffSel->push_back(coeff);
然后进行面特征优化。
面特征优化（calculateTransformationSurf）
优化部分主要求出剩下的三个偏导，然后计算得到的 rx,rz,ty位姿增量。
帧间的点云变换矩阵在本博客线特征优化部分讲了，可以跳过去看一下。
得到k+1时刻变换到k时刻 i 点的坐标：
然后就可以计算d对 rx,rz,ty的偏导
接着来看看代码，代码中b6应该写错了：
bool calculateTransformationSurf(int iterCount){
int pointSelNum = laserCloudOri->points.size();
cv::Mat matA(pointSelNum, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matAt(3, pointSelNum, CV_32F, cv::Scalar::all(0));
cv::Mat matAtA(3, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matB(pointSelNum, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matAtB(3, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matX(3, 1, CV_32F, cv::Scalar::all(0));
float srx = sin(transformCur[0]);
float crx = cos(transformCur[0]);
float sry = sin(transformCur[1]);
float cry = cos(transformCur[1]);
float srz = sin(transformCur[2]);
float crz = cos(transformCur[2]);
float tx = transformCur[3];
float ty = transformCur[4];
float tz = transformCur[5];
float a1 = crx*sry*srz; float a2 = crx*crz*sry; float a3 = srx*sry; float a4 = tx*a1 - ty*a2 - tz*a3;
float a5 = srx*srz; float a6 = crz*srx; float a7 = ty*a6 - tz*crx - tx*a5;
float a8 = crx*cry*srz; float a9 = crx*cry*crz; float a10 = cry*srx; float a11 = tz*a10 + ty*a9 - tx*a8;
float b1 = -crz*sry - cry*srx*srz; float b2 = cry*crz*srx - sry*srz;
float b5 = cry*crz - srx*sry*srz; float b6 = cry*srz + crz*srx*sry;
float c1 = -b6; float c2 = b5; float c3 = tx*b6 - ty*b5; float c4 = -crx*crz; float c5 = crx*srz; float c6 = ty*c5 + tx*-c4;
float c7 = b2; float c8 = -b1; float c9 = tx*-b2 - ty*-b1;
// 构建雅可比矩阵，求解
for (int i = 0; i < pointSelNum; i++) {
pointOri = laserCloudOri->points[i];
coeff = coeffSel->points[i];
float arx = (-a1*pointOri.x + a2*pointOri.y + a3*pointOri.z + a4) * coeff.x
+ (a5*pointOri.x - a6*pointOri.y + crx*pointOri.z + a7) * coeff.y
+ (a8*pointOri.x - a9*pointOri.y - a10*pointOri.z + a11) * coeff.z;
float arz = (c1*pointOri.x + c2*pointOri.y + c3) * coeff.x
+ (c4*pointOri.x - c5*pointOri.y + c6) * coeff.y
+ (c7*pointOri.x + c8*pointOri.y + c9) * coeff.z;
float aty = -b6 * coeff.x + c4 * coeff.y + b2 * coeff.z;
float d2 = coeff.intensity;
matA.at<float>(i, 0) = arx;
matA.at<float>(i, 1) = arz;
matA.at<float>(i, 2) = aty;
matB.at<float>(i, 0) = -0.05 * d2;
}
利用opencv函数计算x：
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
然后退化问题，接着更新位姿增量：
transformCur[0] += matX.at<float>(0, 0);
transformCur[2] += matX.at<float>(1, 0);
transformCur[4] += matX.at<float>(2, 0);
判断位姿是否合法以及迭代是否满足条件：
for(int i=0; i<6; i++){
if(isnan(transformCur[i]))
transformCur[i]=0;
}
float deltaR = sqrt(
pow(rad2deg(matX.at<float>(0, 0)), 2) +
pow(rad2deg(matX.at<float>(1, 0)), 2));
float deltaT = sqrt(
pow(matX.at<float>(2, 0) * 100, 2));
if (deltaR < 0.1 && deltaT < 0.1) {
return false;
}
return true;
}
线特征匹配
线特征匹配的原理和面特征差不多、
如下图：
找到点i的最近邻两个点i,j。同样向量ji和向量li的叉乘是四边形的面积，面积除以向量lj的模长就是点i到线lj的距离。下面来看看算法代码：
找距离i最近的两个点的方法就不再最熟，将两个最近点复制一下：
pointSearchCornerInd1[i] = closestPointInd;
pointSearchCornerInd2[i] = minPointInd2;
我们知道线匹配的距离公式如下，后面我们要优化变换矩阵（帧间矩阵）使d最小。
这里因为线匹配优化的是x,z的位移以及y轴的旋转角度，所以有：
这样一来问题就变成的找到合适的tx,ty,ry是的f与d的差值最小：
接着就是对tx,ty,ry分别偏导，然后找到最优的tx,ty,ry。
上式的三个一阶偏导数，以tx为例，f对tx的一阶偏导为：
而线特征匹配这一步计算了
来看看如何计算的
假设i,j,l的坐标分别为
那么将坐标带入d的距离公式中（p1,p2表示j,l）
可得：
设I12表示|p1-p2|
则有：
为方便书写，我们做以下设定：
则d的表达式为：
那么函数对变量的偏导变为：
而：
上式化简后为：
接着再来看代码，这个时候就容易理解了：
tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]];
tripod2 = laserCloudCornerLast->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 m11 = ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1));
float m22 = ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1));
float m33 = ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1));
float a012 = sqrt(m11 * m11 + m22 * m22 + m33 * m33);
float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
float la = ((y1 - y2)*m11 + (z1 - z2)*m22) / a012 / l12;
float lb = -((x1 - x2)*m11 - (z1 - z2)*m33) / a012 / l12;
float lc = -((x1 - x2)*m22 + (y1 - y2)*m33) / a012 / l12;
float ld2 = a012 / l12;
float s = 1;
if (iterCount >= 5) {
s = 1 - 1.8 * fabs(ld2);
}
if (s > 0.1 && ld2 != 0) {
coeff.x = s * la;
coeff.y = s * lb;
coeff.z = s * lc;
coeff.intensity = s * ld2;
laserCloudOri->push_back(cornerPointsSharp->points[i]);
coeffSel->push_back(coeff);
la,lb,lc表示的就是三个偏导，而ld2表示的是点到线的距离
线特征优化
优化方程如下：
其中三项的中首个偏导在线条特征匹配中已经计算好了，接下来需要计算剩下的偏导。
首先要知道，匹配优化过程是帧间匹配的，之前transformCur保存的是相邻两帧之间的旋转角度以及位移量。现在要将当前点云结束时刻（k+1）的点云投影到当前点云开始时刻(k)
即：
旋转矩阵----绕z轴转roll (rz) —绕x轴转pitch(rx) — 绕 y轴转heading (ry)：
你可能会发现角度符号有点不对，这是因为我们要从k+1变到k，所以角度应该是相反数，然后根据sin,cos函数性质在进行化简得来的。
需要注意的是代码里的坐标转化乱七八糟的，要分清楚rx,ry,rz分别是什么角度。
将上式展开：
带入公式得：
得到x,y,z的解析式后，可以分别求取他们相对于tx,ty,ry的偏导数：
有了各项之后就可以计算出F相对未知参数 tx,tz,ry的求导:
再来看代码就容易理解了：
bool calculateTransformationCorner(int iterCount){
int pointSelNum = laserCloudOri->points.size();
cv::Mat matA(pointSelNum, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matAt(3, pointSelNum, CV_32F, cv::Scalar::all(0));
cv::Mat matAtA(3, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matB(pointSelNum, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matAtB(3, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matX(3, 1, CV_32F, cv::Scalar::all(0));
// 以下为开始计算A,A=[J的偏导],J的偏导的计算公式是什么?
float srx = sin(transformCur[0]);
float crx = cos(transformCur[0]);
float sry = sin(transformCur[1]);
float cry = cos(transformCur[1]);
float srz = sin(transformCur[2]);
float crz = cos(transformCur[2]);
float tx = transformCur[3];
float ty = transformCur[4];
float tz = transformCur[5];
float b1 = -crz*sry - cry*srx*srz; float b2 = cry*crz*srx - sry*srz; float b3 = crx*cry; float b4 = tx*-b1 + ty*-b2 + tz*b3;
float b5 = cry*crz - srx*sry*srz; float b6 = cry*srz + crz*srx*sry; float b7 = crx*sry; float b8 = tz*b7 - ty*b6 - tx*b5;
float c5 = crx*srz;
for (int i = 0; i < pointSelNum; i++) {
pointOri = laserCloudOri->points[i];
coeff = coeffSel->points[i];
float ary = (b1*pointOri.x + b2*pointOri.y - b3*pointOri.z + b4) * coeff.x
+ (b5*pointOri.x + b6*pointOri.y - b7*pointOri.z + b8) * coeff.z;
float atx = -b5 * coeff.x + c5 * coeff.y + b1 * coeff.z;
float atz = b7 * coeff.x - srx * coeff.y - b3 * coeff.z;
float d2 = coeff.intensity;
// A=[J的偏导]; B=[权重系数*(点到直线的距离)] 求解公式: AX=B
// 为了让左边满秩，同乘At-> At*A*X = At*B
matA.at<float>(i, 0) = ary;
matA.at<float>(i, 1) = atx;
matA.at<float>(i, 2) = atz;
matB.at<float>(i, 0) = -0.05 * d2;
}
后面就是通过opencv自带的函数计算出x：
// transpose函数求得matA的转置matAt
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
// 通过QR分解的方法，求解方程AtA*X=AtB，得到X
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
紧接着是退化问题，然后更新姿态：
transformCur[1] += matX.at<float>(0, 0);
transformCur[3] += matX.at<float>(1, 0);
transformCur[5] += matX.at<float>(2, 0);
最后检查姿态是否合法以及迭代条件是否满足等等：
for(int i=0; i<6; i++){
if(isnan(transformCur[i]))
transformCur[i]=0;
}
float deltaR = sqrt(
pow(rad2deg(matX.at<float>(0, 0)), 2));
float deltaT = sqrt(
pow(matX.at<float>(1, 0) * 100, 2) +
pow(matX.at<float>(2, 0) * 100, 2));
if (deltaR < 0.1 && deltaT < 0.1) {
return false;
}
return true;
}