当前位置： 首页 > article >正文

lego-loam featureAssociation 源码注释（七）

article 2024/10/30 19:21:07

我们来看featureAssociation的最后两个函数publishOdometry()和publishCloudsLast();

void runFeatureAssociation()
    {

        if (newSegmentedCloud && newSegmentedCloudInfo && newOutlierCloud &&
            std::abs(timeNewSegmentedCloudInfo - timeNewSegmentedCloud) < 0.05 &&
            std::abs(timeNewOutlierCloud - timeNewSegmentedCloud) < 0.05){

            newSegmentedCloud = false;
            newSegmentedCloudInfo = false;
            newOutlierCloud = false;
        }else{
            return;
        }
        /**
        	1. Feature Extraction
        */
        adjustDistortion();

        calculateSmoothness();

        markOccludedPoints();

        extractFeatures();

        publishCloud(); // cloud for visualization
	
        /**
		2. Feature Association
        */
        if (!systemInitedLM) {
            checkSystemInitialization();
            return;
        }

        updateInitialGuess();

        updateTransformation();

        integrateTransformation();

        publishOdometry();

        publishCloudsLast(); // cloud to mapOptimization
    }
};

 publishOdometry()

void publishOdometry(){
        geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformSum[2], -transformSum[0], -transformSum[1]);

        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.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);
    }

 需要注意的有几个点，第一是参数的顺序为什么是transformSum[2], -transformSum[0], -transformSum[1]，第二，为什么后两个参数都加了负号？

也就是输入的顺序其实是rz、rx、ry，所以geoQuat.x对应的是rz，这和laserOdometry.pose.pose.orientation.z对得上。

然后为什么加了负号？对应这两行代码前面也有负号，也对得上。

laserOdometry.pose.pose.orientation.x = -geoQuat.y;
laserOdometry.pose.pose.orientation.y = -geoQuat.z;

如果您有更好的理解，欢迎一起讨论，比心！！

然后来看publishCloudsLast()；

publishCloudsLast()

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]);
        }

        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;

            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);
        }
    }

首先来看updateImuRollPitchYawStartSinCos();

这个函数出现过两次，一次是在adjustDistortion()中，用第一个点的imu值初始化位姿的时候，用第一个点的imu值计算过一次。这一次是在TransformToEnd之前，也就是用当前点的imu值计算一次，所以update的意思就体现在更新。

updateImuRollPitchYawStartSinCos()

void updateImuRollPitchYawStartSinCos(){
        cosImuRollStart = cos(imuRollStart);
        cosImuPitchStart = cos(imuPitchStart);
        cosImuYawStart = cos(imuYawStart);
        sinImuRollStart = sin(imuRollStart);
        sinImuPitchStart = sin(imuPitchStart);
        sinImuYawStart = sin(imuYawStart);
    }

这个函数本身并不复杂，没有什么可说的，唯一需要注意的就是imuStart的定义，是用imuCur的数据来定义的。

imuRollStart = imuRollCur;
imuPitchStart = imuPitchCur;
imuYawStart = imuYawCur;
imuVeloXStart = imuVeloXCur;
imuVeloYStart = imuVeloYCur;
imuVeloZStart = imuVeloZCur;
imuShiftXStart = imuShiftXCur;
imuShiftYStart = imuShiftYCur;
imuShiftZStart = imuShiftZCur;

 然后我们再来看TransformToEnd：

int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size();
        for (int i = 0; i < cornerPointsLessSharpNum; i++) {
            TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]);
        }

TransformToEnd()

 void TransformToEnd(PointType const * const pi, PointType * const po)
    {
        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;

        float x3 = cos(ry) * x2 - sin(ry) * z2;
        float y3 = y2;
        float z3 = sin(ry) * x2 + cos(ry) * z2;

        rx = transformCur[0];
        ry = transformCur[1];
        rz = transformCur[2];
        tx = transformCur[3];
        ty = transformCur[4];
        tz = transformCur[5];

        float x4 = cos(ry) * x3 + sin(ry) * z3;
        float y4 = y3;
        float z4 = -sin(ry) * x3 + cos(ry) * z3;

        float x5 = x4;
        float y5 = cos(rx) * y4 - sin(rx) * z4;
        float z5 = sin(rx) * y4 + cos(rx) * z4;

        float x6 = cos(rz) * x5 - sin(rz) * y5 + tx;
        float y6 = sin(rz) * x5 + cos(rz) * y5 + ty;
        float z6 = z5 + tz;

        float x7 = cosImuRollStart * (x6 - imuShiftFromStartX) 
                 - sinImuRollStart * (y6 - imuShiftFromStartY);
        float y7 = sinImuRollStart * (x6 - imuShiftFromStartX) 
                 + cosImuRollStart * (y6 - imuShiftFromStartY);
        float z7 = z6 - imuShiftFromStartZ;

        float x8 = x7;
        float y8 = cosImuPitchStart * y7 - sinImuPitchStart * z7;
        float z8 = sinImuPitchStart * y7 + cosImuPitchStart * z7;

        float x9 = cosImuYawStart * x8 + sinImuYawStart * z8;
        float y9 = y8;
        float z9 = -sinImuYawStart * x8 + cosImuYawStart * z8;

        float x10 = cos(imuYawLast) * x9 - sin(imuYawLast) * z9;
        float y10 = y9;
        float z10 = sin(imuYawLast) * x9 + cos(imuYawLast) * z9;

        float x11 = x10;
        float y11 = cos(imuPitchLast) * y10 + sin(imuPitchLast) * z10;
        float z11 = -sin(imuPitchLast) * y10 + cos(imuPitchLast) * z10;

        po->x = cos(imuRollLast) * x11 + sin(imuRollLast) * y11;
        po->y = -sin(imuRollLast) * x11 + cos(imuRollLast) * y11;
        po->z = z11;
        po->intensity = int(pi->intensity);
    }

这个代码转来转去也比较绕，我们来理清一下：

首先假设同一帧的点云都是同一位姿，也就是在扫描过程中小车匀速运动。

然后根据特征优化得到的位姿，按时间线性插值做改正，得到从Start初始位姿到当前点的位姿改正量。

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];

从x1y1z1到x3y3z3的过程是计算当前点的线性位姿改正量，从x3y3z3到x6y6z6是从当前点的坐标系又转回Start初始坐标系，你看1->3是zxy的旋转顺序，3->6是yxz的旋转顺序！！为什么要转换初始坐标系呢？

因为我们之前的假设不成立，扫描过程中不是一个匀速运动的过程。

我们要通过imu的数据插值，来对位姿进行修正：

从x6y6z6到x9y9z9的过程就是通过我们更新过后的cosimu和sinimu的值做位姿修正。

但是这个修正非常粗糙，没有用到imu预积分的内容，在lio-sam中对imu的处理更加细致，未来我们也会注释lio-sam的代码。

最后一次转换x9y9z9到po，就是为下一帧的特征匹配做准备，我们知道，当前帧的最后一个点对应的imu数值，也就是最后一个imu数值，是下一帧的初始坐标系。

我们将当前点转到下一帧的初始坐标系上，再将下一帧的特征点转到初始坐标系上，就可以循环优化位姿了！！

！！注意转换之后，更新了kd树！！这同样是为特征匹配优化位姿做准备！！

if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) {
            kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
            kdtreeSurfLast->setInputCloud(laserCloudSurfLast);
        }

 最后向建图的程序发布数据

 if (frameCount >= skipFrameNum + 1) {

            frameCount = 0;

            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);
        }

 发布的数据有离群点，角特征点和面特征点。

skipFrameNum初值为1。

所以frameCount == 2时才发布，也就是两帧一发布，建图的频率比雷达里程计慢一半。

！！终于把featureAssociation注释完啦！！

下一节进入mapOptmization！！


http://www.kler.cn/a/371947.html

相关文章：

  • Vue3 学习笔记（十三）Vue组件详解
  • 代码随想录算法训练营第十二天（补） 二叉树| 二叉树理论知识、深度优先遍历、广度优先遍历
  • 使用Python和OpenCV实现火焰检测
  • [mysql]相关子查询
  • 探熵科技|以科技赋能销售，创造卓越业绩。
  • excel IF函数用法
  • 使用 Kafka 和 MinIO 实现人工智能数据工作流
  • Windows 上更新OpenSSL 到 1.1.1
  • 现代化可观测性平台（1）
  • C语言常用的数据类型有哪些？
  • uniapp封装movable-area+movable-view组件，实现悬浮按钮可拖动，自动吸附边缘效果，自动向两边靠拢
  • element ui中el-image组件查看图片的坑
  • QT相机连接与拍照
  • 深度学习（七）深度强化学习：融合创新的智能之路（7/10）
  • Python 爬虫项目实战：爬取某云热歌榜歌曲
  • 使用Python来下一场雪
  • STM32F103C8T6 IO 操作
  • DevOps赋能：优化业务价值流的实战策略与路径（下）
  • ViSual studio如何安装 并使用GeographicLib
  • 大模型提示词简介 举例
  • zjy-sqlite-manage使用文档v1
  • 每日读则推(十四)——Meta Movie Gen: the most advanced media foundation models to-date
  • 等保行业如何选择核实的安全防御技术
  • Python 机器学习中的模型解释性与可解释性
  • 有防蓝光的护眼灯有哪些品牌？介绍五款值得入手的品牌和型号
  • 深度学习-交叉熵损失函数