【LeGO-LOAM论文阅读(二)--特征提取(二)】
简介
上篇博客介绍了特征提取的原理以及坐标转化和插值的源码理解,这篇将介绍特征提取的后续算法模块。
源码
1、新数据进来进行坐标转换和插补等工作
见【LeGO-LOAM论文阅读(二)–特征提取(一)】
2、进行光滑度计算
光滑度计算公式:
S取10,代表参与计算点数,计算点左右各有5个,r表示深度距离,计算点云开始5个点后到点云结束5个点前的点云平滑度,计算方式与论文中给的并不一样。
// 计算光滑性,这里的计算没有完全按照公式进行,// 缺少除以总点数i和r[i]void calculateSmoothness(){int cloudSize = segmentedCloud->points.size();for (int i = 5; i < cloudSize - 5; i++) {float diffRange = segInfo.segmentedCloudRange[i-5] + segInfo.segmentedCloudRange[i-4]+ segInfo.segmentedCloudRange[i-3] + segInfo.segmentedCloudRange[i-2]+ segInfo.segmentedCloudRange[i-1] - segInfo.segmentedCloudRange[i] * 10+ segInfo.segmentedCloudRange[i+1] + segInfo.segmentedCloudRange[i+2]+ segInfo.segmentedCloudRange[i+3] + segInfo.segmentedCloudRange[i+4]+ segInfo.segmentedCloudRange[i+5]; cloudCurvature[i] = diffRange*diffRange;
初始化点云标签,初始化为0,surfPointsFlat标记为-1,surfPointsLessFlatScan为不大于0的标签,cornerPointsSharp标记为2,cornerPointsLessSharp标记为1。
// 在markOccludedPoints()函数中对该参数进行重新修改cloudNeighborPicked[i] = 0;// 在extractFeatures()函数中会对标签进行修改,// 初始化为0,surfPointsFlat标记为-1,surfPointsLessFlatScan为不大于0的标签// cornerPointsSharp标记为2,cornerPointsLessSharp标记为1cloudLabel[i] = 0;cloudSmoothness[i].value = cloudCurvature[i];cloudSmoothness[i].ind = i;}}
3、标记阻塞点。
阻塞点指点云之间相互遮挡而且距离比较近的点。将被遮挡的点云连续五个标志位cloudNeighborPicked标记为1。depth大的那个就是被遮挡的那个。具体参数设置为什么是10和0.3我也不清楚,应该是是试验过的,各位可以考虑话说弄成别的试试
void markOccludedPoints(){int cloudSize = segmentedCloud->points.size();for (int i = 5; i < cloudSize - 6; ++i){float depth1 = segInfo.segmentedCloudRange[i];float depth2 = segInfo.segmentedCloudRange[i+1];int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[i+1] - segInfo.segmentedCloudColInd[i]));if (columnDiff < 10){// 选择距离较远的那些点,并将他们标记为1if (depth1 - depth2 > 0.3){cloudNeighborPicked[i - 5] = 1;cloudNeighborPicked[i - 4] = 1;cloudNeighborPicked[i - 3] = 1;cloudNeighborPicked[i - 2] = 1;cloudNeighborPicked[i - 1] = 1;cloudNeighborPicked[i] = 1;}else if (depth2 - depth1 > 0.3){cloudNeighborPicked[i + 1] = 1;cloudNeighborPicked[i + 2] = 1;cloudNeighborPicked[i + 3] = 1;cloudNeighborPicked[i + 4] = 1;cloudNeighborPicked[i + 5] = 1;cloudNeighborPicked[i + 6] = 1;}}
接着排除共面的情况,舍弃距离变换比较大的点
float diff1 = std::abs(segInfo.segmentedCloudRange[i-1] - segInfo.segmentedCloudRange[i]);float diff2 = std::abs(segInfo.segmentedCloudRange[i+1] - segInfo.segmentedCloudRange[i]);// 选择距离变化较大的点,并将他们标记为1if (diff1 > 0.02 * segInfo.segmentedCloudRange[i] && diff2 > 0.02 * segInfo.segmentedCloudRange[i])cloudNeighborPicked[i] = 1;}}
4、特征抽取。
这部分对计算好平滑度的点云将进行特征提取,提取出线特征和面特征。由于点云过于庞大,后面处理的时候对点云进行了下采样来提高计算效率。
清空上一帧特征:
void extractFeatures(){cornerPointsSharp->clear();cornerPointsLessSharp->clear();surfPointsFlat->clear();surfPointsLessFlat->clear();
将每一帧点云数据按水平角度分为6份,每份是60°的激光数据,sp储存的是每份数据的起点,ep储存的是每份数据的终点。
for (int i = 0; i < N_SCAN; i++) {surfPointsLessFlatScan->clear();for (int j = 0; j < 6; j++) {// sp和ep的含义是什么???startPointer,endPointer?int sp = (segInfo.startRingIndex[i] * (6 - j) + segInfo.endRingIndex[i] * j) / 6;int ep = (segInfo.startRingIndex[i] * (5 - j) + segInfo.endRingIndex[i] * (j + 1)) / 6 - 1;if (sp >= ep)continue;
将每份点云按照平滑度进行排序(由小到大):
std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());
提取线特征的条件:不是被淘汰的点(cloudNeighborPicked=0)并且平滑度大于设定的阈值还有不能是地面点:
int largestPickedNum = 0;for (int k = ep; k >= sp; k--) {// 每次ind的值就是等于k??? 有什么意义?// 因为上面对cloudSmoothness进行了一次从小到大排序,所以ind不一定等于k了int ind = cloudSmoothness[k].ind;if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] > edgeThreshold &&segInfo.segmentedCloudGroundFlag[ind] == false) {
在满足条件的点云中,将最大平滑度的两个点云加入到cornerPointsSharp中,并将标志cloudLabel置为2
largestPickedNum++;if (largestPickedNum <= 2) {// 论文中nFe=2,cloudSmoothness已经按照从小到大的顺序排列,// 所以这边只要选择最后两个放进队列即可// cornerPointsSharp标记为2cloudLabel[ind] = 2;cornerPointsSharp->push_back(segmentedCloud->points[ind]);cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
将平滑度前20的点云加入到cornerPointsLessSharp中,并将cloudLabel置为1
} else if (largestPickedNum <= 20) {// 塞20个点到cornerPointsLessSharp中去// cornerPointsLessSharp标记为1cloudLabel[ind] = 1;cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);} else {break;}
将已经加入到cornerPointsSharp和cornerPointsLessSharp中的点淘汰,后面不再用:
cloudNeighborPicked[ind] = 1;
计算已经被选为线特征点云前后5个点之间的距离差值,若距离较近
同样淘汰(避免两个特征点表示的是同一个线特征):
for (int l = 1; l <= 5; l++) {// 从ind+l开始后面5个点,每个点index之间的差值,// 确保columnDiff<=10,然后标记为我们需要的点int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l - 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}for (int l = -1; l >= -5; l--) {// 从ind+l开始前面五个点,计算差值然后标记int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}}}
提取面特征的条件,不能是淘汰点,平滑度小于设定阈值并且只能是地面点:
int smallestPickedNum = 0;for (int k = sp; k <= ep; k++) {int ind = cloudSmoothness[k].ind;// 平面点只从地面点中进行选择? 为什么要这样做?if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] < surfThreshold &&segInfo.segmentedCloudGroundFlag[ind] == true) {
将满足条件的点加入surfPointsFlat同时标志cloudLabel设为-1:
cloudLabel[ind] = -1;surfPointsFlat->push_back(segmentedCloud->points[ind]);
将最小平滑度的四个点放入surfPointsFlat,同时设置为淘汰点。
// 论文中nFp=4,将4个最平的平面点放入队列中smallestPickedNum++;if (smallestPickedNum >= 4) {break;}cloudNeighborPicked[ind] = 1;
同时判断面特征点前后5个点并标记(和线特征一样):
for (int l = 1; l <= 5; l++) {// 从前面往后判断是否是需要的邻接点,是的话就进行标记int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l - 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}for (int l = -1; l >= -5; l--) {// 从后往前开始标记int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));if (columnDiff > 10)break;cloudNeighborPicked[ind + l] = 1;}}}
将cloudLabe<= 0的点先都加入 surfPointsLessFlatScan中:
for (int k = sp; k <= ep; k++) {if (cloudLabel[k] <= 0) {surfPointsLessFlatScan->push_back(segmentedCloud->points[k]);}}}
surfPointsLessFlatScan中的点云会非常的多,为了提高计算效率,对点云进行下采样:
// surfPointsLessFlatScan中有过多的点云,如果点云太多,计算量太大// 进行下采样,可以大大减少计算量surfPointsLessFlatScanDS->clear();downSizeFilter.setInputCloud(surfPointsLessFlatScan);downSizeFilter.filter(*surfPointsLessFlatScanDS);*surfPointsLessFlat += *surfPointsLessFlatScanDS;}}
5、发布四种点云信息。
分别发布平滑度大的点云、平滑度较大的点云、平滑度小的点云、平滑度较小的点云:
void publishCloud(){sensor_msgs::PointCloud2 laserCloudOutMsg;if (pubCornerPointsSharp.getNumSubscribers() != 0){pcl::toROSMsg(*cornerPointsSharp, laserCloudOutMsg);laserCloudOutMsg.header.stamp = cloudHeader.stamp;laserCloudOutMsg.header.frame_id = "/camera";pubCornerPointsSharp.publish(laserCloudOutMsg);}if (pubCornerPointsLessSharp.getNumSubscribers() != 0){pcl::toROSMsg(*cornerPointsLessSharp, laserCloudOutMsg);laserCloudOutMsg.header.stamp = cloudHeader.stamp;laserCloudOutMsg.header.frame_id = "/camera";pubCornerPointsLessSharp.publish(laserCloudOutMsg);}if (pubSurfPointsFlat.getNumSubscribers() != 0){pcl::toROSMsg(*surfPointsFlat, laserCloudOutMsg);laserCloudOutMsg.header.stamp = cloudHeader.stamp;laserCloudOutMsg.header.frame_id = "/camera";pubSurfPointsFlat.publish(laserCloudOutMsg);}if (pubSurfPointsLessFlat.getNumSubscribers() != 0){pcl::toROSMsg(*surfPointsLessFlat, laserCloudOutMsg);laserCloudOutMsg.header.stamp = cloudHeader.stamp;laserCloudOutMsg.header.frame_id = "/camera";pubSurfPointsLessFlat.publish(laserCloudOutMsg);}}
回到runFeatureAssociation函数里,判断有没有初始化·,即判断是不是第一帧数据,systemInitedLM初始为0,:
if (!systemInitedLM) {checkSystemInitialization();return;}
将第一帧的曲率较大的点保存为上一帧用于匹配的线特征:
void checkSystemInitialization(){// 交换cornerPointsLessSharp和laserCloudCornerLastpcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;cornerPointsLessSharp = laserCloudCornerLast;laserCloudCornerLast = laserCloudTemp;
将第一帧的曲率较小的点保存为上一帧用于匹配的面特征
// 交换surfPointsLessFlat和laserCloudSurfLastlaserCloudTemp = 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;
checkSystemInitialization判断完第一帧数据将systemInitedLM设为1
systemInitedLM = true;
}
6、更新位姿
这里主要是保存当前点云中最后一个点的旋转角、最后一个点相对于第一个点的位移以及速度:
void updateInitialGuess(){imuPitchLast = imuPitchCur;imuYawLast = imuYawCur;imuRollLast = imuRollCur;imuShiftFromStartX = imuShiftFromStartXCur;imuShiftFromStartY = imuShiftFromStartYCur;imuShiftFromStartZ = imuShiftFromStartZCur;imuVeloFromStartX = imuVeloFromStartXCur;imuVeloFromStartY = imuVeloFromStartYCur;imuVeloFromStartZ = imuVeloFromStartZCur;
但是我发现最后一个点的位移变换函数根本没有被调用,我觉得这个函数是要在插值和坐标转那被调用,可以并没有,有没有大佬能解释下。
void ShiftToStartIMU(float pointTime){// 下面三个量表示的是世界坐标系下,从start到cur的坐标的漂移imuShiftFromStartXCur = imuShiftXCur - imuShiftXStart - imuVeloXStart * pointTime;imuShiftFromStartYCur = imuShiftYCur - imuShiftYStart - imuVeloYStart * pointTime;imuShiftFromStartZCur = imuShiftZCur - imuShiftZStart - imuVeloZStart * pointTime;// 从世界坐标系变换到start坐标系float x1 = cosImuYawStart * imuShiftFromStartXCur - sinImuYawStart * imuShiftFromStartZCur;float y1 = imuShiftFromStartYCur;float z1 = sinImuYawStart * imuShiftFromStartXCur + cosImuYawStart * imuShiftFromStartZCur;float x2 = x1;float y2 = cosImuPitchStart * y1 + sinImuPitchStart * z1;float z2 = -sinImuPitchStart * y1 + cosImuPitchStart * z1;imuShiftFromStartXCur = cosImuRollStart * x2 + sinImuRollStart * y2;imuShiftFromStartYCur = -sinImuRollStart * x2 + cosImuRollStart * y2;imuShiftFromStartZCur = z2;}
更新旋转量(相邻两帧):
// 关于下面负号的说明:// 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;
更新位移量(两帧之间):
// 速度乘以时间,当前变换中的位移if (imuVeloFromStartX != 0 || imuVeloFromStartY != 0 || imuVeloFromStartZ != 0){transformCur[3] -= imuVeloFromStartX * scanPeriod;transformCur[4] -= imuVeloFromStartY * scanPeriod;transformCur[5] -= imuVeloFromStartZ * scanPeriod;}}
7、更新变换矩阵。
如果线特征或者面特征太少,直接退出:
void updateTransformation(){if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)return;
寻找面特征对应点findCorrespondingSurfFeatures, iterCount迭代次数?:
for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {laserCloudOri->clear();coeffSel->clear();// 找到对应的特征平面// 然后计算协方差矩阵,保存在coeffSel队列中// laserCloudOri中保存的是对应于coeffSel的未转换到开始时刻的原始点云数据findCorrespondingSurfFeatures(iterCount1);
面特征点数赋值:
void findCorrespondingSurfFeatures(int iterCount){int surfPointsFlatNum = surfPointsFlat->points.size();
将特征点坐标变换到开始时刻 TransformToStart:
for (int i = 0; i < surfPointsFlatNum; i++) {// 坐标变换到开始时刻,参数0是输入,参数1是输出TransformToStart(&surfPointsFlat->points[i], &pointSel);
注意假设了匀速运动模型,也就是说位姿的变化是线性的,而代码中提到的s就是随时间线性变化的因子。回顾之前点云强度的保存的数据的计算,point.intensity = int(segmentedCloud->points[i].intensity) + scanPeriod * relTime;,再结合s的定义, float s = 10 * (pi->intensity - int(pi->intensity));,发现s就是relTime,为当前点云时间减去初始点云相对于帧点云时间差的比值。
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/29float 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;}
每5此迭代重新匹配特征点:
if (iterCount % 5 == 0) {
在上一帧曲率较小的点(平面特征点)组成的KD树中寻找一个最近邻点:
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];
找到该最近邻点的线束(第几行第几列)point.intensity保存里点云的行列值:
// point.intensity 保存的是什么值? 第几次scan?// thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;// fullInfoCloud->points[index].intensity = range;// 在imageProjection.cpp文件中有上述两行代码,两种类型的值,应该存的是上面一个int closestPointScan = int(laserCloudSurfLast->points[closestPointInd].intensity);
在最近邻点i 所在线束寻找再寻找另外一个最近点,以及在最近邻点i ii所在线束的相邻线束中寻找第三个最近点,组成匹配平面:
// 主要功能是找到2个scan之内的最近点,并将找到的最近点及其序号保存// 之前扫描的保存到minPointSqDis2,之后的保存到minPointSqDis2float pointSqDis, minPointSqDis2 = nearestFeatureSearchSqDist, minPointSqDis3 = nearestFeatureSearchSqDist;
首先在雷达点云存储的正方向(ScanID增大的方向和雷达旋转正方向)寻找其他两个点,为什么加上2.5,因为上下两个scan相差2°:
for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++) {// int类型的值加上2.5? 为什么不直接加上2?// 四舍五入if (int(laserCloudSurfLast->points[j].intensity) > closestPointScan + 2.5) {break;}
计算距离,找到最近邻的点,将之前扫描到的点保存在minPointSqDis2中,之后扫描到的保存在minPointSqDis3中:
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队列,距离,法向量值放入coeffSellaserCloudOri->push_back(surfPointsFlat->points[i]);coeffSel->push_back(coeff);}}}}
面特征优化(这块我看晕了,以下参考LeGO-LOAM源码解析5: featureAssociation(三)):
定义各个矩阵,matA表示J,matAtA表示H,matAtB表示b:
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[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) +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;}
线特征匹配与优化:
for (int iterCount2 = 0; iterCount2 < 25; iterCount2++) {laserCloudOri->clear();coeffSel->clear();// 找到对应的特征边/角点// 寻找边特征的方法和寻找平面特征的很类似,过程可以参照寻找平面特征的注释findCorrespondingCornerFeatures(iterCount2);if (laserCloudOri->points.size() < 10)continue;// 通过角/边特征的匹配,计算变换矩阵if (calculateTransformationCorner(iterCount2) == false)break;}}
这块和面特征匹配优化的思想类似:
将曲率大的点(待匹配的特征点)的坐标全部转换到当前帧的初始时刻:
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 ii所在线束的相邻线束中寻找第二个最近点,组成匹配直线:
首先在雷达点云存储的正方向(ScanID增大的方向和雷达旋转正方向)寻找另外一个点:
然后在雷达点云存储的反方向(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;}}}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);}}}}
线特征优化(我已经麻了):
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*BmatA.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;}// transpose函数求得matA的转置matAtcv::transpose(matA, matAt);matAtA = matAt * matA;matAtB = matAt * matB;// 通过QR分解的方法,求解方程AtA*X=AtB,得到Xcv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);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,特征向量matVcv::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;}
以上部分我不完全理解就不赘述了,总之经过上面的骚操作我们得到了当前帧的角度旋转量和位移量。
8、积分总变换。
然后就算旋转变化:
// 旋转角的累计变化量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);
其中 AccumulateRotation功能就是计算旋转变化的:
void AccumulateRotation(float cx, float cy, float cz, float lx, float ly, float lz, float &ox, float &oy, float &oz){// 参考:https://www.cnblogs.com/ReedLW/p/9005621.html// 0--->(cx,cy,cz)--->(lx,ly,lz)// 从0时刻到(cx,cy,cz),然后在(cx,cy,cz)的基础上又旋转(lx,ly,lz)// 求最后总的位姿结果是什么?// R*p_cur = R_c*R_l*p_cur ==> R=R_c* R_l//// |cly*clz+sly*slx*slz clz*sly*slx-cly*slz clx*sly|// R_l=| clx*slz clx*clz -slx|// |cly*slx*slz-clz*sly cly*clz*slx+sly*slz cly*clx|// R_c=...// -srx=(ccx*scy,-scx,cly*clx)*(clx*slz,clx*clz,-slx)// ...// 然后根据R再来求(ox,oy,oz)float srx = cos(lx)*cos(cx)*sin(ly)*sin(cz) - cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx);ox = -asin(srx);float srycrx = sin(lx)*(cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy)) + cos(lx)*sin(ly)*(cos(cy)*cos(cz) + sin(cx)*sin(cy)*sin(cz)) + cos(lx)*cos(ly)*cos(cx)*sin(cy);float crycrx = cos(lx)*cos(ly)*cos(cx)*cos(cy) - cos(lx)*sin(ly)*(cos(cz)*sin(cy) - cos(cy)*sin(cx)*sin(cz)) - sin(lx)*(sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx));oy = atan2(srycrx / cos(ox), crycrx / cos(ox));float srzcrx = sin(cx)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) + cos(cx)*sin(cz)*(cos(ly)*cos(lz) + sin(lx)*sin(ly)*sin(lz)) + cos(lx)*cos(cx)*cos(cz)*sin(lz);float crzcrx = cos(lx)*cos(lz)*cos(cx)*cos(cz) - cos(cx)*sin(cz)*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly)) - sin(cx)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx));oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox));}
接着计算当前帧与初始帧的位移变化:
// 进行平移分量的更新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);
void PluginIMURotation(float bcx, float bcy, float bcz, float blx, float bly, float blz, float alx, float aly, float alz, float &acx, float &acy, float &acz){// 参考:https://www.cnblogs.com/ReedLW/p/9005621.html// -imuStart imuEnd 0// transformSum.rot= R * R * R// YXZ ZXY k+1// bcx,bcy,bcz (rx, ry, rz)构成了 R_(k+1)^(0)// blx,bly,blz(imuPitchStart, imuYawStart, imuRollStart) 构成了 R_(YXZ)^(-imuStart)// alx,aly,alz(imuPitchLast, imuYawLast, imuRollLast)构成了 R_(ZXY)^(imuEnd)float sbcx = sin(bcx);float cbcx = cos(bcx);float sbcy = sin(bcy);float cbcy = cos(bcy);float sbcz = sin(bcz);float cbcz = cos(bcz);float sblx = sin(blx);float cblx = cos(blx);float sbly = sin(bly);float cbly = cos(bly);float sblz = sin(blz);float cblz = cos(blz);float salx = sin(alx);float calx = cos(alx);float saly = sin(aly);float caly = cos(aly);float salz = sin(alz);float calz = cos(alz);float srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz);acx = -asin(srx);float srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);float crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);acy = atan2(srycrx / cos(acx), crycrx / cos(acx));float srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + calx*cblx*salz*sblz);float crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) - calx*calz*cblx*sblz);acz = atan2(srzcrx / cos(acx), crzcrx / cos(acx));}
最终位姿变换:
transformSum[0] = rx;transformSum[1] = ry;transformSum[2] = rz;transformSum[3] = tx;transformSum[4] = ty;transformSum[5] = tz;}
9、发布里程计及上一帧点云信息
发布里程计:
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);}
发布点云:
把特征点云投影到每帧的结尾时刻:
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++;
发布各类点云(发布界外点云、线特征点云、面特征点云):
if (frameCount >= skipFrameNum + 1) {frameCount = 0;// 调整坐标系,x=y,y=z,z=xadjustOutlierCloud();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);}}
参考:
LeGO-LOAM源码解析5: featureAssociation(三)
坐标转换与imu融合
【LeGO-LOAM论文阅读(二)--特征提取(二)】相关推荐
- 论文阅读笔记(二)——牛的人脸识别,能做到吗?
论文阅读笔记(二)--牛的人脸识别,能做到吗? 论文简介 论文中文翻译:<牛的人脸识别,能做到吗?> 论文名称:<Face Recognition of Cattle: Can it ...
- 【Transformer】医学分割领域的应用与扩展(论文阅读)(二) || DETR
声明:仅学习使用~ 目录 1. Transformer学习 2. DETR 1. Transformer学习 前篇指路:[Transformer]医学分隔领域的应用与扩展(论文阅读)(一) 继续- 关 ...
- LOAM论文阅读笔记
目录 1.特征点的提取 2.特征点的匹配 3.运动估计 4.运动估计优化 5.建图 参考文章: 1.特征点的提取 velodyne 16雷达每次返回的数据称为一帧(sweep),一帧由16条线组成(每 ...
- 定位系列论文阅读-RoNIN(二)-Robust Neural Inertial Navigation in the Wild: Benchmark, Evaluations
这里写目录标题 0.Abstract 0.1逐句翻译 0.2总结 1. Introduction 1.1逐句翻译 第一段(就是说惯性传感器十分重要有研究的必要) 第二段(惯性导航是非常理想的一个导航方 ...
- GRAIL Efficient Time Series Representation Learning论文阅读笔记(二)
GRAIL Efficient Time Series Representation Learning 有效的时间序列表示学习 作者 芝加哥大学的John Paparrizos和Michael J. ...
- 论文阅读笔记(二):Bridging Video-text Retrieval with Multiple Choice Questions
文章目录 前言 一.Introduction 二.Method 1.双编码器结构 2.Multiple Choice Questions(MCQ) 3.目标函数 4.模型结构 4.1视频编码器 4.2 ...
- 论文阅读笔记(二)【ACL2021】知识抽取NER
学习内容 由于刚刚进入知识图谱领域,对该领域的研究热点不是很了解,所以本文直接翻译ACL2021中关于知识抽取NER中各个论文的摘要和共享: 并且适时在最后写出自己的理解: 同时自己也会在了解完全部后 ...
- 知识图谱论文阅读【十二】【KDD2020】 使用贝叶斯图卷积神经网络推荐精确和多样化项目的框架【看不懂,待续】
题目: A Framework for Recommending Accurate and Diverse Items Using Bayesian Graph Convolutional Neura ...
- 自然语言处理NLP文本分类顶会论文阅读笔记(二)
笔记目录 文本分类 Switch Transformers: Scaling to Trillion Parameter Models with Simple and Efficient Sparsi ...
- 【LeGO-LOAM论文阅读(二)--特征提取(一)】
论文理论部分 特征提取不是从原始点云中进行提取,而是从点云分割中分割出的地面点和分割点中进行提取.参考:LeGO-LOAM论文翻译(内容精简) 过程如下: 只看核心理论部分还是很好理解的.总体流程:( ...
最新文章
- 暴力关闭Windows defender
- 【ABAP】更新交货单交货数量和拣配数量
- 操作系统(三)操作系统的发展和分类
- Spring 5 新特性:函数式Web框架
- boost::ptr_deque相关的测试程序
- java获取接口数据类型_java中调用第三方接口获取数据的方式
- 关闭sublime3自动更新(要输入license才会奏效)
- mysql 与QT的连接
- 小白猿简洁好看的个人介绍单页HTML5源码
- Java基础---File类,就是这么简单
- PHP SEVER预定义变量
- mysql case默认_linux下安装mysql
- 迫切想要成功之后的喜悦感,失败太久有点心灵上小小的打击,还需要继续前进。...
- 【AMESim】AMESim和Simulink联合仿真步骤
- 逻辑回归算法原理MATLAB,逻辑回归算法(MATLAB)
- oracle经纬度精确查询位置,基于经纬度定位车辆所在交通小区位置的方法与流程...
- Android WIFI功能——WifiManager
- 【WeNews】三胞债务重组方案出炉 650亿元金融债务如何化解
- 金蝶KIS商贸版即时库存表二次开发增加保质期有效期至字段列
- 计算机word表格线设置在哪里,word表格文字显示不全_word表格怎么设置显示最后一行线?_word表格怎么加一行...
热门文章
- 如何通过三视图判断立方体个数_中考难点,由三视图确定小立方体个数的问题求解有策略...
- MySQL主从架构、读写分离、集群相关面试问题
- Word2016写论文之题注功能——公式自动编号右对齐等操作
- Office365 Word 打开某个文件就自动卡死关闭
- 安装验证jmeter是否成功
- html下拉菜单换背景颜色,如何在select框下拉菜单中更改蓝色突出显示的颜色
- SAP 账号被锁定,显示无法再进行口令登录的解决办法
- 考研英语二语法知识点
- 在线浏览pdf doc html,前端实现在线预览pdf、word、xls、ppt等文件
- java emoji问号_java过滤emoji字符