任何形式的里程计包括imu、编码器、激光、视觉等均存在累计误差致命缺点，会导致随着SLAM距离的增加，其与真实位置偏差逐渐变大。因此具有闭环能力的后端处理十分必要；HDL后端采用g2o中图优化思想进行后端处理，关键帧作为顶点，而平面，gps，imu，闭环等信息作为边进行约束。
整个后端主要包括3个进程进行处理：顶点和边添加、定时优化、定时发布地图；
顶点
将帧间匹配中激光雷达的pose作为顶点，在hdl中引入了关键帧概念，实际上在整个SLAM过程中，选取一定距离或角度间隔的点云数据作为关键帧，其中将关键帧中的pose作为顶点。
其中优化前的顶点由关键帧（激光里程计）提供的pose作为顶点初始值；每次添加顶点时，同时记录整个slam轨迹的累加值，用于后续闭环条件判断。
约束边
激光里程计约束
关键帧的pose，作为顶点，相邻顶点的里程计位置变换矩阵作为边。
// 获取相对位置
Eigen::Isometry3d relative_pose = keyframe->odom.inverse() * prev_keyframe->odom;
// 信息矩阵
Eigen::MatrixXd information = inf_calclator->calc_information_matrix(keyframe->cloud, prev_keyframe->cloud, relative_pose);
// 添加边
auto edge = graph_slam->add_se3_edge(keyframe->node, prev_keyframe->node, relative_pose, information);
// 增加鲁棒性
graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("odometry_edge_robust_kernel", "NONE"), private_nh.param<double>("odometry_edge_robust_kernel_size", 1.0));
闭环约束
- 闭环检测
闭环约束边前提需要检测闭环可能。即robot在slam中到了曾经经历过环境，并检测出来，称为闭环检测。由于所有顶点存储均为关键帧，因此闭环也就是新的关键帧是否出现在历史关键帧附近。HDL采用了较为暴力的方法，即关键帧与符合初步条件的关键帧逐一进行匹配。其中闭环检测初步条件包括：
- 两次闭环需要有一定距离间隔，因为短距离内闭环无意义（即防止同一位置附近自己闭环）。
- 闭环检测的两个keyframe 需要经历一定轨迹累计间隔，防止假闭环出现（即相当于里程计功能）；
- 两个keyframe 在优化前距离一定范围内；
std::vector<KeyFrame::Ptr> find_candidates(const std::vector<KeyFrame::Ptr>& keyframes, const KeyFrame::Ptr& new_keyframe) const {
// too close to the last registered loop edge
// 累计距离过近，则无需进行闭环检测，无意义
// 即闭环需要经过一定距离间隔
if(new_keyframe->accum_distance - last_edge_accum_distance < distance_from_last_edge_thresh) {
return std::vector<KeyFrame::Ptr>();
}
std::vector<KeyFrame::Ptr> candidates;
candidates.reserve(32);
for(const auto& k : keyframes) {
// traveled distance between keyframes is too small
// 闭环匹配的两帧累计距离间隔须有一定范围
if(new_keyframe->accum_distance - k->accum_distance < accum_distance_thresh) {
continue;
}
// 两帧关键帧的坐标间隔小于一定范围则认为是匹配对
const auto& pos1 = k->node->estimate().translation();
const auto& pos2 = new_keyframe->node->estimate().translation();
// estimated distance between keyframes is too small
double dist = (pos1.head<2>() - pos2.head<2>()).norm();
if(dist > distance_thresh) {
continue;
}
//
candidates.push_back(k);
}
return candidates;
}
如果满足以上条件的，则可作为闭环可能匹配的候选。将所有候选keyFrame对进行一一点云匹配；如果匹配相关度高于一定值时，则检测到闭环。并获取新的关键帧与变换历史帧的相对位姿；
- 添加闭环边
闭环边约束与激光里程计边约束完全一样；
Eigen::Isometry3d relpose(loop->relative_pose.cast<double>());
// 计算信息矩阵
Eigen::MatrixXd information_matrix = inf_calclator->calc_information_matrix(loop->key1->cloud, loop->key2->cloud, relpose);
// 添加闭环的约束边
auto edge = graph_slam->add_se3_edge(loop->key1->node, loop->key2->node, relpose, information_matrix);
graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("loop_closure_edge_robust_kernel", "NONE"), private_nh.param<double>("loop_closure_edge_robust_kernel_size", 1.0));
- 关键帧说明
在做闭环时，包含两类keyframe，已经检测过的keyframe和未检测过闭环的新添加的new_keyframe。满足条件的两个关键帧，则是将new_keyframe中的每一个和keyframe中每一个进行判断和点云匹配，较为暴力；
平面约束
- 说明
从功能上显然能看出，主要目的是约束SLAM过程中高度上产生的误差。由于实际环境中，无论室内还是室外，采用的lidar传感器主要为水平安装，因此在高度上约束较少，在室外可采用gps提供的高度信息进行约束，如果没有gps信号，则高度误差较大。
如果提前已知robot运行的为平面环境，则可根据平面信息进行约束，即锁定高度信息，防止地图倾斜或闭环上上下重影；（注：非平面环境不可使用，否则会加大误差，因为平面约束就是认为slam所行走的轨迹均在一个平面上）
在hdl整体介绍中已经说明采用RANSAC拟合出平面参数。通过以下收集函数进行收集对应关键帧的平面信息。
void floor_coeffs_callback(const hdl_graph_slam::FloorCoeffsConstPtr& floor_coeffs_msg) {
if(floor_coeffs_msg->coeffs.empty()) {
return;
}
std::lock_guard<std::mutex> lock(floor_coeffs_queue_mutex);
floor_coeffs_queue.push_back(floor_coeffs_msg);
}
将每帧检测的平面参数信息放入队列缓存中，待处理；
2. 添加约束边
bool updated = false;
for(const auto& floor_coeffs : floor_coeffs_queue) {
if(floor_coeffs->header.stamp > latest_keyframe_stamp) {
break;
}
// 找到平面时间戳对应的关键帧
auto found = keyframe_hash.find(floor_coeffs->header.stamp);
if(found == keyframe_hash.end()) {
continue;
}
遍历每一个平面参数，并根据时间戳找到对应的keyframe，其目的即约束该关键帧pose。
// 初始化平面节点
if(!floor_plane_node) {
floor_plane_node = graph_slam->add_plane_node(Eigen::Vector4d(0.0, 0.0, 1.0, 0.0));
floor_plane_node->setFixed(true);
}
约束的第一个平面作为初始值，即固定平面。后续所有平面都以第一帧平面作为地平面信息参考。
const auto& keyframe = found->second;
// 添加平面约束
Eigen::Vector4d coeffs(floor_coeffs->coeffs[0], floor_coeffs->coeffs[1], floor_coeffs->coeffs[2], floor_coeffs->coeffs[3]);
Eigen::Matrix3d information = Eigen::Matrix3d::Identity() * (1.0 / floor_edge_stddev);
auto edge = graph_slam->add_se3_plane_edge(keyframe->node, floor_plane_node, coeffs, information);
graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("floor_edge_robust_kernel", "NONE"), private_nh.param<double>("floor_edge_robust_kernel_size", 1.0));
// 记录
keyframe->floor_coeffs = coeffs;
updated = true;
}
其中平面约束边在g2o中有现成定义，不需要创建。构建的约束边为平面与定顶点之间的边。
imu约束
void imu_callback(const sensor_msgs::ImuPtr& imu_msg) {
//都不需要
if(!enable_imu_orientation && !enable_imu_acceleration) {
return;
}
std::lock_guard<std::mutex> lock(imu_queue_mutex);
imu_msg->header.stamp += ros::Duration(imu_time_offset);
imu_queue.push_back(imu_msg);
}
hdl采用imu信息主要采用姿态角和重力加速度方向进行约束。实时收集imu数据并放入缓存队列中。
auto closest_imu = imu_cursor;
for(auto imu = imu_cursor; imu != imu_queue.end(); imu++) {
auto dt = ((*closest_imu)->header.stamp - keyframe->stamp).toSec();
auto dt2 = ((*imu)->header.stamp - keyframe->stamp).toSec();
if(std::abs(dt) < std::abs(dt2)) {
break;
}
closest_imu = imu;
}
从imu队列中遍历找到对应的时间戳最近的keyframe。
// 获取姿态角和线加速度
const auto& imu_ori = (*closest_imu)->orientation;
const auto& imu_acc = (*closest_imu)->linear_acceleration;
geometry_msgs::Vector3Stamped acc_imu;
geometry_msgs::Vector3Stamped acc_base;
geometry_msgs::QuaternionStamped quat_imu;
geometry_msgs::QuaternionStamped quat_base;
quat_imu.header.frame_id = acc_imu.header.frame_id = (*closest_imu)->header.frame_id;
quat_imu.header.stamp = acc_imu.header.stamp = ros::Time(0);
acc_imu.vector = (*closest_imu)->linear_acceleration;
quat_imu.quaternion = (*closest_imu)->orientation;
// 将imu位置转换到base坐标系下
try {
tf_listener.transformVector(base_frame_id, acc_imu, acc_base);
tf_listener.transformQuaternion(base_frame_id, quat_imu, quat_base);
} catch(std::exception& e) {
std::cerr << "failed to find transform!!" << std::endl;
return false;
}
keyframe->acceleration = Eigen::Vector3d(acc_base.vector.x, acc_base.vector.y, acc_base.vector.z);
keyframe->orientation = Eigen::Quaterniond(quat_base.quaternion.w, quat_base.quaternion.x, quat_base.quaternion.y, quat_base.quaternion.z);
keyframe->orientation = keyframe->orientation;
获取imu中对应历史关键帧最新匹配的一帧数据，并根据静态tf变换，转换到baseframe坐标系下的姿态和加速度信息。
- 添加姿态约束
Eigen::MatrixXd info = Eigen::MatrixXd::Identity(3, 3) / imu_orientation_edge_stddev;
auto edge = graph_slam->add_se3_prior_quat_edge(keyframe->node, *keyframe->orientation, info);
graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("imu_orientation_edge_robust_kernel", "NONE"), private_nh.param<double>("imu_orientation_edge_robust_kernel_size", 1.0));
- 添加重力加速度约束
Eigen::MatrixXd info = Eigen::MatrixXd::Identity(3, 3) / imu_acceleration_edge_stddev;
g2o::OptimizableGraph::Edge* edge = graph_slam->add_se3_prior_vec_edge(keyframe->node, -Eigen::Vector3d::UnitZ(), *keyframe->acceleration, info);
graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("imu_acceleration_edge_robust_kernel", "NONE"), private_nh.param<double>("imu_acceleration_edge_robust_kernel_size", 1.0));
疑问：姿态约束是否准确，因为时间戳并没有严格对齐，即没有进行根据时间进行插值
gps约束
GPS较为简单，因为直接提供的是全局绝对位置，仅需将gps经纬度转换为大地坐标系，并且将初始位置进行坐标初始处理即可。
方法同样将每帧gps信息放入缓存队列中，然后根据时间戳找到对应的keyframe即顶点进行约束。
// 减去初始偏移量（即初始gps位置）
xyz -= (*zero_utm);
// 关键帧添加gps位置
keyframe->utm_coord = xyz;
// 根据gps是否存在高度值，添加概率图边
g2o::OptimizableGraph::Edge* edge;
if(std::isnan(xyz.z())) {
Eigen::Matrix2d information_matrix = Eigen::Matrix2d::Identity() / gps_edge_stddev_xy;
edge = graph_slam->add_se3_prior_xy_edge(keyframe->node, xyz.head<2>(), information_matrix);
} else {
Eigen::Matrix3d information_matrix = Eigen::Matrix3d::Identity();
information_matrix.block<2, 2>(0, 0) /= gps_edge_stddev_xy;
information_matrix(2, 2) /= gps_edge_stddev_z;
edge = graph_slam->add_se3_prior_xyz_edge(keyframe->node, xyz, information_matrix);
}
// 增加鲁棒性
graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("gps_edge_robust_kernel", "NONE"), private_nh.param<double>("gps_edge_robust_kernel_size", 1.0));
定时优化
之所以前面所经所有传感器信息在优化前均需要放入buffer队列中，是因为hdl采用后端优化机制是定时进行调用优化。因此在每次优化间隔间，不同传感器可独立进行收集数据缓存，在优化时根据最新的keyframe的时间戳去匹配所有其他传感器数据信息，添加对应边约束，并剔除过时的传感器数据；
// 处理关键帧队列
bool keyframe_updated = flush_keyframe_queue();
if(!keyframe_updated) {
std_msgs::Header read_until;
read_until.stamp = ros::Time::now() + ros::Duration(30, 0);
read_until.frame_id = points_topic;
read_until_pub.publish(read_until);
read_until.frame_id = "/filtered_points";
read_until_pub.publish(read_until);
}
// 如果4类传感器数据均无处理结果，则无需进行优化
if(!keyframe_updated & !flush_floor_queue() & !flush_gps_queue() & !flush_imu_queue()) {
return;
}
其中keyframe中pose作为优化图中的顶点，为核心数据。如果所有传感器约束均没有添加，则无需进行优化；
// loop detection
std::vector<Loop::Ptr> loops = loop_detector->detect(keyframes, new_keyframes, *graph_slam);
// 存在闭环，则将闭环约束加入概率图中
for(const auto& loop : loops) {
Eigen::Isometry3d relpose(loop->relative_pose.cast<double>());
// 计算信息矩阵
Eigen::MatrixXd information_matrix = inf_calclator->calc_information_matrix(loop->key1->cloud, loop->key2->cloud, relpose);
// 添加闭环的约束边
auto edge = graph_slam->add_se3_edge(loop->key1->node, loop->key2->node, relpose, information_matrix);
graph_slam->add_robust_kernel(edge, private_nh.param<std::string>("loop_closure_edge_robust_kernel", "NONE"), private_nh.param<double>("loop_closure_edge_robust_kernel_size", 1.0));
}
// 将new_keyframes复制到keyframes，用于历史记录， 而new_keyframes 用于与历史keyframes闭环检测
std::copy(new_keyframes.begin(), new_keyframes.end(), std::back_inserter(keyframes));
// 清除所有新添加的关键帧
new_keyframes.clear();
其中闭环检测在每次优化过程中进行，找到所有可能闭环条件，并增加闭环约束。当处理闭环后，则新添加的关键帧作为历史关键帧进行保存，并清除新关键帧队列，从而保证新添加的keyframe仅有一次机会与历史keyframe进行闭环约束。
// 优化
int num_iterations = private_nh.param<int>("g2o_solver_num_iterations", 1024);
graph_slam->optimize(num_iterations);
调用g2o进行图优化。
const auto& keyframe = keyframes.back();
Eigen::Isometry3d trans = keyframe->node->estimate() * keyframe->odom.inverse();
trans_odom2map_mutex.lock();
trans_odom2map = trans.matrix().cast<float>();
trans_odom2map_mutex.unlock();
将最新帧的keyframe的优化后的pose作为 里程计进行发布；
std::vector<KeyFrameSnapshot::Ptr> snapshot(keyframes.size());
std::transform(keyframes.begin(), keyframes.end(), snapshot.begin(), [=](const KeyFrame::Ptr& k) { return std::make_shared<KeyFrameSnapshot>(k); });
// 每优化一次将优化的结果复制到keyframes_snapshot中，用于发布地图
// 采用交换的方法可以让keyframes_snapshot之前的数据内存进行回收
keyframes_snapshot_mutex.lock();
keyframes_snapshot.swap(snapshot);
keyframes_snapshot_mutex.unlock();
复制一份优化后的历史关键帧，用于地图发布进程进行地图发布；
定时发布地图
// 将复制的关键帧队列，拼接成map
auto cloud = map_cloud_generator->generate(snapshot, map_cloud_resolution);
if(!cloud) {
return;
}
cloud->header.frame_id = map_frame_id;
cloud->header.stamp = snapshot.back()->cloud->header.stamp;
sensor_msgs::PointCloud2Ptr cloud_msg(new sensor_msgs::PointCloud2());
pcl::toROSMsg(*cloud, *cloud_msg);
// 发布map
map_points_pub.publish(cloud_msg);
根据优化后提供的keyframe队列，进行点云拼接形成slam点云地图，并进行发布。
2325
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
