Lego_Loam包括了Image projection、Feature association、MapOptmization、Transform Fusion四个部分,下面博主将按照算法的逻辑顺序对代码中的重要函数进行讲解。本节是解析Feature association文件中的特征匹配部分。
该专栏的其他章节链接如下
https://blog.csdn.net/HUASHUDEYANJING/article/details/130332367
一、Feature Extraction
经过上一节的点云特征提取,我们已经获得了稳健的角点和面点如下图所示,下面将介绍如何配准得到位姿Feature Association部分的源码解析
1. Feature Extraction特征提取
// 1.1 点云运动补偿
adjustDistortion();
// 1.2 计算平滑度
calculateSmoothness();
// 1.3 标记遮挡点
markOccludedPoints();
// 1.4 提取特征
extractFeatures();
// 1.5 发布点云
publishCloud();
2. Feature Association特征匹配
if (!systemInitedLM) {
// 2.1 检查系统初始化,点云投影到结束点
checkSystemInitialization();
return;}
// 2.2 更新初始猜测位置
updateInitialGuess();
// 2.3 特征匹配,输出Transformation
updateTransformation();
// 2.4 融合IMU坐标Transformation
integrateTransformation();
二、函数解析
2.1 publishCloudsLast
- 作用:将当前帧点云投影到结束点并发布,作为下一帧匹配的对象,(匹配之前需要有一个匹配对象点云)
- 输入:
cornerPointsSharp角特征 cornerPointsLessSharp缓角特征 surfPointsFlat面特征 surfPointsLessFlat缓面特征 adjustCloud修正点云
- 输出:
/laser_cloud_corner_last /laser_cloud_surf_last /outlier_cloud_last /laser_odom_to_init
- 代码:
下图中红色为前一帧,绿色为当前帧void publishCloudsLast(){ //把特征点云投影到每帧的结尾时刻 updateImuRollPitchYawStartSinCos(); int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size(); for (int i = 0; i < cornerPointsLessSharpNum; i++) { 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 (frameCount >= skipFrameNum + 1) { frameCount = 0; //发布上一帧outlier pubOutlierCloudLast.publish(outlierCloudLast2); //发布上一帧线特征 pubLaserCloudCornerLast.publish(laserCloudCornerLast2); //发布上一帧面特征 pubLaserCloudSurfLast.publish(laserCloudSurfLast2);}
2.2 updateInitialGuess
- 作用:把当前IMU数值作为位姿先验
- 输入:IMU数值
- 输出:transformCur位姿矩阵
// 如果IMU有旋转,则更新变换矩阵以反映新的旋 void updateInitialGuess(){ if (imuAngularFromStartX != 0 || imuAngularFromStartY != 0 || imuAngularFromStartZ != 0){transformCur[0] = - imuAngularFromStartY; // 根据Y轴旋转更新变换矩阵的第一个元素 transformCur[1] = - imuAngularFromStartZ; // 根据Z轴旋转更新变换矩阵的第二个元素 transformCur[2] = - imuAngularFromStartX; // 根据X轴旋转更新变换矩阵的第三个元素} // 如果IMU有移动,则更新变换矩阵以反映新的平移 if (imuVeloFromStartX != 0 || imuVeloFromStartY != 0 || imuVeloFromStartZ != 0) {transformCur[3] -= imuVeloFromStartX * scanPeriod; // 根据X轴移动更新变换矩阵的第四个元素 transformCur[4] -= imuVeloFromStartY * scanPeriod; // 根据Y轴移动更新变换矩阵的第五个元素 transformCur[5] -= imuVeloFromStartZ * scanPeriod; // 根据Z轴移动更新变换矩阵的第六个元素 } }
2.3 updateTransformation
- 作用:匹配角和面特征点,计算位姿
- 输入:
laserCloudSurfLast surfPointsFlat laserCloudCornerLast cornerPointsSharp
- 输出:transformCur位姿矩阵
-
代码
void updateTransformation(){ //检查特征点 if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100) return; // 面特征匹配 for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) { laserCloudOri->clear(); coeffSel->clear(); 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; } }
2.4 findCorrespondingFeatures
计算残差之前,需要先找到对应点,对于角点找点到最近的直线,对于面点找点到最近的平面,,如下图所示
2.4.1 findCorrespondingCornerFeatures
- 作用:找到当前角点的对应点
- 输入:laserCloudCornerLast 、cornerPointsSharp
- 输出:coeffSel所有角点的特征匹配集
- 代码
void findCorrespondingCornerFeatures(int iterCount){ for (int i = 0; i < cornerPointsSharpNum; i++) { TransformToStart(&cornerPointsSharp->points[i], &pointSel); //转换到起始点 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; 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.4.2 findCorrespondingSurfFeatures
- 作用:找到当前面点的对应面
- 输入:laserCloudCornerLast、surfPointsFlat
- 输出:coeffSel所有面点的特征匹配集
- 代码:
void findCorrespondingSurfFeatures(int iterCount){ TransformToStart(&surfPointsFlat->points[i], &pointSel); kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1; if (pointSearchSurfInd2[i] >= 0 && pointSearchSurfInd3[i] >= 0) { // 分别取这3个点(在前一帧的平面点取) tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]]; //a tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]]; //b tripod3 = laserCloudSurfLast->points[pointSearchSurfInd3[i]]; //c 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->push_back(surfPointsFlat->points[i]); coeffSel->push_back(coeff);
2.5 calculateTransformation
- 作用:优化参数目标
- 输入: coeffSel
- 输出:transformCur位姿参数
- 步骤
损失函数
待优化变量
平移旋转矩阵
通过平移旋转矩阵将点云投影,构建残差方程
使用列文虎克迭代求解变量
旋转平移矩阵变化足够小,停止迭代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) { return false;
2.6 integrateTransformation
- 作用:将IMU信息融入到位姿更新当中
- 输入:transformCur transformSum IMU当前数据
- 输出:transformCur transformSum
- 代码
void integrateTransformation(){ float rx, ry, rz, tx, ty, tz; //transformSum 是 IMU累计的变化量 ,0,1,2 分别代表的是 pitch yaw roll 的内容, //transformCur 是当前的分量,函数的作用是局部坐标转换成全局坐标 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); //加入 imu 当前数据 更新位姿 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; }
2.7 publishOdometry
作用:发布位姿信息
输入: transformSum
输出: /laser_odom_to_init
代码
最终发布位姿,格式如下图所示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); //scan与scan的位姿变换 // 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); //发布TF } }
下一节将介绍MapOptmization的源码解析,如果喜欢本专栏请关注吧
评论(0)
您还未登录,请登录后发表或查看评论