简述
mapOptmization.cpp 作为LeGO-LOAM的全局姿态优化部分,主要采用了两步优化方法——a. scan-to-model的地图优化;b. 因子图优化,分别由 mapOptimization::scan2MapOptimization() 和 mapOptimization::saveKeyFramesAndFactor() 两个函数承担。
下图图示了每次优化后,位姿变量的变化:
float transformLast[6]; // 上一关键帧 经过因子图优化后的位姿,用于添加因子图节点。
float transformSum[6]; // 里程计的六自由度位姿(p y r x y z)
float transformIncre[6]; // 两次位姿优化时,里程计的相对位姿变化,
float transformTobeMapped[6]; // 机器人的位姿,全局姿态优化的目标变量:位姿优化的初始值->在地图优化过程中得到更新->经过因子图优化后的位姿
float transformBefMapped[6]; // 未经过scan-to-model优化的位姿,即里程计位姿。
float transformAftMapped[6]; // 由经过scan-to-model优化后的位姿赋值,并在因子图优化后进行修正。
变量注释
class mapOptimization{
private:
// 因子图优化相关变量
NonlinearFactorGraph gtSAMgraph;
Values initialEstimate;
Values optimizedEstimate;
ISAM2 *isam;
Values isamCurrentEstimate;
noiseModel::Diagonal::shared_ptr priorNoise;
noiseModel::Diagonal::shared_ptr odometryNoise;
noiseModel::Diagonal::shared_ptr constraintNoise;
// ROS句柄
ros::NodeHandle nh;
// 多个发布者
ros::Publisher pubLaserCloudSurround;
ros::Publisher pubOdomAftMapped;
ros::Publisher pubKeyPoses;
ros::Publisher pubHistoryKeyFrames;
ros::Publisher pubIcpKeyFrames;
ros::Publisher pubRecentKeyFrames;
ros::Publisher pubRegisteredCloud;
// 五个订阅者
ros::Subscriber subLaserCloudCornerLast;
ros::Subscriber subLaserCloudSurfLast;
ros::Subscriber subOutlierCloudLast;
ros::Subscriber subLaserOdometry;
ros::Subscriber subImu;
// 为了信息发布而定义的msg
nav_msgs::Odometry odomAftMapped;
tf::StampedTransform aftMappedTrans;
tf::TransformBroadcaster tfBroadcaster;
vector<pcl::PointCloud<PointType>::Ptr> cornerCloudKeyFrames; // 关键帧的角点点云集合
vector<pcl::PointCloud<PointType>::Ptr> surfCloudKeyFrames; // 关键帧的平面点点云集合
vector<pcl::PointCloud<PointType>::Ptr> outlierCloudKeyFrames; // 关键帧的离群点点云集合
deque<pcl::PointCloud<PointType>::Ptr> recentCornerCloudKeyFrames; //
deque<pcl::PointCloud<PointType>::Ptr> recentSurfCloudKeyFrames; //
deque<pcl::PointCloud<PointType>::Ptr> recentOutlierCloudKeyFrames; //
int latestFrameID;
vector<int> surroundingExistingKeyPosesID; // 局部关键帧集合(ID集合),(随着最新关键帧的改变而逐步增减的)
deque<pcl::PointCloud<PointType>::Ptr> surroundingCornerCloudKeyFrames; //局部关键帧角点点云集合
deque<pcl::PointCloud<PointType>::Ptr> surroundingSurfCloudKeyFrames; // 局部关键帧平面点点云集合
deque<pcl::PointCloud<PointType>::Ptr> surroundingOutlierCloudKeyFrames; // 局部关键帧离群点点云集合
PointType previousRobotPosPoint; // 上一帧关键帧位姿点
PointType currentRobotPosPoint; // 使用经过 scan-to-model 优化的位姿创建当前位置点
pcl::PointCloud<PointType>::Ptr cloudKeyPoses3D; // 所有关键帧三自由度位置集合 (x,y,z表示位置,i表示位姿点在集合中的索引)(这是经过因子图优化后的位置)
pcl::PointCloud<PointTypePose>::Ptr cloudKeyPoses6D; //关键帧六自由度位姿集合 (x,y,z表示位置,i表示索引,rpy表示姿态,time记录里程计时间)(这是经过因子图优化后的位置)
pcl::PointCloud<PointType>::Ptr surroundingKeyPoses; // 局部关键帧位置集合——目标点附近的位置点集合。
pcl::PointCloud<PointType>::Ptr surroundingKeyPosesDS; // 上者的降采样结果
pcl::PointCloud<PointType>::Ptr laserCloudCornerLast; // 通过Msg接收到的Less角点点云
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast; // 通过Msg接收到的Less平面点点云
pcl::PointCloud<PointType>::Ptr laserCloudCornerLastDS; // Less角点点云的降采样
pcl::PointCloud<PointType>::Ptr laserCloudSurfLastDS; // Less平面点点云的降采样
pcl::PointCloud<PointType>::Ptr laserCloudOutlierLast; // 通过Msg接收到的离群点点云
pcl::PointCloud<PointType>::Ptr laserCloudOutlierLastDS; // 离群点点云的降采样
pcl::PointCloud<PointType>::Ptr laserCloudSurfTotalLast; // Less平面点和离群点的点云
pcl::PointCloud<PointType>::Ptr laserCloudSurfTotalLastDS; // 上者的降采样
pcl::PointCloud<PointType>::Ptr laserCloudOri;
pcl::PointCloud<PointType>::Ptr coeffSel;
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap; // 局部角点点云地图 (每轮进行清空)
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap; // 局部平面点点云地图 (每轮进行清空)
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMapDS; // 局部角点点云的降采样 (每轮进行清空)
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMapDS; // 局部角点点云的降采样 (每轮进行清空)
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap; // 局部角点点云地图 的KD树
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap; // 局部平面点点云地图 的KD树
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurroundingKeyPoses; //所有关键帧位置点集合的KD树
pcl::KdTreeFLANN<PointType>::Ptr kdtreeHistoryKeyPoses; //
pcl::PointCloud<PointType>::Ptr nearHistoryCornerKeyFrameCloud;
pcl::PointCloud<PointType>::Ptr nearHistoryCornerKeyFrameCloudDS;
pcl::PointCloud<PointType>::Ptr nearHistorySurfKeyFrameCloud;
pcl::PointCloud<PointType>::Ptr nearHistorySurfKeyFrameCloudDS;
pcl::PointCloud<PointType>::Ptr latestCornerKeyFrameCloud;
pcl::PointCloud<PointType>::Ptr latestSurfKeyFrameCloud;
pcl::PointCloud<PointType>::Ptr latestSurfKeyFrameCloudDS;
pcl::KdTreeFLANN<PointType>::Ptr kdtreeGlobalMap;
pcl::PointCloud<PointType>::Ptr globalMapKeyPoses;
pcl::PointCloud<PointType>::Ptr globalMapKeyPosesDS;
pcl::PointCloud<PointType>::Ptr globalMapKeyFrames;
pcl::PointCloud<PointType>::Ptr globalMapKeyFramesDS;
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
pcl::VoxelGrid<PointType> downSizeFilterCorner;
pcl::VoxelGrid<PointType> downSizeFilterSurf;
pcl::VoxelGrid<PointType> downSizeFilterOutlier;
pcl::VoxelGrid<PointType> downSizeFilterHistoryKeyFrames;
pcl::VoxelGrid<PointType> downSizeFilterSurroundingKeyPoses;
pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyPoses;
pcl::VoxelGrid<PointType> downSizeFilterGlobalMapKeyFrames;
// 信息的时间戳
double timeLaserCloudCornerLast; // Less角点的时间
double timeLaserCloudSurfLast; // Less平面点的时间
double timeLaserOdometry; // 里程计时间
double timeLaserCloudOutlierLast; // 离群点点云 的时间
double timeLastGloalMapPublish;
// 信息接收的标志位
bool newLaserCloudCornerLast; //Less角点的标志位
bool newLaserCloudSurfLast; //Less平面点的标志位
bool newLaserOdometry; //里程计信息的标志位
bool newLaserCloudOutlierLast; //离群点点云的标志位
float transformLast[6]; // 上一关键帧 经过因子图优化后的位姿
float transformSum[6]; // 里程计的六自由度位姿(p y r x y z)
float transformIncre[6]; // 两次位姿优化时,里程计的相对位姿变化
float transformTobeMapped[6]; // 机器人的位姿,全局姿态优化的目标变量:位姿优化的初始值->在地图优化过程中得到更新->经过因子图优化后的位姿
float transformBefMapped[6]; // 未经过scan-to-model优化的位姿,即里程计位姿。
float transformAftMapped[6]; // 由经过scan-to-model优化后的位姿赋值,并在因子图优化后进行修正。
int imuPointerFront; //与里程计时间对准的IMU数据索引
int imuPointerLast; //最新的IMU数据索引
// IMU时间、数据队列
double imuTime[imuQueLength];
float imuRoll[imuQueLength];
float imuPitch[imuQueLength];
std::mutex mtx;
double timeLastProcessing; // 上一次进行全局位姿优化的时间
PointType pointOri, pointSel, pointProj, coeff;
cv::Mat matA0; //存放距离平面点最近的五个点(5×3矩阵)
cv::Mat matB0; //5*1 的矩阵
cv::Mat matX0; //3*1 的矩阵
cv::Mat matA1; //3×3 的协方差矩阵
cv::Mat matD1; //1*3 的特征值向量
cv::Mat matV1;// 3个 特征向量
bool isDegenerate;
cv::Mat matP;
// 一些点云的数量
int laserCloudCornerFromMapDSNum; // 局部角点点云中点的数量(经过降采样)
int laserCloudSurfFromMapDSNum; // 局部平面点点云中点的数量(经过降采样)
int laserCloudCornerLastDSNum;
int laserCloudSurfLastDSNum;
int laserCloudOutlierLastDSNum;
int laserCloudSurfTotalLastDSNum;
bool potentialLoopFlag;
double timeSaveFirstCurrentScanForLoopClosure;
int closestHistoryFrameID;
int latestFrameIDLoopCloure;
bool aLoopIsClosed;
float cRoll, sRoll, cPitch, sPitch, cYaw, sYaw, tX, tY, tZ; // 点云相对于世界坐标系的关系
float ctRoll, stRoll, ctPitch, stPitch, ctYaw, stYaw, tInX, tInY, tInZ;//用来记录变换的三角函数值
}
msg的订阅
class mapOptimization{
private:
ros::Subscriber subLaserCloudCornerLast;
ros::Subscriber subLaserCloudSurfLast;
ros::Subscriber subOutlierCloudLast;
ros::Subscriber subLaserOdometry;
ros::Subscriber subImu;
...
public:
//构造函数
mapOptimization():nh("~"){
//五个subscribe的定义
subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2, &mapOptimization::laserCloudCornerLastHandler, this);
subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2, &mapOptimization::laserCloudSurfLastHandler, this);
subOutlierCloudLast = nh.subscribe<sensor_msgs::PointCloud2>("/outlier_cloud_last", 2, &mapOptimization::laserCloudOutlierLastHandler, this);
subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("/laser_odom_to_init", 5, &mapOptimization::laserOdometryHandler, this);
subImu = nh.subscribe<sensor_msgs::Imu> (imuTopic, 50, &mapOptimization::imuHandler, this);
...
}
// 三个点云主题("/laser_cloud_corner_last","/laser_cloud_surf_last","/outlier_cloud_last")的回调函数,
// 分别接收当前帧的 Less角点点云、Less平面点点云、离群点点云
// 并记录时间
// timeLaserCloudCornerLast/timeLaserCloudSurfLast/timeLaserCloudOutlierLast
// 记录点云
// laserCloudCornerLast/laserCloudSurfLast/laserCloudOutlierLast
// 标志位置true
// newLaserCloudCornerLast/newLaserCloudSurfLast/newLaserCloudOutlierLast
void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& msg){
...
}
void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& msg){
...
}
void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& msg){
...
}
//主题 "/laser_odom_to_init" 的回调函数,接收里程计信息,记录:
// timeLaserOdometry 里程计时间
// transformSum[] 记录六自由度位姿,即里程计位姿
// newLaserOdometry 标志位置1
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry){
...
}
// 主题 imuTopic 的回调函数,接收IMU数据信息,并记录在IMU数据队列中:
// imuTime 时间队列
// imuRoll、imuPitch RPY队列(另外一个姿态值没有用到,所以就只记录了其中2个值)
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn){
...
}
main函数 与 mapOptimization.run()
//主函数,
int main(int argc, char** argv)
{
ros::init(argc, argv, "lego_loam");
ROS_INFO("\033[1;32m---->\033[0m Map Optimization Started.");
mapOptimization MO;
//闭环线程(暂时不做考虑)
std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
//可视化线程(暂不考虑)
std::thread visualizeMapThread(&mapOptimization::visualizeGlobalMapThread, &MO);
ros::Rate rate(200);
while (ros::ok()){
ros::spinOnce();
//全局姿态优化的主要处理部分
MO.run();
rate.sleep();
}
loopthread.join();
visualizeMapThread.join();
return 0;
}
class mapOptimization{
private:
...
public:
...
void run(){
// 三种点云、以及里程计信息正常接收、且时间戳一致
if (newLaserCloudCornerLast && std::abs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 &&
newLaserCloudSurfLast && std::abs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 &&
newLaserCloudOutlierLast && std::abs(timeLaserCloudOutlierLast - timeLaserOdometry) < 0.005 &&
newLaserOdometry)
{
//标志位重新置回false
newLaserCloudCornerLast = false; newLaserCloudSurfLast = false; newLaserCloudOutlierLast = false; newLaserOdometry = false;
// 创建对象 lock 对mtx进行上锁,当lock被析构时,自动解锁
std::lock_guard<std::mutex> lock(mtx);
// 全局姿态优化的时间判断条件:距离上次优化已经过去0.3s
if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {
timeLastProcessing = timeLaserOdometry;
transformAssociateToMap(); //
// 根据当前和上一次全局姿态优化时的里程计 transformSum transformBefMapped
// 以及上一次全局姿态优化的结果 transformAftMapped
// 计算当前姿态优化的初始值,赋值给 transformTobeMapped
extractSurroundingKeyFrames();
// 函数功能:根据当前位置,提取局部关键帧集合,以及对应的三个关键帧点云集合
// 步骤:
// 1. 在 关键帧位置集合cloudKeyPoses3D 中
// 检索 当前位置currentRobotPosPoint 附近的姿态点
// 获得局部位置点,赋值给 局部位置点集合surroundingKeyPoses
// 2. 根据 局部位置点集合surroundingKeyPoses 更新
// 局部关键帧集合 surroundingExistingKeyPosesID
// 局部关键帧 角点点云集合surroundingCornerCloudKeyFrames
// 局部关键帧 平面点点云集合surroundingSurfCloudKeyFrames
// 局部关键帧 离群点点云集合surroundingOutlierCloudKeyFrames
// 增加新进入局部的关键帧、并删除离开局部的关键帧。
// 3. 为局部点云地图赋值
// laserCloudCornerFromMap 所有局部关键帧的角点集合
// laserCloudSurfFromMap 所有局部关键帧平面点和离群点的几何
downsampleCurrentScan();
// 对来自msg的点云进行降采样
// laserCloudCornerLast //角点点云
// laserCloudSurfLast // 平面点点云
// laserCloudOutlierLast // 离群点点云
// laserCloudSurfTotalLast // 平面点+离群点
//使用 scan-to-model 的策略优化了 变换 transformTobeMapped
scan2MapOptimization();
// 使用scan-to-model位姿优化,获得当前时间点机器人的位姿 transformTobeMapped
// 参考IMU的姿态 对 transformTobeMapped 进行中值滤波,获得最终的机器人位姿
// 为 transformBefMapped 赋值为 里程计位姿,即scan-to-model优化前的位姿
// 为 transformAftMapped 赋值为 transformTobeMapped 即scan-to-model优化后的位姿
saveKeyFramesAndFactor();
// 1选定关键帧
// 2根据新的关键帧更新因子图
// 3经过因子图优化后,更新当前位姿
// transformAftMapped
// transformTobeMapped
// 并用因子图优化后的位姿 创建关键帧位置点和位姿点,添加到集合
// cloudKeyPoses3D 关键帧位置点集合
// cloudKeyPoses6D 关键帧位姿点集合
// 并为 transformLast 赋值
// 更新
// cornerCloudKeyFrames 关键帧角点点云集合
// surfCloudKeyFrames 关键帧平面点点云集合
// outlierCloudKeyFrames关键帧离群点点云集合
//闭环处理矫正、暂不考虑
correctPoses();
publishTF();
// 主题"/aft_mapped_to_init"发布优化后的位姿信息
// msg.header.stamp 取的是 里程计时间信息
// msg.pose 存储本坐标系位姿"/camera_init" 经过全局位姿优化的姿态
// msg.twist 存储子坐标系位姿"/aft_mapped" 未经过全局优化的姿态,即里程计信息
// 发布tf变换
// msg.frame_id_ = "/camera_init"
// msg.child_frame_id_ = "/aft_mapped";
// msg 的姿态变换为 transformAftMapped 即优化后的机器人位姿距离原点的位姿变换
publishKeyPosesAndFrames();
// 主题 "/key_pose_origin" 发布关键帧位姿点集合
// 主题 "/recent_cloud" 发布 局部平面点点云
// 主题 "/registered_cloud" 发布 当前帧点云
clearCloud();
}
}
}
};