featureAssociation.cpp解析三
- 七、优化初始化
- 八、特征关联
loam源码地址：https://github.com/RobustFieldAutonomyLab/LeGO-LOAM.
论文学习：【论文阅读】LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain.
LeGO-LOAM源码解析汇总：
LeGO-LOAM源码解析1： 算法整体框架和utility.h.
LeGO-LOAM源码解析2： imageProjection.
LeGO-LOAM源码解析3： featureAssociation（一）.
LeGO-LOAM源码解析4： featureAssociation（二）.
七、优化初始化
判断有没有初始化,即判断是不是第一帧，若是第一帧则直接保存数据后推出：
if (!systemInitedLM) {
checkSystemInitialization();
return;
}
将第一帧的曲率较大的点保存为上一帧用于匹配的线特征：
void checkSystemInitialization(){
// 交换cornerPointsLessSharp和laserCloudCornerLast
pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
cornerPointsLessSharp = laserCloudCornerLast;
laserCloudCornerLast = laserCloudTemp;
将第一帧的曲率较小的点保存为上一帧用于匹配的面特征：
// 交换surfPointsLessFlat和laserCloudSurfLast
laserCloudTemp = surfPointsLessFlat;
surfPointsLessFlat = laserCloudSurfLast;
laserCloudSurfLast = laserCloudTemp;
将特征点保存到KD树中：
kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
kdtreeSurfLast->setInputCloud(laserCloudSurfLast);
记录特征点数：
laserCloudCornerLastNum = laserCloudCornerLast->points.size();
laserCloudSurfLastNum = laserCloudSurfLast->points.size();
发布特征点云消息：
sensor_msgs::PointCloud2 laserCloudCornerLast2;
pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
laserCloudCornerLast2.header.stamp = cloudHeader.stamp;
laserCloudCornerLast2.header.frame_id = "/camera";
pubLaserCloudCornerLast.publish(laserCloudCornerLast2);
sensor_msgs::PointCloud2 laserCloudSurfLast2;
pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
laserCloudSurfLast2.header.stamp = cloudHeader.stamp;
laserCloudSurfLast2.header.frame_id = "/camera";
pubLaserCloudSurfLast.publish(laserCloudSurfLast2);
保存累积旋转量：
transformSum[0] += imuPitchStart;
transformSum[2] += imuRollStart;
初始化完成：
systemInitedLM = true;
}
八、特征关联
1. 更新初始位姿updateInitialGuess
该函数主要是将当前时刻保存的IMU数据作为先验数据：
这里主要是保存当前点云中最后一个点的旋转角、最后一个点相对于第一个点的位移以及速度，但是很奇怪的一点是imuShiftFromStartXCur所在的进行计算的函数ShiftToStartIMU()其实从来没有被调用过，所以初值是0？希望有人能给我解答：
void updateInitialGuess(){
imuPitchLast = imuPitchCur;
imuYawLast = imuYawCur;
imuRollLast = imuRollCur;
imuShiftFromStartX = imuShiftFromStartXCur;
imuShiftFromStartY = imuShiftFromStartYCur;
imuShiftFromStartZ = imuShiftFromStartZCur;
imuVeloFromStartX = imuVeloFromStartXCur;
imuVeloFromStartY = imuVeloFromStartYCur;
imuVeloFromStartZ = imuVeloFromStartZCur;
预测的旋转量实际上就是当前帧第一个点与上一帧第一个点的旋转角差值，负号是指 R 当 前 帧 上 一 帧 = − R 上 一 帧 当 前 帧 R_{当前帧}^{上一帧}=-R_{上一帧}^{当前帧} R当前帧上一帧=−R上一帧当前帧
// 关于下面负号的说明：
// transformCur是在Cur坐标系下的 p_start=R*p_cur+t
// R和t是在Cur坐标系下的
// 而imuAngularFromStart是在start坐标系下的，所以需要加负号
if (imuAngularFromStartX != 0 || imuAngularFromStartY != 0 || imuAngularFromStartZ != 0){
transformCur[0] = - imuAngularFromStartY;
transformCur[1] = - imuAngularFromStartZ;
transformCur[2] = - imuAngularFromStartX;
}
而预测的平移量实际上就是当前帧最后一个点与当前帧第一个点的平移量差值，负号是指 t 当 前 帧 上 一 帧 = − t 上 一 帧 当 前 帧 t_{当前帧}^{上一帧}=-t_{上一帧}^{当前帧} t当前帧上一帧=−t上一帧当前帧
// 速度乘以时间，当前变换中的位移
if (imuVeloFromStartX != 0 || imuVeloFromStartY != 0 || imuVeloFromStartZ != 0){
transformCur[3] -= imuVeloFromStartX * scanPeriod;
transformCur[4] -= imuVeloFromStartY * scanPeriod;
transformCur[5] -= imuVeloFromStartZ * scanPeriod;
}
}
2. 更新变换矩阵updateTransformation
(1) 寻找特征对应点findCorrespondingSurfFeatures && findCorrespondingCornerFeatures
对于面特征和线特征的匹配分别有两个函数，他们在进行特征匹配之前都将点云点转换到当前帧的初始时刻：
a) 坐标转换到起始时刻TransformToStart
在这里值🉐注意就是假设了匀速运动模型，也就是说位姿的变化是线性的，而代码中提到的
s
s
s就是随时间线性变化的因子。回顾之前点云强度的保存的数据的计算，point.intensity = int(segmentedCloud->points[i].intensity) + scanPeriod * relTime;，再结合
s
s
s的定义， float s = 10 * (pi->intensity - int(pi->intensity));，发现
s
s
s就是relTime，为当前点云时间减去初始点云相对于帧点云时间差的比值。计算方式为:
P
i
n
=
0
=
T
n
=
i
n
=
0
∗
P
i
n
=
i
P_i^{n=0}=T_{n=i}^{n=0}*P^{n=i}_i
Pin=0=Tn=in=0∗Pin=i
void TransformToStart(PointType const * const pi, PointType * const po)
{
// intensity代表的是：整数部分ring序号，小数部分是当前点在这一圈中所花的时间
// 关于intensity， 参考 adjustDistortion() 函数中的定义
// s代表的其实是一个比例，s的计算方法应该如下：
// s=(pi->intensity - int(pi->intensity))/SCAN_PERIOD
// ===> SCAN_PERIOD=0.1(雷达频率为10hz)
// 以上理解感谢github用户StefanGlaser
// https://github.com/laboshinl/loam_velodyne/issues/29
float s = 10 * (pi->intensity - int(pi->intensity));
float rx = s * transformCur[0];
float ry = s * transformCur[1];
float rz = s * transformCur[2];
float tx = s * transformCur[3];
float ty = s * transformCur[4];
float tz = s * transformCur[5];
float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty);
float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty);
float z1 = (pi->z - tz);
float x2 = x1;
float y2 = cos(rx) * y1 + sin(rx) * z1;
float z2 = -sin(rx) * y1 + cos(rx) * z1;
po->x = cos(ry) * x2 - sin(ry) * z2;
po->y = y2;
po->z = sin(ry) * x2 + cos(ry) * z2;
po->intensity = pi->intensity;
}
b) 面特征匹配findCorrespondingSurfFeatures
这里和LOAM中的计算一模一样，如果不熟悉的可以参考loam源码解析4： laserOdometry（二）。
将曲率小的点（待匹配的特征点）的坐标全部转换到当前帧的初始时刻：
void findCorrespondingSurfFeatures(int iterCount){
int surfPointsFlatNum = surfPointsFlat->points.size();
for (int i = 0; i < surfPointsFlatNum; i++) {
// 坐标变换到开始时刻，参数0是输入，参数1是输出
TransformToStart(&surfPointsFlat->points[i], &pointSel);
每五次迭代重新匹配特征点：
if (iterCount % 5 == 0) {
在上一帧曲率较小的点组成的KD树中寻找一个最近邻点：
// k点最近邻搜索，这里k=1
kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
// sq:平方，距离的平方值
// 如果nearestKSearch找到的1(k=1)个邻近点满足条件
if (pointSearchSqDis[0] < nearestFeatureSearchSqDist) {
closestPointInd = pointSearchInd[0];
找到该最近邻点的线束：
// thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;
// fullInfoCloud->points[index].intensity = range;
// 在imageProjection.cpp文件中有上述两行代码，两种类型的值，应该存的是上面一个
int closestPointScan = int(laserCloudSurfLast->points[closestPointInd].intensity);
在最近邻点 i i i所在线束寻找再寻找另外一个最近点，以及在最近邻点 i i i所在线束的相邻线束中寻找第三个最近点，组成匹配平面：
首先在雷达点云存储的正方向(ScanID增大的方向和雷达旋转正方向)寻找其他两个点：
// 主要功能是找到2个scan之内的最近点，并将找到的最近点及其序号保存
// 之前扫描的保存到minPointSqDis2，之后的保存到minPointSqDis2
float pointSqDis, minPointSqDis2 = nearestFeatureSearchSqDist, minPointSqDis3 = nearestFeatureSearchSqDist;
for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++) {
// 相邻的ScanID相差两度
// 四舍五入
if (int(laserCloudSurfLast->points[j].intensity) > closestPointScan + 2.5) {
break;
}
pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
(laserCloudSurfLast->points[j].x - pointSel.x) +
(laserCloudSurfLast->points[j].y - pointSel.y) *
(laserCloudSurfLast->points[j].y - pointSel.y) +
(laserCloudSurfLast->points[j].z - pointSel.z) *
(laserCloudSurfLast->points[j].z - pointSel.z);
if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScan) {
if (pointSqDis < minPointSqDis2) {
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
} else {
if (pointSqDis < minPointSqDis3) {
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
}
然后在雷达点云存储的反方向(ScanID减小的方向和雷达旋转反方向)寻找其他两个点：
// 往前找
for (int j = closestPointInd - 1; j >= 0; j--) {
if (int(laserCloudSurfLast->points[j].intensity) < closestPointScan - 2.5) {
break;
}
pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
(laserCloudSurfLast->points[j].x - pointSel.x) +
(laserCloudSurfLast->points[j].y - pointSel.y) *
(laserCloudSurfLast->points[j].y - pointSel.y) +
(laserCloudSurfLast->points[j].z - pointSel.z) *
(laserCloudSurfLast->points[j].z - pointSel.z);
if (int(laserCloudSurfLast->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;
}
找到特征点的对应点以后的工作就是计算距离了，点面距离公式与论文一致（如上），在这里还计算了平面法向量（就是点到平面的直线的方向向量）在各个轴的方向分解，在雅可比的计算中使用：
// 前后都能找到对应的最近点在给定范围之内
// 那么就开始计算距离
// [pa,pb,pc]是tripod1，tripod2，tripod3这3个点构成的一个平面的方向量，
// ps是模长，它是三角形面积的2倍
if (pointSearchSurfInd2[i] >= 0 && pointSearchSurfInd3[i] >= 0) {
tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]];
tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]];
tripod3 = laserCloudSurfLast->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;
// 距离没有取绝对值
// 两个向量的点乘，分母除以ps中已经除掉了，
// 加pd原因:pointSel与tripod1构成的线段需要相减
float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
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);
}
}
}
}
c) 线特征匹配findCorrespondingCornerFeatures
将曲率大的点（待匹配的特征点）的坐标全部转换到当前帧的初始时刻：
void findCorrespondingCornerFeatures(int iterCount){
int cornerPointsSharpNum = cornerPointsSharp->points.size();
for (int i = 0; i < cornerPointsSharpNum; i++) {
TransformToStart(&cornerPointsSharp->points[i], &pointSel);
每五次迭代重新匹配特征点：
if (iterCount % 5 == 0) {
在上一帧曲率较大的点组成的KD树中寻找一个最近邻点：
kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1;
找到该最近邻点的线束：
if (pointSearchSqDis[0] < nearestFeatureSearchSqDist) {
closestPointInd = pointSearchInd[0];
int closestPointScan = int(laserCloudCornerLast->points[closestPointInd].intensity);
在最近邻点 i i i所在线束的相邻线束中寻找第二个最近点，组成匹配直线：
首先在雷达点云存储的正方向(ScanID增大的方向和雷达旋转正方向)寻找另外一个点：
float pointSqDis, minPointSqDis2 = nearestFeatureSearchSqDist;
for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++) {
if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan + 2.5) {
break;
}
pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
(laserCloudCornerLast->points[j].x - pointSel.x) +
(laserCloudCornerLast->points[j].y - pointSel.y) *
(laserCloudCornerLast->points[j].y - pointSel.y) +
(laserCloudCornerLast->points[j].z - pointSel.z) *
(laserCloudCornerLast->points[j].z - pointSel.z);
if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan) {
if (pointSqDis < minPointSqDis2) {
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
}
然后在雷达点云存储的反方向(ScanID减小的方向和雷达旋转反方向)寻找其他两个点：
for (int j = closestPointInd - 1; j >= 0; j--) {
if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan - 2.5) {
break;
}
pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
(laserCloudCornerLast->points[j].x - pointSel.x) +
(laserCloudCornerLast->points[j].y - pointSel.y) *
(laserCloudCornerLast->points[j].y - pointSel.y) +
(laserCloudCornerLast->points[j].z - pointSel.z) *
(laserCloudCornerLast->points[j].z - pointSel.z);
if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan) {
if (pointSqDis < minPointSqDis2) {
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
}
}
找到用于匹配的两个点（形成边缘线）：
pointSearchCornerInd1[i] = closestPointInd;
pointSearchCornerInd2[i] = minPointInd2;
}
找到特征点的对应点以后的工作就是计算距离了，点线距离公式与论文一致（如上），在这里还计算了就是点到直线的方向向量在各个轴的方向分解，在雅可比的计算中使用：
if (pointSearchCornerInd2[i] >= 0) {
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);
}
}
}
}
(2) 迭代优化calculateTransformationSurf && calculateTransformationCorner
对应论文中所提到的两阶段优化方法，在代码中的体现是将线特征和面特征分别进行优化，使用线特征优化 [ θ y , t x , t z ] [\theta_y,t_x,t_z] [θy,tx,tz]，使用面特征优化 [ θ x , θ z , t y ] [\theta_x,\theta_z,t_y] [θx,θz,ty]。由于坐标表述的问题，可能与论文中描述的不太一致，但是思想是一样的。面特征来自于地面，用于优化地面运动的物体中的 θ p i t c h \theta_{pitch} θpitch、 θ r o l l \theta_{roll} θroll以及垂直与地面的位移。位姿中剩下的三个量由线特征优化。可以参考loam源码解析5 ： laserOdometry（三）中的优化代码。
a) 线特征优化calculateTransformationCorner
定义各个矩阵，matA表示
J
J
J，matAtA表示
H
H
H，matAtB表示
b
b
b：
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和损失向量B，拼装成为一个大的求解矩阵:
// 以下为开始计算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函数进行求解，利用QR分解加速：
// 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);
关于退化问题的讨论，与LOAM中一致，请参考loam源码解析5 ： laserOdometry（三）中的退化问题分析：
if (iterCount == 0) {
cv::Mat matE(1, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matV(3, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matV2(3, 3, CV_32F, cv::Scalar::all(0));
// 计算At*A的特征值和特征向量
// 特征值存放在matE，特征向量matV
cv::eigen(matAtA, matE, matV);
matV.copyTo(matV2);
// 退化的具体表现是指什么？
isDegenerate = false;
float eignThre[3] = {10, 10, 10};
for (int i = 2; i >= 0; i--) {
if (matE.at<float>(0, i) < eignThre[i]) {
for (int j = 0; j < 3; j++) {
matV2.at<float>(i, j) = 0;
}
// 存在比10小的特征值则出现退化
isDegenerate = true;
} else {
break;
}
}
matP = matV.inv() * matV2;
}
if (isDegenerate) {
cv::Mat matX2(3, 1, CV_32F, cv::Scalar::all(0));
matX.copyTo(matX2);
matX = matP * matX2;
}
使用迭代计算的增量进行姿态更新：
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;
}
b) 面特征优化calculateTransformationSurf
定义各个矩阵，matA表示
J
J
J，matAtA表示
H
H
H，matAtB表示
b
b
b：
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));
对每个面特征点求关于对应三个变量的解雅可比矩阵A和损失向量B，拼装成为一个大的求解矩阵:
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函数进行求解，利用QR分解加速：
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
关于退化问题的讨论，与LOAM中一致，请参考loam源码解析5 ： laserOdometry（三）中的退化问题分析：
if (iterCount == 0) {
cv::Mat matE(1, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matV(3, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matV2(3, 3, CV_32F, cv::Scalar::all(0));
cv::eigen(matAtA, matE, matV);
matV.copyTo(matV2);
isDegenerate = false;
float eignThre[3] = {10, 10, 10};
for (int i = 2; i >= 0; i--) {
if (matE.at<float>(0, i) < eignThre[i]) {
for (int j = 0; j < 3; j++) {
matV2.at<float>(i, j) = 0;
}
isDegenerate = true;
} else {
break;
}
}
matP = matV.inv() * matV2;
}
if (isDegenerate) {
cv::Mat matX2(3, 1, CV_32F, cv::Scalar::all(0));
matX.copyTo(matX2);
matX = matP * matX2;
}
使用迭代计算的增量进行姿态更新：
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;
}
(3) 主体流程
面特征匹配：
void updateTransformation(){
if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)
return;
for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {
laserCloudOri->clear();
coeffSel->clear();
// 找到对应的特征平面
// 然后计算协方差矩阵，保存在coeffSel队列中
// laserCloudOri中保存的是对应于coeffSel的未转换到开始时刻的原始点云数据
findCorrespondingSurfFeatures(iterCount1);
面特征优化：
if (laserCloudOri->points.size() < 10)
continue;
// 通过面特征的匹配，计算变换矩阵
if (calculateTransformationSurf(iterCount1) == false)
break;
}
线特征匹配：
for (int iterCount2 = 0; iterCount2 < 25; iterCount2++) {
laserCloudOri->clear();
coeffSel->clear();
// 找到对应的特征边/角点
// 寻找边特征的方法和寻找平面特征的很类似，过程可以参照寻找平面特征的注释
findCorrespondingCornerFeatures(iterCount2);
线特征优化：
if (laserCloudOri->points.size() < 10)
continue;
// 通过角/边特征的匹配，计算变换矩阵
if (calculateTransformationCorner(iterCount2) == false)
break;
}
}
3. 更新累积变化矩阵integrateTransformation
说明：AccumulateRotation函数和PluginIMURotation与LOAM中一模一样，关于这部分的理解，请参考loam源码解析3 ： laserOdometry（一）的第六部分：姿态变换函数。
计算当前帧与初始帧的旋转变换：
// 旋转角的累计变化量
void integrateTransformation(){
float rx, ry, rz, tx, ty, tz;
// AccumulateRotation作用
// 将计算的两帧之间的位姿“累加”起来，获得相对于第一帧的旋转矩阵
// transformSum + (-transformCur) =(rx,ry,rz)
AccumulateRotation(transformSum[0], transformSum[1], transformSum[2],
-transformCur[0], -transformCur[1], -transformCur[2], rx, ry, rz);
计算当前帧与初始帧的平移变换：
// 进行平移分量的更新
float x1 = cos(rz) * (transformCur[3] - imuShiftFromStartX)
- sin(rz) * (transformCur[4] - imuShiftFromStartY);
float y1 = sin(rz) * (transformCur[3] - imuShiftFromStartX)
+ cos(rz) * (transformCur[4] - imuShiftFromStartY);
float z1 = transformCur[5] - imuShiftFromStartZ;
float x2 = x1;
float y2 = cos(rx) * y1 - sin(rx) * z1;
float z2 = sin(rx) * y1 + cos(rx) * z1;
tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
ty = transformSum[4] - y2;
tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);
修正累积位姿变换：
// 与accumulateRotatio联合起来更新transformSum的rotation部分的工作
// 可视为transformToEnd的下部分的逆过程
PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart,
imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);
累积位姿变换：
transformSum[0] = rx;
transformSum[1] = ry;
transformSum[2] = rz;
transformSum[3] = tx;
transformSum[4] = ty;
transformSum[5] = tz;
}
4. 里程计发布publishOdometry
这里比较简单，直接发布了tf的里程计实例：
void publishOdometry(){
geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformSum[2], -transformSum[0], -transformSum[1]);
// rx,ry,rz转化为四元数发布
laserOdometry.header.stamp = cloudHeader.stamp;
laserOdometry.pose.pose.orientation.x = -geoQuat.y;
laserOdometry.pose.pose.orientation.y = -geoQuat.z;
laserOdometry.pose.pose.orientation.z = geoQuat.x;
laserOdometry.pose.pose.orientation.w = geoQuat.w;
laserOdometry.pose.pose.position.x = transformSum[3];
laserOdometry.pose.pose.position.y = transformSum[4];
laserOdometry.pose.pose.position.z = transformSum[5];
pubLaserOdometry.publish(laserOdometry);
// laserOdometryTrans 是用于tf广播
laserOdometryTrans.stamp_ = cloudHeader.stamp;
laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
laserOdometryTrans.setOrigin(tf::Vector3(transformSum[3], transformSum[4], transformSum[5]));
tfBroadcaster.sendTransform(laserOdometryTrans);
}
5. 发布点云publishCloudsLast
把特征点云投影到每帧的结尾时刻，TransformToEnd可以参考loam源码解析3 ： laserOdometry（一）的第五部分：畸变去除函数：
void publishCloudsLast(){
updateImuRollPitchYawStartSinCos();
int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size();
for (int i = 0; i < cornerPointsLessSharpNum; i++) {
// TransformToEnd的作用是将k+1时刻的less特征点转移至k+1时刻的sweep的结束位置处的雷达坐标系下
TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]);
}
int surfPointsLessFlatNum = surfPointsLessFlat->points.size();
for (int i = 0; i < surfPointsLessFlatNum; i++) {
TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]);
}
用KD树存储当前帧点云(下一帧来该帧点云就是上一帧拉)：
pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
cornerPointsLessSharp = laserCloudCornerLast;
laserCloudCornerLast = laserCloudTemp;
laserCloudTemp = surfPointsLessFlat;
surfPointsLessFlat = laserCloudSurfLast;
laserCloudSurfLast = laserCloudTemp;
laserCloudCornerLastNum = laserCloudCornerLast->points.size();
laserCloudSurfLastNum = laserCloudSurfLast->points.size();
if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) {
kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
kdtreeSurfLast->setInputCloud(laserCloudSurfLast);
}
frameCount++;
发布界外点云（没有聚类成功的点云），adjustOutlierCloud函数主要是进行坐标转换：
if (frameCount >= skipFrameNum + 1) {
frameCount = 0;
// 调整坐标系，x=y,y=z,z=x
adjustOutlierCloud();
sensor_msgs::PointCloud2 outlierCloudLast2;
pcl::toROSMsg(*outlierCloud, outlierCloudLast2);
outlierCloudLast2.header.stamp = cloudHeader.stamp;
outlierCloudLast2.header.frame_id = "/camera";
pubOutlierCloudLast.publish(outlierCloudLast2);
发布线特征点云：
sensor_msgs::PointCloud2 laserCloudCornerLast2;
pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
laserCloudCornerLast2.header.stamp = cloudHeader.stamp;
laserCloudCornerLast2.header.frame_id = "/camera";
pubLaserCloudCornerLast.publish(laserCloudCornerLast2);
发布面特征点云：
sensor_msgs::PointCloud2 laserCloudSurfLast2;
pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
laserCloudSurfLast2.header.stamp = cloudHeader.stamp;
laserCloudSurfLast2.header.frame_id = "/camera";
pubLaserCloudSurfLast.publish(laserCloudSurfLast2);
}
}
1089
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
