准备工作
- 引用头文件
// 引用自定义的函数 #include "utility.h" // cloud_info自定义数据结构，save_map自定义的服务 #include "lio_sam/cloud_info.h" #include "lio_sam/save_map.h" // GTSAM相关头文件 #include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Pose3.h> #include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/BetweenFactor.h> #include <gtsam/navigation/GPSFactor.h> #include <gtsam/navigation/ImuFactor.h> #include <gtsam/navigation/CombinedImuFactor.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/Marginals.h> #include <gtsam/nonlinear/Values.h> #include <gtsam/inference/Symbol.h> #include <gtsam/nonlinear/ISAM2.h> // 使用命名空间以及缩写 using namespace gtsam; using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) using symbol_shorthand::V; // Vel (xdot,ydot,zdot) using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) using symbol_shorthand::G; // GPS pose
- 自定义数据结构，存储激光雷达位姿6D pose
/* * A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp) 自定义一个点云数据结构（6D位姿点云结构） */ struct PointXYZIRPYT { PCL_ADD_POINT4D PCL_ADD_INTENSITY; // preferred way of adding a XYZ+padding float roll; float pitch; float yaw; double time; EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned } EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment // 注册自定义的PointXYZIRPYT到PCL 配合自定义的PointXYZIRPYT结构体使用 POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT, (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) (float, roll, roll) (float, pitch, pitch) (float, yaw, yaw) (double, time, time)) // 别名 typedef PointXYZIRPYT PointTypePose;
- 定义mapOptimization类所使用的各种变量
class mapOptimization : public ParamServer { public: // gtsam因子图 gtsam::NonlinearFactorGraph gtSAMgraph; // gtsamValues()graphValues gtsam::Values initialEstimate; gtsam::Values optimizedEstimate; // 定义一个isam2优化器 gtsam::ISAM2 *isam; // 存储图中当前的估计值（包括很多因子） gtsam::Values isamCurrentEstimate; // LiDAR位姿协方差(在进行GTSAM优化过程中输出的一个值) // 若该值很大，则说明LiDAR位姿的精度不高，需要添加GPS因子修正，反之，则不添加GPS因子 Eigen::MatrixXd poseCovariance; // 发布各种消息 ros::Publisher pubLaserCloudSurround; // 发布局部关键帧map的特征点云 "lio_sam/mapping/map_global" ros::Publisher pubLaserOdometryGlobal;// 发布激光里程计，rviz中表现为坐标轴 "lio_sam/mapping/odometry" ros::Publisher pubLaserOdometryIncremental;// 发布激光里程计，它与"lio_sam/mapping/odometry"基本一样，只是roll、pitch用imu数据加权平均了一下，z做了限制 ros::Publisher pubKeyPoses; // 发布历史关键帧里程计 "lio_sam/mapping/trajectory" ros::Publisher pubPath; // 发布激光里程计路径，rviz中表现为载体的运行轨迹 "lio_sam/mapping/path" // 发布回环相关点云 ros::Publisher pubHistoryKeyFrames; // 发布闭环匹配关键帧局部map "lio_sam/mapping/icp_loop_closure_history_cloud" ros::Publisher pubIcpKeyFrames; // 发布当前关键帧经过闭环优化后的位姿变换之后的特征点云 "lio_sam/mapping/icp_loop_closure_corrected_cloud" // 发布局部地图 实时点云 ros::Publisher pubRecentKeyFrames; // 发布局部map的降采样平面点集合 "lio_sam/mapping/map_local" ros::Publisher pubRecentKeyFrame; // 发布历史帧（累加的）的角点、平面点降采样集合 "lio_sam/mapping/cloud_registered" // 发布原始点云配准处理后的点云 ros::Publisher pubCloudRegisteredRaw; // 发布当前帧原始点云配准之后的点云"lio_sam/mapping/cloud_registered_raw" // 发布回环约束边 ros::Publisher pubLoopConstraintEdge; // 发布闭环边，rviz中表现为闭环帧之间的连线 "/lio_sam/mapping/loop_closure_constraints" ros::Subscriber subCloud; // 订阅当前激光帧点云信息，来自featureExtraction "lio_sam/feature/cloud_info" // 实际使用中都没有使用 ros::Subscriber subGPS; // 订阅GPS里程计 gpsTopic默认为"odometry/gpsz" ros::Subscriber subLoop; // 订阅回环 来自外部闭环检测程序提供的闭环数据，本程序没有提供，这里实际没用上 // 保存地图数据服务 ros::ServiceServer srvSaveMap; // GPS数据队列 std::deque<nav_msgs::Odometry> gpsQueue; // 从特征点提取节点传出的cloudinfo信息 lio_sam::cloud_info cloudInfo; // 角点关键帧，平面点关键帧 （typedef pcl::PointXYZI PointType）; vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames; vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames; pcl::PointCloud<PointType>::Ptr cloudKeyPoses3D; // 存储关键帧的位置信息的点云 pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D; // 存储关键帧的6D位姿信息的点云 pcl::PointCloud<PointType>::Ptr copy_cloudKeyPoses3D; pcl::PointCloud<PointTypePose>::Ptr copy_cloudKeyPoses6D; // 用于里程计优化的角点与平面点 pcl::PointCloud<PointType>::Ptr laserCloudCornerLast; // corner feature set from odoOptimization pcl::PointCloud<PointType>::Ptr laserCloudSurfLast; // surf feature set from odoOptimization pcl::PointCloud<PointType>::Ptr laserCloudCornerLastDS; // downsampled corner featuer set from odoOptimization pcl::PointCloud<PointType>::Ptr laserCloudSurfLastDS; // downsampled surf featuer set from odoOptimization // 当前帧与局部map匹配上了的角点、平面点，加入同一集合 pcl::PointCloud<PointType>::Ptr laserCloudOri; // 后面是对应点的参数（直线方程与平面方程系数） pcl::PointCloud<PointType>::Ptr coeffSel; std::vector<PointType> laserCloudOriCornerVec; // corner point holder for parallel computation std::vector<PointType> coeffSelCornerVec; std::vector<bool> laserCloudOriCornerFlag; std::vector<PointType> laserCloudOriSurfVec; // surf point holder for parallel computation std::vector<PointType> coeffSelSurfVec; std::vector<bool> laserCloudOriSurfFlag; // map与pair的作用 // 每个pair 可以存储两个值，这两种值的类型没有限制。 // map类似于一个容器,可以存储关键值索引、pair。 // 这里使用map容器分别存储两种特征点类型 map<int, pair<pcl::PointCloud<PointType>, pcl::PointCloud<PointType>>> laserCloudMapContainer; // 局部地图的一个容器 pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap; // 局部地图中的角点集合 pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap; // 局部地图中的面片点集合 pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMapDS; // 角点局部地图的下采样后的点云 pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMapDS; // 面点局部地图的下采样后的点云 // 局部地图 建立kdtree用于找相邻点 pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap; // 角点局部地图的kdtree pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap; // 面点局部地图的kdtree // 对关键帧位置建立kdtree pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurroundingKeyPoses; pcl::KdTreeFLANN<PointType>::Ptr kdtreeHistoryKeyPoses; // 下采样 pcl::VoxelGrid<PointType> downSizeFilterCorner; pcl::VoxelGrid<PointType> downSizeFilterSurf; // 回环进行ICP匹配时使用的降采样滤波器 pcl::VoxelGrid<PointType> downSizeFilterICP; // 设置关键帧周围的相关关键帧数量不会太密（参数文件中设置为2m） // for surrounding key poses of scan-to-map optimization pcl::VoxelGrid<PointType> downSizeFilterSurroundingKeyPoses; // 存储当前cloudinfo的时间戳信息 ros::Time timeLaserInfoStamp; double timeLaserInfoCur; // 最优变换 float transformTobeMapped[6]; std::mutex mtx; std::mutex mtxLoopInfo; // LM第一次进行优化是否发生退化 // 如果发生退化，则在退化方向不更新 bool isDegenerate = false; cv::Mat matP; // 定义退化方向，若没有退化则为单位向量 int laserCloudCornerFromMapDSNum = 0; // 当前局部地图下采样后的角点数目 int laserCloudSurfFromMapDSNum = 0; // 当前局部地图下采样后的面点数目 int laserCloudCornerLastDSNum = 0; // 当前帧下采样后的角点的数目 int laserCloudSurfLastDSNum = 0; // 当前帧下采样后的面点数目 // 回环检测相关 bool aLoopIsClosed = false; // 回环索引，前一个为新的关键帧索引，后一个为旧的关键帧索引 map<int, int> loopIndexContainer; // from new to old // 回环索引队列 vector<pair<int, int>> loopIndexQueue; // 回环对的相对位姿 vector<gtsam::Pose3> loopPoseQueue; // 回环对应的噪声项 vector<gtsam::noiseModel::Diagonal::shared_ptr> loopNoiseQueue; // 存储外部回环检测模块发送的回环信息 deque<std_msgs::Float64MultiArray> loopInfoVec; // 行驶路径 nav_msgs::Path globalPath; // 当前帧位姿 Eigen::Affine3f transPointAssociateToMap; // 前一帧位姿 Eigen::Affine3f incrementalOdometryAffineFront; // 当前帧位姿 Eigen::Affine3f incrementalOdometryAffineBack;
- 构造函数
// 构造函数 mapOptimization() { // gtsam初始化 gtsam::ISAM2Params parameters; parameters.relinearizeThreshold = 0.1; parameters.relinearizeSkip = 1; isam = new ISAM2(parameters); // 优化器实例化 // 发布处理后的结果 pubKeyPoses = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/trajectory", 1); pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_global", 1); // 发布激光里程计，rviz中表现为坐标轴 pubLaserOdometryGlobal = nh.advertise<nav_msgs::Odometry> ("lio_sam/mapping/odometry", 1); // 发布激光里程计，它与上面的激光里程计基本一样，只是增量式的累加位姿变化，roll、pitch用imu数据加权平均了一下 pubLaserOdometryIncremental = nh.advertise<nav_msgs::Odometry> ("lio_sam/mapping/odometry_incremental", 1); pubPath = nh.advertise<nav_msgs::Path>("lio_sam/mapping/path", 1); // 订阅来自特征提取节点的点云信息 gpsTopic 来自外部闭环检测程序提供的闭环数据(本程序没有提供，这里实际没用上) subCloud = nh.subscribe<lio_sam::cloud_info>("lio_sam/feature/cloud_info", 1, &mapOptimization::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay()); // 并没有接收到相关消息 subGPS = nh.subscribe<nav_msgs::Odometry> (gpsTopic, 200, &mapOptimization::gpsHandler, this, ros::TransportHints().tcpNoDelay()); subLoop = nh.subscribe<std_msgs::Float64MultiArray>("lio_loop/loop_closure_detection", 1, &mapOptimization::loopInfoHandler, this, ros::TransportHints().tcpNoDelay()); // 发布一个保存地图功能的服务 srvSaveMap = nh.advertiseService("lio_sam/save_map", &mapOptimization::saveMapService, this); // 发布闭环匹配关键帧局部map pubHistoryKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/icp_loop_closure_history_cloud", 1); // 发布当前关键帧经过闭环优化后的位姿变换之后的特征点云 pubIcpKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/icp_loop_closure_corrected_cloud", 1); // 发布闭环边，rviz中表现为闭环帧之间的连线 pubLoopConstraintEdge = nh.advertise<visualization_msgs::MarkerArray>("/lio_sam/mapping/loop_closure_constraints", 1); // 发布局部map的降采样平面点集合 pubRecentKeyFrames = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/map_local", 1); // 发布历史帧（累加的）的角点、平面点降采样集合 pubRecentKeyFrame = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/cloud_registered", 1); // 发布当前帧原始点云配准之后的点云 pubCloudRegisteredRaw = nh.advertise<sensor_msgs::PointCloud2>("lio_sam/mapping/cloud_registered_raw", 1); // 体素滤波设置珊格大小 downSizeFilterCorner.setLeafSize(mappingCornerLeafSize, mappingCornerLeafSize, mappingCornerLeafSize); downSizeFilterSurf.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize); downSizeFilterICP.setLeafSize(mappingSurfLeafSize, mappingSurfLeafSize, mappingSurfLeafSize); // 设置关键帧周围的相关关键帧数量不会太密（参数文件中设置为2m） downSizeFilterSurroundingKeyPoses.setLeafSize(surroundingKeyframeDensity, surroundingKeyframeDensity, surroundingKeyframeDensity); // for surrounding key poses of scan-to-map optimization // 各种变量预先分配内存 allocateMemory(); }
- 分配内存空间
// 各种变量预先分配内存 void allocateMemory() { // 存储点云关键帧位姿 包括 3D与6d版本 cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>()); cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>()); copy_cloudKeyPoses3D.reset(new pcl::PointCloud<PointType>()); copy_cloudKeyPoses6D.reset(new pcl::PointCloud<PointTypePose>()); // 构建关键帧周边关键位姿的kdtree 历史位姿的kdtree kdtreeSurroundingKeyPoses.reset(new pcl::KdTreeFLANN<PointType>()); kdtreeHistoryKeyPoses.reset(new pcl::KdTreeFLANN<PointType>()); // 角点、平面点特征 laserCloudCornerLast.reset(new pcl::PointCloud<PointType>()); // corner feature set from odoOptimization laserCloudSurfLast.reset(new pcl::PointCloud<PointType>()); // surf feature set from odoOptimization laserCloudCornerLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled corner featuer set from odoOptimization laserCloudSurfLastDS.reset(new pcl::PointCloud<PointType>()); // downsampled surf featuer set from odoOptimization // laserCloudOri.reset(new pcl::PointCloud<PointType>()); coeffSel.reset(new pcl::PointCloud<PointType>()); // laserCloudOriCornerVec.resize(N_SCAN * Horizon_SCAN); coeffSelCornerVec.resize(N_SCAN * Horizon_SCAN); laserCloudOriCornerFlag.resize(N_SCAN * Horizon_SCAN); laserCloudOriSurfVec.resize(N_SCAN * Horizon_SCAN); coeffSelSurfVec.resize(N_SCAN * Horizon_SCAN); laserCloudOriSurfFlag.resize(N_SCAN * Horizon_SCAN); std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false); std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false); laserCloudCornerFromMap.reset(new pcl::PointCloud<PointType>()); laserCloudSurfFromMap.reset(new pcl::PointCloud<PointType>()); laserCloudCornerFromMapDS.reset(new pcl::PointCloud<PointType>()); laserCloudSurfFromMapDS.reset(new pcl::PointCloud<PointType>()); // 对局部地图的特征点构建KDtree kdtreeCornerFromMap.reset(new pcl::KdTreeFLANN<PointType>()); kdtreeSurfFromMap.reset(new pcl::KdTreeFLANN<PointType>()); // 变换数组初始化为0 for (int i = 0; i < 6; ++i){ transformTobeMapped[i] = 0; } matP = cv::Mat(6, 6, CV_32F, cv::Scalar::all(0)); }
激光点云处理
订阅原始的激光点云数据并进行处理，主要流程如下：
/** * 订阅当前激光帧点云信息，来自featureExtraction * 1、当前帧位姿初始化 * 1) 如果是第一帧，用原始imu数据的RPY初始化当前帧位姿（旋转部分） * 2) 后续帧，用imu预积分里程计计算两帧之间的增量位姿变换，作用于前一帧的激光位姿，得到当前帧激光预测位姿 * 3）如果不存在IMU预积分数据，则使用IMU磁力计的旋转部分 * 2、提取局部角点、平面点云集合，加入局部map * 1) 对最近的一帧关键帧，搜索时空维度上相邻的关键帧集合，降采样一下 * 2) 对关键帧集合中的每一帧，提取对应的角点、平面点，加入局部map中 * 3、当前激光帧角点、平面点集合降采样 * 4、scan-to-map优化当前帧位姿 * (1) 要求当前帧特征点数量足够多，且匹配的点数够多，才执行优化 * (2) 迭代30次（上限）优化 * 1) 当前激光帧角点寻找局部map匹配点 * a.更新当前帧位姿，将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线（用距离中心点的协方差矩阵，特征值进行判断），则认为匹配上了 * b.计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数 * 2) 当前激光帧平面点寻找局部map匹配点 * a.更新当前帧位姿，将当前帧平面点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成平面（最小二乘拟合平面），则认为匹配上了 * b.计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数 * 3) 提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合 * 4) 对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped * (3)用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合，更新当前帧位姿的roll、pitch，约束z坐标 * 5、设置当前帧为关键帧并执行因子图优化 * 1) 计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧 * 2) 添加激光里程计因子、GPS因子、闭环因子 * 3) 执行因子图优化 * 4) 得到当前帧优化后位姿，位姿协方差 * 5) 添加cloudKeyPoses3D，cloudKeyPoses6D，更新transformTobeMapped，添加当前关键帧的角点、平面点集合 * 6、更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹 * 7、发布激光里程计 * 8、发布里程计、点云、轨迹 */ // 接收处理来自特征提取节点的点云信息 void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn) { // extract time stamp // 提取当前时间戳（一个为ROS Time格式，另外一个为double格式） timeLaserInfoStamp = msgIn->header.stamp; timeLaserInfoCur = msgIn->header.stamp.toSec(); // extract info and feature cloud // 提取cloudinfo中的角点和面点（最新的角点与面片点） 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; // 控制后端频率，两帧处理一帧（mappingProcessInterval在参数文件中设置为0.15s） if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval) { timeLastProcessing = timeLaserInfoCur; /** * 作为基于优化方式的点云匹配，初始值是非常重要的，一个好的初始值会帮助优化问题快速收敛且避免局部最优解的情况 * @brief 当前帧位姿初始化 * 1、如果是第一帧，用原始imu数据的RPY初始化当前帧位姿（旋转部分） * 2、后续帧，用imu预积分里程计计算两帧之间的增量位姿变换，作用于前一帧的激光位姿，得到当前帧激光帧预测位姿 * 3、如果不存在IMU预积分里程计数据，则仍使用IMU磁力计中的旋转部分进行初始化预测位姿 * （得到了front的转换） */ updateInitialGuess(); /** * @brief 提取当前帧相关的关键帧并且构建点云局部地图 * 2、提取局部角点、平面点云集合，加入局部map * 1) 对最新的一帧关键帧，搜索时空维度上相邻的关键帧集合，降采样一下 * 2) 此外，对最新的一帧关键帧，添加一些时间上比较近的关键帧。以防机器人在原地转圈时，导致出现局部地图为空的情况 * 3) 对关键帧集合中的每一帧，提取对应的角点、平面点，加入局部map中 */ extractSurroundingKeyFrames(); /** * @brief 对当前帧进行下采样 * 分别对当前的扫描 角点 与 面片点 进行下采样 */ downsampleCurrentScan(); /** * @brief 对点云配准进行优化问题构建求解 * scan-to-map优化当前帧位姿 * (1) 要求当前帧特征点数量足够多，且匹配的点数够多，才执行优化 * (2) 迭代30次（上限）优化 * 1) 当前激光帧角点寻找局部map匹配点 * a.更新当前帧位姿，将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线（用距离中心点的协方差矩阵，特征值进行判断），则认为匹配上了 * b.计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数 * 2) 当前激光帧平面点寻找局部map匹配点 * a.更新当前帧位姿，将当前帧平面点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成平面（最小二乘拟合平面），则认为匹配上了 * b.计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数 * 3) 提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合 * 4) 对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped * (3)用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合，更新当前帧位姿的roll、pitch，约束z坐标 * （得到了end的转换） */ scan2MapOptimization(); /** * @brief 设置当前帧为关键帧并执行因子图优化 * 1、计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧 * 2、添加激光里程计因子、GPS因子、闭环因子 * 3、执行因子图优化 * 4、得到当前帧优化后位姿，位姿协方差 * 5、添加到cloudKeyPoses3D，cloudKeyPoses6D，更新transformTobeMapped，添加当前关键帧的角点、平面点集合 */ saveKeyFramesAndFactor(); /** * @brief 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹 * 1、判断是否添加了回环因子（或者GPS因子）进行优化 * 2、若使用了，则清空局部地图map容器中的关键帧，清除全局路径 * 3、遍历因子图中所有变量节点的位姿，更新关键帧变量中的位姿（cloudKeyPoses3D与cloudKeyPoses6D），更新全局路径 * 使用优化后的位姿更新已有的关键帧位姿 */ correctPoses(); /** * @brief 发布里程计 * 1、发布当前位姿transformTobeMapped， "lio_sam/mapping/odometry" * 2、发布增量式位姿“lio_sam/mapping/odometry_incremental”， 与上一话题基本相同 * 3、发送lidar在odom坐标系下（odometryFrame("odom")与 "lidar_link"）的tf（其值本质上也是transformTobeMapped） */ publishOdometry(); /** * @brief 发布各种信息 * 1、发布关键帧位置"lio_sam/mapping/trajectory" * 2、发布降采样后的局部地图面片点 "lio_sam/mapping/map_local" * 3、发布实时显示的点云，"lio_sam/mapping/cloud_registered" * 4、发布原始配准后的点云"lio_sam/mapping/cloud_registered_raw" * 5、发布载体的运行轨迹 "lio_sam/mapping/path" */ publishFrames(); } }
- 角点、面片点匹配
/** * @brief 角点优化，当前帧角点寻找局部map对应的角点 * 1、更新当前帧位姿，将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线（用距离中心点的协方差矩阵，特征值进行判断），则认为匹配上了 * 2、计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数 */ void cornerOptimization() { // 获取Affine类型的变换，即更新transPointAssociateToMap变量 updatePointAssociateToMap(); // 使用openmp并行加速 #pragma omp parallel for num_threads(numberOfCores) // 遍历当前帧的角点 for (int i = 0; i < laserCloudCornerLastDSNum; i++) { // 定义原始点，配对的点，系数（最多存取4个参数） PointType pointOri, pointSel, coeff; std::vector<int> pointSearchInd; std::vector<float> pointSearchSqDis; // 选择第i个点作为当前点 pointOri = laserCloudCornerLastDS->points[i]; // 将该点从当前帧通过初始的位姿转换到地图坐标系下去，使用了transPointAssociateToMap变量 // 从ori换成sel pointAssociateToMap(&pointOri, &pointSel); // 在角点地图里寻找距离当前点比较近的5个点,索引存在pointSearchInd中，对应的距离存在pointSearchSqDis变量中 kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); // 协方差矩阵 cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0)); cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0)); cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0)); // 计算找到的点中距离当前点最远的点，如果距离太大那说明这个约束不可信，就跳过 if (pointSearchSqDis[4] < 1.0) { // 五个点的坐标均值 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; // 计算五个点的协方差矩阵（3*3），仅计算矩阵的上半部分 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); // 这是线特征性，要求最大特征值大于3倍的次大特征值（即λ1 >> λ2≈λ3） if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) { // 当前点在map坐标系下的坐标 float x0 = pointSel.x; float y0 = pointSel.y; float z0 = pointSel.z; // 特征向量对应的就是直线的方向向量 // 通过点的均值沿着特征向量的方向（直线）两边拓展，构成一个线的两个端点 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); 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); // 下面是计算点到线的残差和垂线方向（及雅克比方向） // area_012，也就是三个点（x0,x1,x2）组成的三角形面积*2，叉积的模|axb|=a*b*sin(theta) 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))); // x1,x2两点之间的距离 float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2)); // 两次叉积，得到点到直线的垂线段单位向量（x分量） float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12; // (y分量) float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; // (z分量) float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; // x0点到x1x2两点连接的直线之间的距离，面积/底边长 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; // 如果残差小于1m，就认为是一个有效的约束 if (s > 0.1) { // 当前激光帧角点，加入匹配集合中（注意：加入的点坐标是ori，即未进行转换的点） laserCloudOriCornerVec[i] = pointOri; // 当前角点对应的系数 coeffSelCornerVec[i] = coeff; // 标记，代表此点有效，可以用于匹配优化 laserCloudOriCornerFlag[i] = true; } } } } } /** * @brief 当前激光帧平面点寻找局部map匹配点 * 1、更新当前帧位姿，将当前帧平面点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成平面（最小二乘拟合平面），则认为匹配上了 * 2、计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数 */ void surfOptimization() { // 获取当前Affine类型的变换 updatePointAssociateToMap(); #pragma omp parallel for num_threads(numberOfCores) for (int i = 0; i < laserCloudSurfLastDSNum; i++) { // 定义当前原始点，转换到世界坐标系下的点，对应的系数 PointType pointOri, pointSel, coeff; // 用于kdtree搜索的结果（索引与距离） std::vector<int> pointSearchInd; std::vector<float> pointSearchSqDis; // 同样找5个面点 pointOri = laserCloudSurfLastDS->points[i]; // 转换到世界坐标系 pointAssociateToMap(&pointOri, &pointSel); // 查找当前点（已经转换到世界坐标系）在局部地图中的最近的5个最近邻点 kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); // 最近邻的五个点，集合成一个5*3的矩阵 Eigen::Matrix<float, 5, 3> matA0; // 定义平面方程时使用，方程的常数项，设置为1 Eigen::Matrix<float, 5, 1> matB0; // 存储平面方程的系数 Eigen::Vector3f matX0; // 平面方程Ax + By + Cz + 1 = 0 matA0.setZero(); matB0.fill(-1); matX0.setZero(); // 同样最大距离不能超过1m 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; } // 假设平面方程为ax+by+cz+1=0，这里就是求方程的系数abc，d=1 matX0 = matA0.colPivHouseholderQr().solve(matB0); // 求出来x的就是这个平面的法向量，也就是系数abc 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); // 归一化，将法向量模长统一为1 pa /= ps; pb /= ps; pc /= ps; pd /= ps; // 判断选出的5个点所拟合平面是否有效 bool planeValid = true; for (int j = 0; j < 5; j++) { // 每个点代入平面方程，计算点到平面的距离，如果距离大于0.2m认为这个平面曲率偏大，就是无效的平面 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(pointSel.x * pointSel.x + pointSel.y * pointSel.y + pointSel.z * pointSel.z)); // 平面的系数（经过权重调整） coeff.x = s * pa; coeff.y = s * pb; coeff.z = s * pc; coeff.intensity = s * pd2; // 如果S大于阈值，则认为当前点有效 if (s > 0.1) { // 添加到待匹配的面片点集合中（注意：添加的是原始坐标） laserCloudOriSurfVec[i] = pointOri; // 添加对应的系数（平面方程） coeffSelSurfVec[i] = coeff; // 标记 当前面片点有效，可以用于优化匹配 laserCloudOriSurfFlag[i] = true; } } } } }
- 两种类型特征点融合、LM优化
/** * @brief 提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合 */ void combineOptimizationCoeffs() { // combine corner coeffs // 遍历当前帧所有的角点 for (int i = 0; i < laserCloudCornerLastDSNum; ++i){ // 只有标志位为true的时候才是有效约束 添加有效的角点与对应参数到统一集合中 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 // 标志位清零，重设为false std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false); std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false); } /** * @brief scan-to-map优化 * 1、对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程， * 2、迭代优化当前位姿，更新transformTobeMapped * 3、判断是否收敛,变化量小于阈值。 * @param iterCount * @return true * @return false */ bool LMOptimization(int iterCount) { // 原始的loam代码是将lidar坐标系转到相机坐标系，这里把原先loam中的代码拷贝了过来，但是为了坐标系的统一，就先转到相机系优化，然后结果转回lidar系 // 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 // 将lidar系转到相机系（更换坐标轴） 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; } // 创建一个 配对点云数量×6大小的矩阵 及 其转置 cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0)); cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0)); // At*A cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0)); // B cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0)); // A*B cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0)); // 优化求解的x cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0)); // 临时变量，代表原始点与对应系数 PointType pointOri, coeff; for (int i = 0; i < laserCloudSelNum; i++) { // 首先将当前点以及点到线（面）的单位向量转到相机系 // 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 // 相机系下的旋转顺序是Y - X - Z对应lidar系下Z -Y -X // 这是求导？ 构建雅克比矩阵？ 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; // lidar -> camera // 这里就是把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; } // 构造JTJ以及-JTe矩阵 cv::transpose(matA, matAt); matAtA = matAt * matA; matAtB = matAt * matB; // 求解增量 // J^T·J·delta_x = -J^T·B // (J^T·J等同与matAtA，delta_x为要求的变化量对应于matX，J为雅克比矩阵，B为残差，matAtB) cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR); // 如果是第一次迭代优化 // 检查近似Hessian矩阵（J^T·J）是否退化，或者称为奇异，行列式值=0 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)); // 对JTJ进行特征值分解，分解成E和V cv::eigen(matAtA, matE, matV); // 复制V到V2 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]) { // 退化方向对应的特征向量全部置0 for (int j = 0; j < 6; j++) { matV2.at<float>(i, j) = 0; } isDegenerate = true; } else { break; } } // 表征退化的方向 // 如果没有退化，matP应该是单位向量 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 }
选取关键帧，添加各种因子并进行优化
/************************************************************************************/ /**********************关键帧、添加各种因子、矫正轨迹并发布*******************************/ /************************************************************************************/ // 根据旋转与平移量判断当前帧是否为关键帧 bool saveFrame() { // 如果没有关键帧，那直接认为是关键帧(第一帧直接为关键帧) if (cloudKeyPoses3D->points.empty()) return true; // 取出上一个关键帧的位姿 Eigen::Affine3f transStart = pclPointToAffine3f(cloudKeyPoses6D->back()); // 当前帧的位姿转成eigen形式 Eigen::Affine3f transFinal = pcl::getTransformation(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]); // 计算两个位姿之间的delta pose Eigen::Affine3f transBetween = transStart.inverse() * transFinal; float x, y, z, roll, pitch, yaw; // 两帧之间的相对变换 转成 平移+欧拉角的形式 pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw); // 任何一个旋转大于给定阈值或者平移大于给定阈值就认为是关键帧 // 旋转大于0.2弧度，平移大于1m if (abs(roll) < surroundingkeyframeAddingAngleThreshold && abs(pitch) < surroundingkeyframeAddingAngleThreshold && abs(yaw) < surroundingkeyframeAddingAngleThreshold && sqrt(x*x + y*y + z*z) < surroundingkeyframeAddingDistThreshold) return false; return true; } /** * @brief 添加激光里程计因子 * 第一个关键帧添加的是PriorFactor * 后续添加的是BetweenFactor */ void addOdomFactor() { // 如果是第一帧关键帧 if (cloudKeyPoses3D->points.empty()) { // 置信度就设置差一点，尤其是不可观的平移和yaw角 noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-2, 1e-2, M_PI*M_PI, 1e8, 1e8, 1e8).finished()); // rad*rad, meter*meter // 增加先验约束，对第0个节点增加约束 gtSAMgraph.add(PriorFactor<Pose3>(0, trans2gtsamPose(transformTobeMapped), priorNoise)); // 加入节点信息（索引 gtsam位姿） initialEstimate.insert(0, trans2gtsamPose(transformTobeMapped)); }else{ // 如果不是第一帧，就增加帧间约束 // 这时帧间约束置信度就设置高一些 noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); // 转成gtsam的格式 gtsam::Pose3 poseFrom = pclPointTogtsamPose3(cloudKeyPoses6D->points.back()); gtsam::Pose3 poseTo = trans2gtsamPose(transformTobeMapped); // 这是一个帧间约束，分别输入两个节点的id，帧间约束大小以及置信度 gtSAMgraph.add(BetweenFactor<Pose3>(cloudKeyPoses3D->size()-1, cloudKeyPoses3D->size(), poseFrom.between(poseTo), odometryNoise)); // 加入节点信息 initialEstimate.insert(cloudKeyPoses3D->size(), poseTo); } } /** * @brief 添加GPS因子 * 找到对应时间的GPS数据 * Z坐标不使用GPS数据，仍使用LiDAR匹配的结果 * 构建一个GPSFactor，添加到GTSAM图中，作用类似于回环 */ void addGPSFactor() { // 如果没有gps信息就算了 if (gpsQueue.empty()) return; // wait for system initialized and settles down // 如果有gps但是没有关键帧信息也算了 if (cloudKeyPoses3D->points.empty()) return; else { // 第一个关键帧和最后一个关键帧相差很近则不添加GPS if (pointDistance(cloudKeyPoses3D->front(), cloudKeyPoses3D->back()) < 5.0) return; } // pose covariance small, no need to correct // gtsam反馈的当前x，y的置信度，如果置信度比较高也不需要gps来修正(阈值设置为25) if (poseCovariance(3,3) < poseCovThreshold && poseCovariance(4,4) < poseCovThreshold) return; // last gps position static PointType lastGPSPoint; // 当还有GPS数据 while (!gpsQueue.empty()) { // 把距离当前帧比较早的帧都抛弃 if (gpsQueue.front().header.stamp.toSec() < timeLaserInfoCur - 0.2) { // message too old gpsQueue.pop_front(); } // 比较晚就则等待lidar匹配计算 else if (gpsQueue.front().header.stamp.toSec() > timeLaserInfoCur + 0.2) { // message too new break; } else { // 说明这个gps时间上距离当前帧已经比较近了,那就把这个数据取出来 nav_msgs::Odometry thisGPS = gpsQueue.front(); gpsQueue.pop_front(); // GPS too noisy, skip float noise_x = thisGPS.pose.covariance[0]; float noise_y = thisGPS.pose.covariance[7]; float noise_z = thisGPS.pose.covariance[14]; // 如果gps的置信度不高，也没有必要使用了（阈值设置为2） if (noise_x > gpsCovThreshold || noise_y > gpsCovThreshold) continue; // 取出gps的位置 float gps_x = thisGPS.pose.pose.position.x; float gps_y = thisGPS.pose.pose.position.y; float gps_z = thisGPS.pose.pose.position.z; // 通常gps的z没有x y准，因此这里可以不使用z值（参数文件中默认为false） if (!useGpsElevation) { // GPS坐标Z轴信息恢复成Lidar匹配的结果，对应的噪声 gps_z = transformTobeMapped[5]; noise_z = 0.01; } // GPS not properly initialized (0,0,0) // gps的x或者y太小说明还没有初始化好 if (abs(gps_x) < 1e-6 && abs(gps_y) < 1e-6) continue; // Add GPS every a few meters // 整理当前的GPS点坐标 PointType curGPSPoint; curGPSPoint.x = gps_x; curGPSPoint.y = gps_y; curGPSPoint.z = gps_z; // 加入gps观测不宜太频繁，相邻不小于5m if (pointDistance(curGPSPoint, lastGPSPoint) < 5.0) continue; else lastGPSPoint = curGPSPoint; // 添加GPS因子 gtsam::Vector Vector3(3); // gps的置信度，标准差设置成最小1m，也就是不会特别信任gps信号 Vector3 << max(noise_x, 1.0f), max(noise_y, 1.0f), max(noise_z, 1.0f); noiseModel::Diagonal::shared_ptr gps_noise = noiseModel::Diagonal::Variances(Vector3); // 调用gtsam中集成的gps的约束 gtsam::GPSFactor gps_factor(cloudKeyPoses3D->size(), gtsam::Point3(gps_x, gps_y, gps_z), gps_noise); gtSAMgraph.add(gps_factor); // 加入gps之后等同于回环，需要触发较多的isam update aLoopIsClosed = true; break; } } } /** * @brief 添加一个回环因子 * 1、遍历回环队列中的每个数据 * 2、添加相应的 回环中两帧索引 帧间位姿 噪声 */ void addLoopFactor() { // 有一个专门的回环检测线程会检测回环，检测到就会给这个队列塞入回环结果 if (loopIndexQueue.empty()) return; // 把队列里所有的回环约束都添加进来 for (int i = 0; i < (int)loopIndexQueue.size(); ++i) { int indexFrom = loopIndexQueue[i].first; int indexTo = loopIndexQueue[i].second; // 队列中第i回环的帧间约束 gtsam::Pose3 poseBetween = loopPoseQueue[i]; // 使用icp的得分作为回环的置信度 gtsam::noiseModel::Diagonal::shared_ptr noiseBetween = loopNoiseQueue[i]; // 加入回环约束 gtSAMgraph.add(BetweenFactor<Pose3>(indexFrom, indexTo, poseBetween, noiseBetween)); } // 清空回环相关队列 loopIndexQueue.clear(); loopPoseQueue.clear(); loopNoiseQueue.clear(); // 标志位置true（是否添加了一个回环） aLoopIsClosed = true; } /** * @brief 设置当前帧为关键帧并执行因子图优化 * 1、计算当前帧与前一帧位姿变换，如果变化太小，不设为关键帧，反之设为关键帧 * 2、添加激光里程计因子、GPS因子、闭环因子 * 3、执行因子图优化 * 4、得到当前帧优化后位姿，位姿协方差 * 5、添加cloudKeyPoses3D，cloudKeyPoses6D，更新transformTobeMapped，添加当前关键帧的角点、平面点集合 */ void saveKeyFramesAndFactor() { // 通过旋转和平移的增量来判断是否是关键帧 // 如果saveFrame函数返回false，则认为不是关键帧 // 直接返回 if (saveFrame() == false) return; // 如果是关键帧就给isam增加因子 // odom factor // 增加odom的因子 addOdomFactor(); // gps的因子 // gps factor addGPSFactor(); // 回环的因子 // loop factor addLoopFactor(); // cout << "****************************************************" << endl; // gtSAMgraph.print("GTSAM Graph:\n"); // 所有因子加完了，就调用isam接口更新图模型 // update iSAM // initialEstimate为位姿的初始估计，在添加odomfactor过程中同时添加 isam->update(gtSAMgraph, initialEstimate); isam->update(); // 如果加入了gps的约束或者回环约束，isam需要进行更多次的优化 if (aLoopIsClosed == true) { // 为什么需要多次？ isam->update(); isam->update(); isam->update(); isam->update(); isam->update(); } // 将约束和节点信息清空，他们已经被加入到isam中去了，因此这里清空不会影响整个优化 gtSAMgraph.resize(0); initialEstimate.clear(); // 下面保存关键帧信息 //save key poses PointType thisPose3D; // PointXYZI xyz存储平移量，I存储索引 PointTypePose thisPose6D; //PointXYZIRPYT 多个属性，用来存储平移、旋转 Pose3 latestEstimate; // 计算当前的估计值 isamCurrentEstimate = isam->calculateEstimate(); // 取出优化后的最新关键帧位姿 latestEstimate = isamCurrentEstimate.at<Pose3>(isamCurrentEstimate.size()-1); // cout << "****************************************************" << endl; // isamCurrentEstimate.print("Current estimate: "); // 平移信息取出来保存进cloudKeyPoses3D这个结构中，其中索引作为intensity值 thisPose3D.x = latestEstimate.translation().x(); thisPose3D.y = latestEstimate.translation().y(); thisPose3D.z = latestEstimate.translation().z(); thisPose3D.intensity = cloudKeyPoses3D->size(); // this can be used as index cloudKeyPoses3D->push_back(thisPose3D); // 6D姿态同样保留下来 thisPose6D.x = thisPose3D.x; thisPose6D.y = thisPose3D.y; thisPose6D.z = thisPose3D.z; thisPose6D.intensity = thisPose3D.intensity ; // 同thisPose3D中的索引 thisPose6D.roll = latestEstimate.rotation().roll(); thisPose6D.pitch = latestEstimate.rotation().pitch(); thisPose6D.yaw = latestEstimate.rotation().yaw(); thisPose6D.time = timeLaserInfoCur; // 当前关键帧对应的时间戳 cloudKeyPoses6D->push_back(thisPose6D); // cout << "****************************************************" << endl; // cout << "Pose covariance:" << endl; // cout << isam->marginalCovariance(isamCurrentEstimate.size()-1) << endl << endl; // 保存当前LiDAR位姿优化后的置信度 poseCovariance = isam->marginalCovariance(isamCurrentEstimate.size()-1); // save updated transform // 将优化后的位姿更新到transformTobeMapped数组中，作为当前最佳估计值 transformTobeMapped[0] = latestEstimate.rotation().roll(); transformTobeMapped[1] = latestEstimate.rotation().pitch(); transformTobeMapped[2] = latestEstimate.rotation().yaw(); transformTobeMapped[3] = latestEstimate.translation().x(); transformTobeMapped[4] = latestEstimate.translation().y(); transformTobeMapped[5] = latestEstimate.translation().z(); // save all the received edge and surf points pcl::PointCloud<PointType>::Ptr thisCornerKeyFrame(new pcl::PointCloud<PointType>()); pcl::PointCloud<PointType>::Ptr thisSurfKeyFrame(new pcl::PointCloud<PointType>()); // 当前帧的点云的角点和面点分别拷贝一下 pcl::copyPointCloud(*laserCloudCornerLastDS, *thisCornerKeyFrame); pcl::copyPointCloud(*laserCloudSurfLastDS, *thisSurfKeyFrame); // save key frame cloud // 关键帧的点云（角点 面片点）保存下来 cornerCloudKeyFrames.push_back(thisCornerKeyFrame); surfCloudKeyFrames.push_back(thisSurfKeyFrame); // save path for visualization // 根据当前最新位姿更新rviz可视化 updatePath(thisPose6D); } /** * @brief 更新因子图中所有变量节点的位姿，也就是所有历史关键帧的位姿，更新里程计轨迹 * 1、判断是否添加了回环因子（或者GPS因子）进行优化 * 2、若使用了，则清空局部地图map容器中的关键帧，清除全局路径 * 3、遍历因子图中所有变量节点的位姿，更新关键帧变量中的位姿（cloudKeyPoses3D与cloudKeyPoses6D），更新全局路径 * 使用优化后的位姿更新已有的关键帧位姿 */ void correctPoses() { // 没有关键帧，自然也没有什么意义 if (cloudKeyPoses3D->points.empty()) return; // 只有回环以及gps信息这些会触发全局调整信息才会触发 if (aLoopIsClosed == true) { // clear map cache // 很多位姿会变化，因子之前的容器内转到世界坐标系下的很多点云就需要调整，因此这里索性清空 laserCloudMapContainer.clear(); // 清空点云地图容器中的点云 // clear path // 清空path globalPath.poses.clear(); // update key poses // 然后更新所有的位姿 int numPoses = isamCurrentEstimate.size(); // 当前所有位姿的数量 for (int i = 0; i < numPoses; ++i) { // 更新所有关键帧的位姿（3D格式的关键帧位姿） cloudKeyPoses3D->points[i].x = isamCurrentEstimate.at<Pose3>(i).translation().x(); cloudKeyPoses3D->points[i].y = isamCurrentEstimate.at<Pose3>(i).translation().y(); cloudKeyPoses3D->points[i].z = isamCurrentEstimate.at<Pose3>(i).translation().z(); // 更新（6D格式的关键帧位姿） cloudKeyPoses6D->points[i].x = cloudKeyPoses3D->points[i].x; cloudKeyPoses6D->points[i].y = cloudKeyPoses3D->points[i].y; cloudKeyPoses6D->points[i].z = cloudKeyPoses3D->points[i].z; cloudKeyPoses6D->points[i].roll = isamCurrentEstimate.at<Pose3>(i).rotation().roll(); cloudKeyPoses6D->points[i].pitch = isamCurrentEstimate.at<Pose3>(i).rotation().pitch(); cloudKeyPoses6D->points[i].yaw = isamCurrentEstimate.at<Pose3>(i).rotation().yaw(); // 同时更新path updatePath(cloudKeyPoses6D->points[i]); } // 标志位复位 aLoopIsClosed = false; } } /** * @brief 更新路径 * 把矫正后的位姿添加到globalPath中 * @param pose_in */ void updatePath(const PointTypePose& pose_in) { // 定义一个位姿 geometry_msgs::PoseStamped pose_stamped; pose_stamped.header.stamp = ros::Time().fromSec(pose_in.time); pose_stamped.header.frame_id = odometryFrame; pose_stamped.pose.position.x = pose_in.x; pose_stamped.pose.position.y = pose_in.y; pose_stamped.pose.position.z = pose_in.z; tf::Quaternion q = tf::createQuaternionFromRPY(pose_in.roll, pose_in.pitch, pose_in.yaw); pose_stamped.pose.orientation.x = q.x(); pose_stamped.pose.orientation.y = q.y(); pose_stamped.pose.orientation.z = q.z(); pose_stamped.pose.orientation.w = q.w(); // 添加到路径中 globalPath.poses.push_back(pose_stamped); } /** * @brief 发布里程计 * 1、发布当前位姿transformTobeMapped， "lio_sam/mapping/odometry" * 2、发布增量式位姿“lio_sam/mapping/odometry_incremental”， 与上一话题基本相同，是一个累计相对变换，不断增加的里程计（平滑，不会突变） * 3、发送lidar在odom坐标系下（odometryFrame("odom")与 "lidar_link"）的tf（其值本质上也是transformTobeMapped） */ void publishOdometry() { // Publish odometry for ROS (global) // 发送当前帧的位姿 nav_msgs::Odometry laserOdometryROS; laserOdometryROS.header.stamp = timeLaserInfoStamp; laserOdometryROS.header.frame_id = odometryFrame; laserOdometryROS.child_frame_id = "odom_mapping"; laserOdometryROS.pose.pose.position.x = transformTobeMapped[3]; laserOdometryROS.pose.pose.position.y = transformTobeMapped[4]; laserOdometryROS.pose.pose.position.z = transformTobeMapped[5]; laserOdometryROS.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]); // 发布当前位姿"lio_sam/mapping/odometry" pubLaserOdometryGlobal.publish(laserOdometryROS); // Publish TF // 发送lidar在odom坐标系下的tf（其值本质上也是transformTobeMapped） static tf::TransformBroadcaster br; tf::Transform t_odom_to_lidar = tf::Transform(tf::createQuaternionFromRPY(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]), tf::Vector3(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5])); tf::StampedTransform trans_odom_to_lidar = tf::StampedTransform(t_odom_to_lidar, timeLaserInfoStamp, odometryFrame, "lidar_link"); br.sendTransform(trans_odom_to_lidar); // Publish odometry for ROS (incremental) // 发送增量位姿变换 // 这里主要用于给imu预积分模块使用，需要里程计是平滑的 static bool lastIncreOdomPubFlag = false; // 定义一个里程计变量（存储增量式的值） // 这个增量式的里程计不会由于优化而突然改变值，不会优化之后进行纠正，只是不断累计增量 static nav_msgs::Odometry laserOdomIncremental; // incremental odometry msg static Eigen::Affine3f increOdomAffine; // incremental odometry in affine // 该标志位处理一次后始终为true if (lastIncreOdomPubFlag == false) { lastIncreOdomPubFlag = true; // 记录当前位姿（第一帧直接在这里转换） laserOdomIncremental = laserOdometryROS; increOdomAffine = trans2Affine3f(transformTobeMapped); } else { // 上一帧的最佳位姿和当前帧最佳位姿（scan matching之后，而不是根据回环或者gps调整之后的位姿）之间的位姿增量 Eigen::Affine3f affineIncre = incrementalOdometryAffineFront.inverse() * incrementalOdometryAffineBack; // 位姿增量叠加到上一帧位姿上（增量累计的里程计） increOdomAffine = increOdomAffine * affineIncre; float x, y, z, roll, pitch, yaw; // 分解成欧拉角+平移向量 pcl::getTranslationAndEulerAngles (increOdomAffine, x, y, z, roll, pitch, yaw); // 如果有imu数据，同样对roll和pitch做插值 if (cloudInfo.imuAvailable == true) { if (std::abs(cloudInfo.imuPitchInit) < 1.4) { double imuWeight = 0.1; tf::Quaternion imuQuaternion; tf::Quaternion transformQuaternion; double rollMid, pitchMid, yawMid; // slerp roll // 对roll进行插值（实质上IMU发挥的作用很小） transformQuaternion.setRPY(roll, 0, 0); imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0); tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid); roll = rollMid; // slerp pitch // 对pitch进行插值（实质上IMU发挥的作用很小） transformQuaternion.setRPY(0, pitch, 0); imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0); tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid); pitch = pitchMid; } } // 增量式的lidar里程计 laserOdomIncremental.header.stamp = timeLaserInfoStamp; laserOdomIncremental.header.frame_id = odometryFrame; laserOdomIncremental.child_frame_id = "odom_mapping"; laserOdomIncremental.pose.pose.position.x = x; laserOdomIncremental.pose.pose.position.y = y; laserOdomIncremental.pose.pose.position.z = z; laserOdomIncremental.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw); // 当前帧与map进行匹配，LM优化过程中，第一次迭代矩阵分解判断是否发生了退化 // 协方差这一属性作为是否退化的标志位 if (isDegenerate) laserOdomIncremental.pose.covariance[0] = 1; else laserOdomIncremental.pose.covariance[0] = 0; } // 发布增量式的里程计 pubLaserOdometryIncremental.publish(laserOdomIncremental); } /** * @brief 发布各种信息 * 1、发布关键帧位置"lio_sam/mapping/trajectory" * 2、发布降采样后的局部地图面片点 "lio_sam/mapping/map_local" * 3、发布实时显示的点云，"lio_sam/mapping/cloud_registered" * 4、发布原始配准后的点云"lio_sam/mapping/cloud_registered_raw" * 5、发布载体的运行轨迹 "lio_sam/mapping/path" */ void publishFrames() { if (cloudKeyPoses3D->points.empty()) return; // publish key poses // 发送关键帧位置（关键帧位置已经被存储为pointtype类型，所以发布成pointcloud2类型的点云） publishCloud(&pubKeyPoses, cloudKeyPoses3D, timeLaserInfoStamp, odometryFrame); // Publish surrounding key frames // 发送周围局部地图点云信息（odometryFrame的值为“odom”） publishCloud(&pubRecentKeyFrames, laserCloudSurfFromMapDS, timeLaserInfoStamp, odometryFrame); // publish registered key frame // 发布配准后的关键帧，也就是当前点云（转换到世界坐标系下） // 话题名称 "lio_sam/mapping/cloud_registered" if (pubRecentKeyFrame.getNumSubscribers() != 0) { pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>()); PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped); // 把当前点云转换到世界坐标系下去（分别变换角点与面片点） *cloudOut += *transformPointCloud(laserCloudCornerLastDS, &thisPose6D); *cloudOut += *transformPointCloud(laserCloudSurfLastDS, &thisPose6D); // 发送当前点云 publishCloud(&pubRecentKeyFrame, cloudOut, timeLaserInfoStamp, odometryFrame); } // publish registered high-res raw cloud // 发布高分辨率的配准后点云（cloud_deskewed是在点云去畸变处理节点中提取的有效点云） // 变换到世界坐标系并发布 // 话题名称 "lio_sam/mapping/cloud_registered_raw" if (pubCloudRegisteredRaw.getNumSubscribers() != 0) { pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>()); pcl::fromROSMsg(cloudInfo.cloud_deskewed, *cloudOut); PointTypePose thisPose6D = trans2PointTypePose(transformTobeMapped); *cloudOut = *transformPointCloud(cloudOut, &thisPose6D); // 发送原始点云 publishCloud(&pubCloudRegisteredRaw, cloudOut, timeLaserInfoStamp, odometryFrame); } // publish path // 发送path if (pubPath.getNumSubscribers() != 0) { globalPath.header.stamp = timeLaserInfoStamp; globalPath.header.frame_id = odometryFrame; pubPath.publish(globalPath); } } /************************************************************************************/ /**********************关键帧、添加各种因子、矫正轨迹并发布*******************************/ /************************************************************************************/
回环检测
详细参考：LIO-SAM回环检测模块代码解析
Main函数
int main(int argc, char** argv) { ros::init(argc, argv, "lio_sam"); mapOptimization MO; ROS_INFO("\033[1;32m----> Map Optimization Started.\033[0m"); // 分别设置两个线程，执行回环检测与可视化 std::thread loopthread(&mapOptimization::loopClosureThread, &MO); std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO); ros::spin(); // 加入两个线程 loopthread.join(); visualizeMapThread.join(); return 0; }
