0. 简介
个人在想在长期执行的SLAM程序时，当场景发生替换时，激光SLAM如何有效的更新或者替换地图是非常关键的。在看了很多Life-Long的文章后，个人觉得可以按照以下思路去做。这里可以给大家分享一下
<br/>
1. 初始化保存关键帧
首先对应的应该是初始化设置，初始化设置当中会保存关键帧数据，这里的对应的关键帧点云数据会被存放在history_kf_lidar当中，这个数据是和关键帧状态一一对应的。
/**
* @brief 初始化当前第一个关键帧
*/
void dlio::OdomNode::initializeInputTarget() {
// 构建第一个关键帧
// 更新prev_scan_stamp
this->prev_scan_stamp = this->scan_stamp;
// 保存关键帧的姿态 以及去畸变降采样后的点云
this->keyframes.push_back(std::make_pair(std::make_pair(this->lidarPose.p, this->lidarPose.q), this->current_scan));
// 保存关键帧消息时间戳
this->keyframe_timestamps.push_back(this->scan_header_stamp);
// 保存关键帧点云协方差
this->keyframe_normals.push_back(this->gicp.getSourceCovariances());
// 初始T-corr为单位的 T = T_corr * T_prior
this->keyframe_transformations.push_back(this->T_corr);
this->keyframe_transformations_prior.push_back(this->T_prior);
this->keyframe_stateT.push_back(this->T);
// 保存历史的lidar系点云
std::unique_lock<decltype(this->history_kf_lidar_mutex)> lock_his_lidar(this->history_kf_lidar_mutex);
pcl::PointCloud<PointType>::Ptr temp(new pcl::PointCloud<PointType>);
pcl::copyPointCloud(*this->current_scan_lidar, *temp);
this->history_kf_lidar.push_back(temp);
lock_his_lidar.unlock();
// 保存当前的关键帧信息 等待后端处理
// tempKeyframe的姿态这里有两种处理方法
// 1. 使用lidarPose 2. 使用state
// 根据DLIO作者的解释 使用lidarPose更加稳定
// (https://github.com/vectr-ucla/direct_lidar_inertial_odometry/issues/13#issuecomment-1638876779)
this->currentFusionState = this->state;
this->tempKeyframe.pos = this->lidarPose.p;
this->tempKeyframe.rot = this->currentFusionState.q;
this->tempKeyframe.vSim = {1};
this->tempKeyframe.submap_kf_idx = {0};
this->tempKeyframe.time = this->scan_stamp;
this->v_kf_time.push_back(this->scan_stamp);
pcl::copyPointCloud(*this->current_scan, *this->tempKeyframe.pCloud);
this->KeyframesInfo.push_back(this->tempKeyframe);
this->saveFirstKeyframeAndUpdateFactor();
}();
在保存第一针状态并更新因子的时候，会同时缓存一个update_map_info。这个缓存了回环数据（依赖addLoopFactor函数）以及基于Gtsam计算出来的(iSAMCurrentEstimate)函数。
添加里程计因子 - 通过调用this->addOdomFactor()，函数向因子图中添加了一个里程计因子。里程计因子可能包含了从传感器得到的运动信息，这些信息用于估算机器人或设备的位置。
更新ISAM（增量平滑和映射） - 使用this->isam->update(this->gtSAMgraph, this->initialEstimate)来更新ISAM的状态，这个步骤通常包括增量优化，以便在获得新数据时快速更新地图或路径。然后再次调用this->isam->update()进行进一步的更新。 清空因子图和初始估计 - 将因子图和初始状态估计清空，这通常发生在更新完成之后，为下一轮更新做准备。 **计算当前估计 **- 使用this->isam->calculateEstimate()来计算当前状态的最佳估计。 获取临时关键帧信息 - 在互斥锁的保护下，从this->tempKeyframe获取临时关键帧的信息，并释放锁。 校正位置 - 通过调用this->correctPoses()函数来校正或调整已经估计的位置。 更新地图信息 - 处理是否发生了回环检测（由this->isLoop变量表示），并将当前估计状态和回环检测的结果推入更新队列this->update_map_info中，用于后续的地图更新或路径规划。
/**
* @brief 对第一帧的处理
*/
void dlio::OdomNode::saveFirstKeyframeAndUpdateFactor()
{
// 添加里程计因子
this->addOdomFactor();
this->isam->update(this->gtSAMgraph, this->initialEstimate);
this->isam->update();
this->gtSAMgraph.resize(0);
this->initialEstimate.clear();
this->iSAMCurrentEstimate = this->isam->calculateEstimate();
std::unique_lock<decltype(this->tempKeyframe_mutex)> lock_temp(this->tempKeyframe_mutex);
KeyframeInfo his_info = this->tempKeyframe;
lock_temp.unlock();
this->correctPoses();
auto loop_copy = this->isLoop;
auto iSAMCurrentEstimate_copy = this->iSAMCurrentEstimate;
std::unique_lock<decltype(this->update_map_info_mutex)> lock_update_map(this->update_map_info_mutex);
this->update_map_info.push(std::make_pair(loop_copy, iSAMCurrentEstimate_copy));
lock_update_map.unlock();
}
在初始状态下，第一帧只用去考虑this->addOdomFactor();这个函数，用于添加odom的因子，里程计因子包含了从传感器得到的运动信息，这些信息用于估算机器人或设备的位置。
静态计数器- static int num_factor用作计数器，记录添加到因子图中的里程计因子数量。
获取当前关键帧信息 - 函数首先通过互斥锁保护的方式获取临时关键帧（this->tempKeyframe）的信息。
初始帧处理 - 如果是第一帧（num_factor为0），函数会设置一个非常小的噪声模型作为先验，表明对初始关键帧的位置非常有信心，并将其添加到因子图和初始估计中。
添加里程计因子 - 对于非第一帧的情况，函数会遍历当前关键帧匹配的子地图关键帧，并对每一个匹配计算一个从匹配关键帧到当前关键帧的位姿变换。这涉及到获取匹配关键帧的位姿并计算它们之间的相对变换（poseFrom.between(poseTo)）。
计算噪声权重 - 函数会根据匹配的相似度计算噪声权重，相似度越高，噪声权重越小，表示对这个匹配的信心越大。这里依赖的是buildSubmapViaJaccard函数
添加因子到因子图 - 将计算得到的位姿变换和相应的噪声模型作为里程计因子添加到因子图中。
更新初始估计- 将当前关键帧的位姿添加到初始估计中。
因子计数器递增 - 最后，num_factor递增，为下一次调用准备
/**
* @brief 添加里程计因子 包含连续里程计和非连续里程计
*/
void dlio::OdomNode::addOdomFactor()
{
static int num_factor = 0;
std::unique_lock<decltype(this->tempKeyframe_mutex)> lock_temp(this->tempKeyframe_mutex);
KeyframeInfo current_kf_info = this->tempKeyframe;
lock_temp.unlock();
// 初始第一帧
if (num_factor == 0)
{
gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(
(gtsam::Vector(6) << 1e-12, 1e-12, 1e-12, 1e-12, 1e-12, 1e-12).finished());
this->gtSAMgraph.addPrior(0, state2Pose3(current_kf_info.rot, current_kf_info.pos), priorNoise);
this->initialEstimate.insert(0, state2Pose3(current_kf_info.rot, current_kf_info.pos));
}
else
{
// 当前帧匹配的子地图关键帧索引
std::vector<int> curr_submap_id = current_kf_info.submap_kf_idx;
// 当前帧匹配的子地图关键帧相似度
std::vector<float> curr_sim = current_kf_info.vSim;
// 当前帧的位姿
gtsam::Pose3 poseTo = state2Pose3(current_kf_info.rot, current_kf_info.pos);
// 遍历与当前帧匹配的子地图关键帧
for (int i = 0; i < curr_submap_id.size(); i++)
{
// 子地图关键帧的位姿
std::unique_lock<decltype(this->keyframes_mutex)> lock_kf(this->keyframes_mutex);
gtsam::Pose3 poseFrom = state2Pose3(this->KeyframesInfo[curr_submap_id[i]].rot,
this->KeyframesInfo[curr_submap_id[i]].pos);
lock_kf.unlock();
// 噪声权重
double weight = 1.0;
if (curr_sim.size() > 0)
{
// 对相似度进行归一化
auto max_sim = std::max_element(curr_sim.begin(), curr_sim.end());
for (auto &it: curr_sim)
it = it / *max_sim;
double sim = curr_sim[i] >= 1 ? 0.99 : curr_sim[i] < 0 ? 0 : curr_sim[i];
// 相似度越大 噪声权重越小
weight = (1 - sim) * this->icpScore;
}
// 添加相邻/不相邻里程计因子
gtsam::noiseModel::Diagonal::shared_ptr odometryNoise = gtsam::noiseModel::Diagonal::Variances(
(gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished() * weight);
this->gtSAMgraph.emplace_shared<gtsam::BetweenFactor<gtsam::Pose3>>(curr_submap_id[i],
num_factor,poseFrom.between(poseTo), odometryNoise);
}
this->initialEstimate.insert(num_factor, poseTo);
}
num_factor++;
}点击激光SLAM如何动态管理关键帧和地图 ——古月居可查看全文我
扫一扫
3497
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
