LIO-SAM 详读代码笔记 -- 5.MapOptimization

最新推荐文章于 2024-05-10 19:26:54 发布

老虎福

最新推荐文章于 2024-05-10 19:26:54 发布

阅读量1k

点赞数

分类专栏：激光SLAM slam 文章标签：自动驾驶人工智能机器学习

本文链接：https://blog.csdn.net/fuzi2012/article/details/126429165

版权

slam 同时被 2 个专栏收录

6 篇文章 3 订阅

订阅专栏

激光SLAM

5 篇文章 2 订阅

订阅专栏

这个模块主要的功能就是接收featureExtraction 传来的coud_info 数据，把点云特征点根据初始化的里程计信息转换到map坐标系下，然后对当前帧的点云特征点信息进行scan-map 匹配优化，这里用到loam里面的点到线，点到面的距离作为残差项，用高斯牛顿法优化里程计信息，然后按照匀速运动模型计算一个超前一个lidar周期的odom_imcremental里程计，传送到IMU 预积分模块使用。

图2.ros topic 发布与订阅关系图关系图（还是叫图2，因为是同一个图）

图1.代码架构图(数据流图)

成员变量列表

变量名	类型	注释	备注
gtSAMgraph	NonlinearFactorGraph	因子图	-
initialEstimate	Values	因子途中待优化的变量	-
optimizedEstimate	Values	因子图中的变量，用于保存优化后的结果	-
*isam	ISAM2	因子图的优化器	-
isamCurrentEstimate	Values	因子图中的变量	-
poseCovariance	Eigen::MatrixXd	因子图中的协方差模型	-
pubLaserCloudSurround	ros::Publisher	发布全局地图？ "lio_sam/mapping/map_global" 这里也没有发布全局地图，有设定阈值，默认时1000米范围内的特征点地图	-
pubLaserOdometryGlobal	ros::Publisher	发布地图优化后的里程计信息 "lio_sam/mapping/odometry" // 在imuPreintegration文件的中的TransformFusion类订阅，融合imu 里程计后发布出发布 "odometry/imu_incremental" // 供imageProjection 1.lidar 运动畸变矫正 2.提取lidar的位姿信息集成到cloud-info中回到当前这个类作lidar 位姿初始化	-
pubLaserOdometryIncremental	ros::Publisher	发布 "lio_sam/mapping/odometry_incremental" 里程计信息，与上面的区别是，比lio_sam/mapping/map_global 超前一个Lidar优化周期的里程计信息， // 在 imuPreintegration 类中订阅，用于因子途中位姿优化的位姿初始化	-
pubKeyPoses	ros::Publisher	发布 "lio_sam/mapping/trajectory"，sensor_msgs::PointCloud2类型系统中没有人订阅	-
pubPath	ros::Publisher	发布 "lio_sam/mapping/path"	-
pubHistoryKeyFrames	ros::Publisher	发布"lio_sam/mapping/icp_loop_closure_history_cloud"	-
pubHistoryKeyFrames	ros::Publisher	发布"lio_sam/mapping/icp_loop_closure_history_cloud"	-
pubIcpKeyFrames	ros::Publisher	//"lio_sam/mapping/icp_loop_closure_corrected_cloud"	-
pubRecentKeyFrames	ros::Publisher	//"lio_sam/mapping/map_local" sensor_msgs::PointCloud2 ?	-
pubRecentKeyFrames	ros::Publisher	//"lio_sam/mapping/cloud_registered sensor_msgs::PointCloud2 ?	-
pubCloudRegisteredRaw	ros::Publisher	// "lio_sam/mapping/cloud_registered_raw"	-
pubLoopConstraintEdge	ros::Publisher	"/lio_sam/mapping/loop_closure_constraints" visualization_msgs::MarkerArray ?	-
pubSLAMInfo	ros::Publisher	//"lio_sam/mapping/slam_info" lio_sam::cloud_info	-
subCloud	ros::Subscriber	//"lio_sam/feature/cloud_info" lio_sam::cloud_info 订阅来自featureExtraction的cloud_info	-
subGPS	ros::Subscriber	//"odometry/gpsz" # GPS odometry topic from navsat, see module_navsat.launch file	-
subLoop	ros::Subscriber	//"lio_loop/loop_closure_detection" 类型std_msgs::Float64MultiArray	-
srvSaveMap	ros::ServiceServer		-
gpsQueue	std::deque	// GPS的消息队列	-
cloudInfo	lio_sam::cloud_info	// featureExtraction发来的点云特征消息，这里不再是队列， //来一个处理一个，处理的延迟应该低于lidar数据发布延迟，不然会卡顿丢帧？优化过程是执行周期 mappingProcessInterval，lidar 数据的频率是10Hz	-
cornerCloudKeyFrames	vector::Ptr>	真正地图存储的数据 // 数组存放每个关键帧的边角点	-
surfCloudKeyFrames	vector::Ptr>	真正地图存储的数据 // 数组存放每个关键帧的平面点	-
cloudKeyPoses3D	vector::Ptr>	真正地图存储的数据 // //存放关键帧的位置 (不包含旋转信息)	-
cloudKeyPoses6D	pcl::PointCloud::Ptr	真正地图存储的数据 // 存放关键帧的位置 (包含旋转信息) ，位置信息跟cloudKeyPoses3D 一致	-
copy_cloudKeyPoses3D	pcl::PointCloud::Ptr	临时变量	-
copy_cloudKeyPoses6D	pcl::PointCloud::Ptr	临时变量	-
laserCloudCornerLast	pcl::PointCloud::Ptr	corner feature set from odoOptimization 从cloud_info 中提取的当前帧的边角点点云数据	-
laserCloudSurfLast	pcl::PointCloud::Ptr	surf feature set from odoOptimization 从cloud_info 中提取的当前帧的平面点点云数据	-
laserCloudCornerLastDS	pcl::PointCloud::Ptr	downsampled corner feature set from odoOptimization	-
laserCloudSurfLastDS	pcl::PointCloud::Ptr	downsampled surf feature set from odoOptimization	-
laserCloudOri	pcl::PointCloud::Ptr	scan-map优化过程中存放lidar 坐标系下的特征点数据	-
coeffSel	pcl::PointCloud::Ptr	// 优化过程中存放距离函数对lidar 坐标系下的特征点映射到map坐标系后的雅可比	-
laserCloudOriCornerVec	std::vector	在使用openmp 加速计算时用于保存角点特征原点的变量// corner point holder for parallel computation	-
coeffSelCornerVec	std::vector	在使用openmp 加速计算时用于保存角点特征原点的雅可比变量// corner point holder for parallel computation	-
laserCloudOriCornerFlag	std::vector	在使用openmp 加速计算时用于保存角点特征原点的占用标志位// corner point holder for parallel computation	-
laserCloudOriSurfVec	std::vector	在使用openmp 加速计算时用于保存平面特征原点的变量// surf point holder for parallel computation	-
coeffSelSurfVec	std::vector	在使用openmp 加速计算时用于保存平面特征原点的雅可比变量// surf point holder for parallel computation	-
laserCloudOriSurfFlag	std::vector	在使用openmp 加速计算时用于保存平面点特征原点的占用标志位// surf point holder for parallel computation	-
laserCloudMapContainer	map, pcl::PointCloud>>	保存地图这个地图容器 1000帧关键帧之后，会清空，所以保存的不是全局地图 // 作用仅仅时缓存一些关键帧数据，能够快速的构建局部地图	-
laserCloudCornerFromMap	pcl::PointCloud::Ptr	局部地图的所有角点特征融合	-
laserCloudSurfFromMap	pcl::PointCloud::Ptr	// 局部地图的所有面点特征	-
laserCloudCornerFromMapDS	pcl::PointCloud::Ptr	// 局部地图的所有角点特征的下采样点云，在scan-map 优化值中真正用到的数据	-
laserCloudSurfFromMapDS	pcl::PointCloud::Ptr	// 局部地图的所有面点特征的下采样点云，在scan-map 优化值中真正用到的数据	-
kdtreeCornerFromMap	pcl::KdTreeFLANN::Ptr	// 局部地图的所有角点特征的下采样点云对用的Kdtree	-
kdtreeSurfFromMap	pcl::KdTreeFLANN::Ptr	// 局部地图的所有面点特征的下采样点云对用的Kdtree	-
kdtreeSurroundingKeyPoses	pcl::KdTreeFLANN::Ptr	// 关键帧位置数组对应的一个KDtree	-
kdtreeHistoryKeyPoses	pcl::KdTreeFLANN::Ptr	// 关键帧位置数组对应的一个KDtree,这个对应可视化部分用到的	-
downSizeFilterCorner	pcl::VoxelGrid	// 下采样器	-
downSizeFilterSurf	pcl::VoxelGrid	// 下采样器	-
downSizeFilterICP	pcl::VoxelGrid	// 下采样器	-
downSizeFilterSurroundingKeyPoses	pcl::VoxelGrid	for surrounding key poses of scan-to-map optimization// 下采样器	-
timeLaserInfoStamp	ros::Time	这个时间戳是cloud_info 的时间戳，经过了imageProjection /featureExtraction 已经不是很新鲜的是时间戳了	-
timeLaserInfoCur	double	这个时间戳是cloud_info 的时间戳，经过了imageProjection /featureExtraction 已经不是很新鲜的是时间戳了	-
transformTobeMapped[6]	float	待优化的位姿变量，整个文件围绕着这个变量进行	-
mtx	std::mutex	互斥量	-
mtxLoopInfo	std::mutex	回环检测的互斥量	-
isDegenerate	bool	标志要不要计算优化变量的协方差，雅可比矩阵的特征值均小于100时不需要计算协方差	-
matP	cv::Mat	优化变量的协方差矩阵	-
laserCloudCornerFromMapDSNum	int		-
laserCloudSurfFromMapDSNum	int		-
laserCloudCornerLastDSNum	int		-
laserCloudSurfLastDSNum	int		-
aLoopIsClosed	bool	标志是否有回环检测模块	-
loopIndexContainer	map		-
loopPoseQueue	vector		-
loopNoiseQueue	vector		-
loopInfoVec	deque		-
globalPath	nav_msgs::Path		-
transPointAssociateToMap	Eigen::Affine3f	把lidar 点云映射到的map坐标系的转换矩阵，对应着transformTobeMapped 的值	-
incrementalOdometryAffineFront	Eigen::Affine3f	本文件整个优化前对应着transformTobeMapped 的值	-
incrementalOdometryAffineBack	Eigen::Affine3f	本文件整个优化后对应着transformTobeMapped 的值，这两转换矩阵作差，就是匀速运动学模型的速度，用于计算lidar 周期的朝一个周期的优化里程计信息，在发布 odom_imcremental 里程计时要用到	-

代码注解

这里的代码的主要逻辑从 cloud_info 的回调函数为入口：

/**
 * @brief  CloudInfo 的回调函数
 * 
 * @param msgIn 
 */
    void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
    {
        // extract time stamp
        timeLaserInfoStamp = msgIn->header.stamp;
        timeLaserInfoCur = msgIn->header.stamp.toSec();

        // extract info and feature cloud
        cloudInfo = *msgIn;
        pcl::fromROSMsg(msgIn->cloud_corner,  *laserCloudCornerLast);  // 当前帧的角点
        pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);    // 当前帧的平面点

        std::lock_guard<std::mutex> lock(mtx);

        static double timeLastProcessing = -1;
        // seconds, regulate mapping frequencymapOptimization 的执行时间间隔，地图优化的执行周期,比lidar 频率慢
        if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval) 
        {
            timeLastProcessing = timeLaserInfoCur;
            /**
             * @brief 把上一次有优化的结果 transformTobeMapped存放到incrementalOdometryAffineFront 之后
             * 用imu预积分的里程计信息或者IMU 信息(只做旋转变换)初始化当前lidar的位姿transformTobeMapped
             * 
             */
            updateInitialGuess();
            /**
             * @brief Construct a new extract Surrounding Key Frames object
             * 在cloudKeyPoses3D 这个数组中搜索 以loudKeyPoses3D.back()为中心 的周边50米的距离的关键帧数据 构建局部地图
             * 并构造角点局部地图和平面点局部地图 ，
             * 存放在laserCloudCornerFromMapDS
             * 和laserCloudSurfFromMapDS
             */
            extractSurroundingKeyFrames();
            /**
             * @brief 对当前帧的点云角点 和平面点 进行下采样
             * 
             */
            downsampleCurrentScan();
            //用局部构建的地图去优化当前帧的位姿 
            scan2MapOptimization();
            // 添加因子图和保存优化后的关键帧数据
            saveKeyFramesAndFactor();
            // 如果发生回环 ，需要把回环对应的历史位姿更新以下
            correctPoses();

            publishOdometry();

            publishFrames();
        }
    }

做值优化问题，都需要有一个初始值，在loam 和lego_loam 中都是通过scan-scan 的方式得出scan-map的初始值，有了imu 数据之后，可以通过imu 预积分数据作scan-map的初始值。


/**
 * @brief 把上一次有优化的结果 transformTobeMapped存放到incrementalOdometryAffineFront 之后
 * 用imu预积分的里程计信息或者IMU 信息(只做旋转变换)初始化当前lidar的位姿transformTobeMapped
 * 
 */
    void updateInitialGuess()
    {
        // save current transformation before any processing
        incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped); // 把上一次优化的结果 保存在incrementalOdometryAffineFront

        static Eigen::Affine3f lastImuTransformation; // 保存IMU 里程计到当前帧时刻的里程计信息
        // initialization
        if (cloudKeyPoses3D->points.empty()) // 地图里面没有数据，表示当前为初始位置，
        {
            // 初始时刻 机器人的朝向取 IMU 的朝向
            transformTobeMapped[0] = cloudInfo.imuRollInit;
            transformTobeMapped[1] = cloudInfo.imuPitchInit;
            transformTobeMapped[2] = cloudInfo.imuYawInit;

            // 超参数：在yaml 文件中设置 if using GPS data, set to "true" ，useImuHeadingInitialization 如果不用 imu朝向 ，把yaw 置为0  
            if (!useImuHeadingInitialization)
                transformTobeMapped[2] = 0;

            lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
            return;
        }

        // use imu pre-integration estimation for pose guess
        static bool lastImuPreTransAvailable = false;
        //这个变量保存IMU 的预积分的里程计信息，关联两次优化
        static Eigen::Affine3f lastImuPreTransformation;
        if (cloudInfo.odomAvailable == true)
        {
            // 这里看到一对 XXXFront 和 XXXBack ，之后肯定是要作差的，表示之前的时刻到当前时刻的位姿差
            Eigen::Affine3f transBack = pcl::getTransformation(cloudInfo.initialGuessX,    cloudInfo.initialGuessY,     cloudInfo.initialGuessZ, 
                                                               cloudInfo.initialGuessRoll, cloudInfo.initialGuessPitch, cloudInfo.initialGuessYaw);
            if (lastImuPreTransAvailable == false)
            {   // 第一次设置时， 
                lastImuPreTransformation = transBack;
                lastImuPreTransAvailable = true;
            } else {
                // 两次优化之间的发生的位姿差 delta_T = T_i^T * T_j
                Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;
                Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
                // T_j'' = T_i' * delta_T 其中 T_j'' 为待优化的变量的初始值
                Eigen::Affine3f transFinal = transTobe * transIncre;
                pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
                                                              transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);

                lastImuPreTransformation = transBack; // 一直保存IMU 预积分里程计的信息，传递下去

                lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
                return;
            }
        }

        // use imu incremental estimation for pose guess (only rotation)
        if (cloudInfo.imuAvailable == true)
        {
            Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
            Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack;

            Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
            Eigen::Affine3f transFinal = transTobe * transIncre;
            pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
                                                          transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);

            lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); // save imu before return;
            return;
        }
    }

在scan-map 优化执行之前，首先需要有一个map，淡然不能是全局map,内存撑爆，且运算时间剧增，也没必要用全局map ,这里用的是距离最新历史关键帧50米范围内的关键帧数据作为一个局部地图取匹配优化当前帧的位姿。

/**
 * 这个函数就是调用了extractNearby函数
 * */
    void extractSurroundingKeyFrames()
    {
        if (cloudKeyPoses3D->points.empty() == true)
            return; 
        // ？
        // if (loopClosureEnableFlag == true)
        // {
        //     extractForLoopClosure();    
        // } else {
        //     extractNearby();
        // }

        extractNearby();
    }

上面这个函数就是调用了extractNearby函数，在cloudKeyPoses3D 这个数组中搜索以loudKeyPoses3D.back()为中心的周边50米的距离的关键帧数据构建局部地图。

/**
 * @brief 在cloudKeyPoses3D 这个数组中搜索 以loudKeyPoses3D.back()为中心 的周边50米的距离的关键帧数据 构建局部地图
 * 
 */
    void extractNearby()
    {   // 搜索出来距离50米的关键帧位姿（位置）存放在这个点云容器中
        pcl::PointCloud<PointType>::Ptr surroundingKeyPoses(new pcl::PointCloud<PointType>());
        pcl::PointCloud<PointType>::Ptr surroundingKeyPosesDS(new pcl::PointCloud<PointType>());
        // 这两个变量是 KDtree 搜索用到的两个临时变量
        std::vector<int> pointSearchInd;
        std::vector<float> pointSearchSqDis;

        // extract all the nearby key poses and downsample them
        kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D); // create kd-tree
        // 以cloudKeyPoses3D->back() 为中心 以surroundingKeyframeSearchRadius（默认50米）为半径 搜索最近的点
        kdtreeSurroundingKeyPoses->radiusSearch(cloudKeyPoses3D->back(), (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis);
        for (int i = 0; i < (int)pointSearchInd.size(); ++i)
        {
            int id = pointSearchInd[i];
            surroundingKeyPoses->push_back(cloudKeyPoses3D->points[id]);
        }

        downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
        // 下采样之后，点云里的点不完全时原始的点，有些是融合过 ，所以时间戳信息 需要重新获取一下
        downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);
        for(auto& pt : surroundingKeyPosesDS->points)
        {   // 再找一遍，把原始点云的最近的点的intensity赋值给新的位置点 ，intensity 在这个是在这个地图中的索引 也是laserCloudMapContainer容器的索引
            kdtreeSurroundingKeyPoses->nearestKSearch(pt, 1, pointSearchInd, pointSearchSqDis);
            pt.intensity = cloudKeyPoses3D->points[pointSearchInd[0]].intensity;
        }

        // also extract some latest key frames in case the robot rotates in one position
        int numPoses = cloudKeyPoses3D->size();
        for (int i = numPoses-1; i >= 0; --i)
        {
            // 过滤完之后 ，surroundingKeyPosesDS在添加一些  时间比较接近的位置点
            if (timeLaserInfoCur - cloudKeyPoses6D->points[i].time < 10.0)
                surroundingKeyPosesDS->push_back(cloudKeyPoses3D->points[i]);
            else
                break;
        }

        extractCloud(surroundingKeyPosesDS);
    }

然后再调用extractCloud 函数,搜索出来距离50米的关键帧位姿（位置）的角点点云和平面点点云,存放在laserCloudCornerFromMapDS和laserCloudSurfFromMapDS中

/**
 * @brief  搜索出来距离50米的关键帧位姿（位置）的角点点云和平面点点云
 * 存放在laserCloudCornerFromMapDS
 * 和laserCloudSurfFromMapDS
 * 
 * @param cloudToExtract 
 */
    void extractCloud(pcl::PointCloud<PointType>::Ptr cloudToExtract)
    {
        // fuse the map
        laserCloudCornerFromMap->clear();
        laserCloudSurfFromMap->clear(); 
        for (int i = 0; i < (int)cloudToExtract->size(); ++i)
        {   // 添加关键帧的搜索半径 ， 局部地图的距离最新帧中心阈值
            if (pointDistance(cloudToExtract->points[i], cloudKeyPoses3D->back()) > surroundingKeyframeSearchRadius)
                continue;

            int thisKeyInd = (int)cloudToExtract->points[i].intensity;
            if (laserCloudMapContainer.find(thisKeyInd) != laserCloudMapContainer.end()) 
            {   // 局部地图 角点地图 和平面点地图
                // transformed cloud available
                *laserCloudCornerFromMap += laserCloudMapContainer[thisKeyInd].first;
                *laserCloudSurfFromMap   += laserCloudMapContainer[thisKeyInd].second;
            } else {
                // transformed cloud not available
                // 把点云 从lidar 坐标系转到world 坐标系
                pcl::PointCloud<PointType> laserCloudCornerTemp = *transformPointCloud(cornerCloudKeyFrames[thisKeyInd],  &cloudKeyPoses6D->points[thisKeyInd]);
                pcl::PointCloud<PointType> laserCloudSurfTemp = *transformPointCloud(surfCloudKeyFrames[thisKeyInd],    &cloudKeyPoses6D->points[thisKeyInd]);
                *laserCloudCornerFromMap += laserCloudCornerTemp;
                *laserCloudSurfFromMap   += laserCloudSurfTemp;
                // 添加到 地图容器中
                laserCloudMapContainer[thisKeyInd] = make_pair(laserCloudCornerTemp, laserCloudSurfTemp);
            }
            
        }

        // Downsample the surrounding corner key frames (or map)
        downSizeFilterCorner.setInputCloud(laserCloudCornerFromMap);
        downSizeFilterCorner.filter(*laserCloudCornerFromMapDS);
        laserCloudCornerFromMapDSNum = laserCloudCornerFromMapDS->size();
        // Downsample the surrounding surf key frames (or map)
        downSizeFilterSurf.setInputCloud(laserCloudSurfFromMap);
        downSizeFilterSurf.filter(*laserCloudSurfFromMapDS);
        laserCloudSurfFromMapDSNum = laserCloudSurfFromMapDS->size();

        // clear map cache if too large
        // 地图数据清空 缓存1000帧
        if (laserCloudMapContainer.size() > 1000)
            laserCloudMapContainer.clear();
    }

有了初始值，有了局部地图，就可以执行scan-map 匹配优化了。

分三部：1.根据初始值变换角点到map 坐标系下，计算点到线的残差项，及其对变换后的点的雅可比，保存起来。

2. 根据初始值变换平面点到map 坐标系下，计算点到面的残差项，及其对变换后的点的雅可比，保存起来。

3. 计算变换后的点对变换矩阵中 R 和t 的雅可比，然后用高斯牛顿法优化待优化变量。

这里详细讲解一下计算过程，可能这部分内容源自loam 或者是其他文献，所以著者也没有详细给出讲解：

(1)先准备以下数据：由于有了初始值 transformTobeMapped ，我们可以得到初始的旋转角 $\theta _{x}$ , $\theta_{y}$ , $\theta_{z}$ , $t_x$ , $t_y$ , $t_z$ 。

所与可以得到旋转矩阵

$R=R_{x}R_{y}R_{z}=\begin{bmatrix} 1& 0&0 \\ 0& cos\theta_x &-sin\theta_x \\ 0& sin\theta_x & cos\theta_x \end{bmatrix}\begin{bmatrix} cos\theta_y& 0&sin\theta_y \\ 0& 1& 0\\ -sin\theta_y& 0 & cos\theta_y \end{bmatrix}\begin{bmatrix} cos\theta_z& -sin\theta_z&0 \\ sin\theta_z& cos\theta_z& 0\\ 0& 0 & 1 \end{bmatrix}$

(2) 论文中给出点到线的距离公式，这里直接沿用，至于怎么来loam中讲得很明白，这里旨在读懂代码，明确代码里的变量怎么来。

点到线的距离公式： $d_{ek} =\frac{\left \| (p^e_{i+1,k} - p^e_{i,u} )\times (p^e_{i+1,k} - p^e_{i,v} )\right \|}{\left \| p^e_{i,u} - p^e_{i,v} \right \|}$ ,为了简化公式编写且与代码中的变量标号相对应，点到线的距离公式简化成 $d =\frac{\left \| (p_0 - p_1 )\times (p_0 - p_2)\right \|}{\left \| p_1- p_2\right \|}$ ，首先要确定这几个点是怎么来的，其中 $p_0$ 为从lidar 坐标系变换到map 坐标系下的点，即 $p_0 = Rp^e + t$ , $p_1$ 和 $p_2$ 主要来自与主成分分析法(PCA),在map 中找出距离 $p_0$ 最近的5个点，作主成分分析，如果这5个的点协方差矩阵的其中一个特征值远大于其他两个，则这5个点成线性分布，最大特征值对应的特征向量即为该直线的方向，在这5个点的中心沿着直线的正反方向各取一个点，即为 $p_1$ 和 $p_2$ 。

（3）这里先给出 $p_0$ 对 $R$ 和 $t$ 的雅可比，因为无论是平面点或者是角点特征，都经过了这个变换，雅可比是一致，代码中也是这么分开来实现的。

$\frac{\partial p_0}{\partial \theta} = \frac{\partial (Rp^e+t)}{\partial \theta}=lim_{\theta->0}\frac{exp(I+\theta^\times )Rp^e-Rp^e}{\theta} =lim_{\theta->0}\frac{(I+\theta^\times )Rp^e-Rp^e}{\theta} =\frac{\theta^\times Rp^e}{\theta} =-\frac{(Rp^e)^\times \theta}{\theta} = -(Rp^e)^\times$

对 $t$ 的雅可比为 $I$ 。

（4）在函数cornerOptimization中求得点到线距离的残差项存放在coeff.intensity 中，雅可比项就是存放在coeff的x,y,z 中。现在来推到点到线距离残差项 $d$ 对 $p_0$ 的雅可比。

1）首先要知道 $\frac{\partial \left \| x\right \|}{\partial x} = \frac{x}{\left \| x \right \|}$ 各位可以用（x,y,z）的模来推导一下.

2) $\frac{\partial d}{\partial p_0} = \frac{1}{\left \| p_1- p_2\right \|}\frac{\partial \left \| (p_0 - p_1 )\times (p_0 - p_2)\right \|}{\partial p_0}$

$= \frac{1}{\left \| p_1- p_2\right \|} \frac{ (p_0 - p_1 )\times (p_0 - p_2)}{\left \| (p_0 - p_1 )\times (p_0 - p_2)\right \|} \frac{\partial (p_0 - p_1 )\times (p_0 - p_2)}{\partial p_0}$

$=\frac{1}{\left \| p_1- p_2\right \|} \frac{ (p_0 - p_1 )\times (p_0 - p_2)}{\left \| (p_0 - p_1 )\times (p_0 - p_2)\right \|} (\frac{\partial (p_0 - p_1 )^\times (p_0 - p_2)}{\partial p_0} + \frac{ (p_0 - p_1 )^\times \partial (p_0 - p_2)}{\partial p_0} )$ $=\frac{1}{\left \| p_1- p_2\right \|} \frac{ (p_0 - p_1 )\times (p_0 - p_2)}{\left \| (p_0 - p_1 )\times (p_0 - p_2)\right \|} (\frac{- (p_0 - p_2)^\times \partial (p_0 - p_1 )}{\partial p_0} + \frac{ (p_0 - p_1 )^\times \partial (p_0 - p_2)}{\partial p_0} )$ $=\frac{1}{\left \| p_1- p_2\right \|} \frac{ (p_0 - p_1 )\times (p_0 - p_2)}{\left \| (p_0 - p_1 )\times (p_0 - p_2)\right \|} (- (p_0 - p_2)^\times + (p_0 - p_1 )^\times )$

$=\frac{1}{\left \| p_1- p_2\right \|} \frac{ (p_0 - p_1 )\times (p_0 - p_2)}{\left \| (p_0 - p_1 )\times (p_0 - p_2)\right \|} (- (p_1 - p_2)^\times )$

然后一项一项对应到代码里面去就好了。这样比看代码好理解一些。点到平面的距离就不写了（懒）。

void cornerOptimization()
    {   // 把初始化过后的待优化变量transformTobeMapped 转换成一个转换矩阵transPointAssociateToMap，
        // 用于对当前帧点云的坐标系转化到map 坐标系 
        updatePointAssociateToMap();

        #pragma omp parallel for num_threads(numberOfCores)
        for (int i = 0; i < laserCloudCornerLastDSNum; i++)
        {
            // pointOri lidar 坐标系下的特征点    
            // pointSel 是pointOri 转化到Map 坐标系下的点
            // coeff 距离函数对pointSel 的雅可比
            PointType pointOri, pointSel, coeff;
            // kdtree 搜索近邻点所需要的两个临时变量
            std::vector<int> pointSearchInd;
            std::vector<float> pointSearchSqDis;

            pointOri = laserCloudCornerLastDS->points[i]; //当前帧的特征点
            // 把点云点从lidar 坐标系映射到Map 坐标系 
            pointAssociateToMap(&pointOri, &pointSel);
            // 以pointSel 为中心在局部地图的KdTree中找最近的5个点
            kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

            cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0)); // matA1 为这5个点的协方差矩阵
            cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0)); // matD1 为协方差矩阵的特征值
            cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0)); // matV1 为协方差矩阵的特征向量
             //从KdTree中找最近的5个点中最远距离的那个的距离小于1M，认为中的找到的这5个点有效       
            if (pointSearchSqDis[4] < 1.0) {
                // C =[cx,cy,cz]^T 为这5个点的中心
                float cx = 0, cy = 0, cz = 0;
                for (int j = 0; j < 5; j++) {
                    cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
                    cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
                    cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
                }
                cx /= 5; cy /= 5;  cz /= 5;

                float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
                for (int j = 0; j < 5; j++) {
                    float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
                    float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
                    float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;

                    a11 += ax * ax; a12 += ax * ay; a13 += ax * az;
                    a22 += ay * ay; a23 += ay * az;
                    a33 += az * az;
                }
                a11 /= 5; a12 /= 5; a13 /= 5; a22 /= 5; a23 /= 5; a33 /= 5;

                matA1.at<float>(0, 0) = a11; matA1.at<float>(0, 1) = a12; matA1.at<float>(0, 2) = a13;
                matA1.at<float>(1, 0) = a12; matA1.at<float>(1, 1) = a22; matA1.at<float>(1, 2) = a23;
                matA1.at<float>(2, 0) = a13; matA1.at<float>(2, 1) = a23; matA1.at<float>(2, 2) = a33;
                // 求取matA1 的特征值和特征向量  
                cv::eigen(matA1, matD1, matV1);
                // matD1 是按从达到小的顺序分布的，如果特征值的第1个值比第二值足够大  则认为 这5个点是成线型分布的
                // 并且 其对应的特征向量则为直线的方向
                if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {
                    // 构造3个点，这样就可计算一个点到其他两个点所在的直线的距离
                    // 1.当前帧lidar 在map坐标系下的映射点
                    float x0 = pointSel.x;
                    float y0 = pointSel.y;
                    float z0 = pointSel.z;
                    // 2. 在5点的均值点处沿着 直线方向 距离0.1 取一个点
                    float x1 = cx + 0.1 * matV1.at<float>(0, 0);
                    float y1 = cy + 0.1 * matV1.at<float>(0, 1);
                    float z1 = cz + 0.1 * matV1.at<float>(0, 2);
                    // 3. 在5点的均值点处沿着 直线反方向 距离0.1 取一个点
                    float x2 = cx - 0.1 * matV1.at<float>(0, 0);
                    float y2 = cy - 0.1 * matV1.at<float>(0, 1);
                    float z2 = cz - 0.1 * matV1.at<float>(0, 2);

                    float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
                                    + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
                                    + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));

                    float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));

                    float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
                              + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;

                    float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
                               - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;

                    float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
                               + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;

                    float ld2 = a012 / l12;
                    // 残差项乘以一个系数，类似于核函数
                    float s = 1 - 0.9 * fabs(ld2);

                    coeff.x = s * la;
                    coeff.y = s * lb;
                    coeff.z = s * lc;
                    coeff.intensity = s * ld2;

                    if (s > 0.1) {
                        laserCloudOriCornerVec[i] = pointOri;
                        coeffSelCornerVec[i] = coeff;
                        laserCloudOriCornerFlag[i] = true;
                    }
                }
            }
        }
    }

    void surfOptimization()
    {
        updatePointAssociateToMap();

        #pragma omp parallel for num_threads(numberOfCores)
        for (int i = 0; i < laserCloudSurfLastDSNum; i++)
        {
            PointType pointOri, pointSel, coeff;
            std::vector<int> pointSearchInd;
            std::vector<float> pointSearchSqDis;

            pointOri = laserCloudSurfLastDS->points[i];
            pointAssociateToMap(&pointOri, &pointSel); 
            kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

            Eigen::Matrix<float, 5, 3> matA0;
            Eigen::Matrix<float, 5, 1> matB0;
            Eigen::Vector3f matX0;

            matA0.setZero();
            matB0.fill(-1);
            matX0.setZero();

            if (pointSearchSqDis[4] < 1.0) {
                for (int j = 0; j < 5; j++) {
                    matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
                    matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
                    matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
                }

                matX0 = matA0.colPivHouseholderQr().solve(matB0);

                float pa = matX0(0, 0);
                float pb = matX0(1, 0);
                float pc = matX0(2, 0);
                float pd = 1;

                float ps = sqrt(pa * pa + pb * pb + pc * pc);
                pa /= ps; pb /= ps; pc /= ps; pd /= ps;

                bool planeValid = true;
                for (int j = 0; j < 5; j++) {
                    if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
                             pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
                             pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2) {
                        planeValid = false;
                        break;
                    }
                }

                if (planeValid) {
                    float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;

                    float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointOri.x * pointOri.x
                            + pointOri.y * pointOri.y + pointOri.z * pointOri.z));

                    coeff.x = s * pa;
                    coeff.y = s * pb;
                    coeff.z = s * pc;
                    coeff.intensity = s * pd2;

                    if (s > 0.1) {
                        laserCloudOriSurfVec[i] = pointOri;
                        coeffSelSurfVec[i] = coeff;
                        laserCloudOriSurfFlag[i] = true;
                    }
                }
            }
        }
    }

    void combineOptimizationCoeffs()
    {
        // combine corner coeffs
        for (int i = 0; i < laserCloudCornerLastDSNum; ++i){
            if (laserCloudOriCornerFlag[i] == true){
                laserCloudOri->push_back(laserCloudOriCornerVec[i]);
                coeffSel->push_back(coeffSelCornerVec[i]);
            }
        }
        // combine surf coeffs
        for (int i = 0; i < laserCloudSurfLastDSNum; ++i){
            if (laserCloudOriSurfFlag[i] == true){
                laserCloudOri->push_back(laserCloudOriSurfVec[i]);
                coeffSel->push_back(coeffSelSurfVec[i]);
            }
        }
        // reset flag for next iteration
        std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);
        std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);
    }

    bool LMOptimization(int iterCount)
    {
        // This optimization is from the original loam_velodyne by Ji Zhang, need to cope with coordinate transformation
        // lidar <- camera      ---     camera <- lidar
        // x = z                ---     x = y
        // y = x                ---     y = z
        // z = y                ---     z = x
        // roll = yaw           ---     roll = pitch
        // pitch = roll         ---     pitch = yaw
        // yaw = pitch          ---     yaw = roll

        // lidar -> camera
        float srx = sin(transformTobeMapped[1]);
        float crx = cos(transformTobeMapped[1]);
        float sry = sin(transformTobeMapped[2]);
        float cry = cos(transformTobeMapped[2]);
        float srz = sin(transformTobeMapped[0]);
        float crz = cos(transformTobeMapped[0]);

        int laserCloudSelNum = laserCloudOri->size();
        if (laserCloudSelNum < 50) {
            return false;
        }

        cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));
        cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));
        cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
        cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
        cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
        cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));

        PointType pointOri, coeff;

        for (int i = 0; i < laserCloudSelNum; i++) {
            // 把LIdar 坐标系(xyz-前左上)的数据转成跟Camera 坐标系（xyz-左 上 前）一样的数据（这么做纯粹是为了抄代码吗？）
            // lidar -> camera
            pointOri.x = laserCloudOri->points[i].y;
            pointOri.y = laserCloudOri->points[i].z;
            pointOri.z = laserCloudOri->points[i].x;
            // lidar -> camera
            coeff.x = coeffSel->points[i].y;
            coeff.y = coeffSel->points[i].z;
            coeff.z = coeffSel->points[i].x;
            coeff.intensity = coeffSel->points[i].intensity;
            // in camera
            float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x
                      + (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y
                      + (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;

            float ary = ((cry*srx*srz - crz*sry)*pointOri.x 
                      + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x
                      + ((-cry*crz - srx*sry*srz)*pointOri.x 
                      + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;

            float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x
                      + (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y
                      + ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;
            // camera -> lidar
            matA.at<float>(i, 0) = arz;
            matA.at<float>(i, 1) = arx;
            matA.at<float>(i, 2) = ary;
            matA.at<float>(i, 3) = coeff.z;
            matA.at<float>(i, 4) = coeff.x;
            matA.at<float>(i, 5) = coeff.y;
            matB.at<float>(i, 0) = -coeff.intensity;
        }

        cv::transpose(matA, matAt);
        matAtA = matAt * matA;
        matAtB = matAt * matB;
        cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);

        if (iterCount == 0) {

            cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));
            cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));
            cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));

            cv::eigen(matAtA, matE, matV);
            matV.copyTo(matV2);

            isDegenerate = false;
            float eignThre[6] = {100, 100, 100, 100, 100, 100};
            for (int i = 5; i >= 0; i--) {
                if (matE.at<float>(0, i) < eignThre[i]) {
                    for (int j = 0; j < 6; j++) {
                        matV2.at<float>(i, j) = 0;
                    }
                    isDegenerate = true;
                } else {
                    break;
                }
            }
            matP = matV.inv() * matV2;
        }

        if (isDegenerate)
        {
            cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));
            matX.copyTo(matX2);
            matX = matP * matX2;
        }

        transformTobeMapped[0] += matX.at<float>(0, 0);
        transformTobeMapped[1] += matX.at<float>(1, 0);
        transformTobeMapped[2] += matX.at<float>(2, 0);
        transformTobeMapped[3] += matX.at<float>(3, 0);
        transformTobeMapped[4] += matX.at<float>(4, 0);
        transformTobeMapped[5] += matX.at<float>(5, 0);

        float deltaR = sqrt(
                            pow(pcl::rad2deg(matX.at<float>(0, 0)), 2) +
                            pow(pcl::rad2deg(matX.at<float>(1, 0)), 2) +
                            pow(pcl::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.05 && deltaT < 0.05) {
            return true; // converged
        }
        return false; // keep optimizing
    }

后面关于因子图的代码就不说了，在imupreintegration 中有讲了，关键是这个文件太长了，乏了。

老虎福

关注

0
点赞
踩
6

收藏

觉得还不错? 一键收藏
1
评论
LIO-SAM 详读代码笔记 -- 5.MapOptimization

这个模块主要的功能就是接收featureExtraction 传来的coud_info 数据，把点云特征点根据初始化的里程计信息转换到map坐标系下，然后对当前帧的点云特征点信息进行scan-map 匹配优化，这里用到loam里面的点到线，点到面的距离作为残差项，用高斯牛顿法优化里程计信息，然后按照匀速运动模型计算一个超前一个lidar周期的odom_imcremental里程计，传送到IMU 预积分模块使用。
复制链接

扫一扫