目录
项目主要功能为:
- prefiltering:点云滤波
- floor_detection:地面平面检测
- scan_matching:前端里程计
- hdl_graph_slam:后端概率图构建
代码文件位于:/hdl_graph_slam/apps/
文件路径下:
- prefiltering_nodelet.cpp
- floor_detection_nodelet.cpp
- scan_matching_odometry_nodelet.cpp
- hdl_graph_slam_nodelet.cpp
apps文件夹存放节点文件,而节点文件就是生成可执行文件的代码文件,相当于整个程序的入口。
其余文件夹内容参考 :hdl_graph_slam源码解读(一):整体介绍
1. prefiltering_nodelet.cpp:点云滤波
这部分主要是对点云实现预处理功能,主要包括
- 点云网格滤波
- 点云离群点 outlier 的去除 (这里可以选择statistic 或者 raduis 两种方式进行滤波)
- 距离滤波(只要点云在指定距离范围内的点)
2. floor_detection_nodelet.cpp:地面平面检测
该 cpp 主要接收 prefiltering_nodelet.cpp
滤波后的点云,并完成地面检测。
主要功能由 boost::optional floor = detect(cloud)
函数完成:
- 可以设置
tilt_matrix
对 Y 轴方向的激光倾斜角进行补偿 - 紧接着,调用
filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height + height_clip_range), false);
函数:输入是滤波后的点云,输出是平面点云 ,vector 4f
对应的是想要的平面约束,plane clip
后续会根据平面约束滤出相关的点云 normal_filtering(filtered) ;
:filtered
表示平面点云,normal_filtering
主要通过非垂直方向对点云进行滤波pcl::RandomSampleConsensus ransac(model_p);
:通过ransac
函数,根据输入的准平面点云进行再一次滤波。主要滤出外点,同时保留内点,并计算平面内点所对应的平面方程系数
3. scan_matching_odometry_nodelet.cpp:前端里程计
该函数主要计算相邻两帧之间的匹配,输入为 prefiltering_nodelet.cpp
滤波后得到的 /filtered_points
,紧接着进入matching
函数:
matching const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud
- 第一步:首先判断是否是关键帧
- 第二步:这里主要调用 PCL 里面现成的库函数,主要有(
ICP
,GICP
,NDT
,GICP_OMP
,NDT_OMP
) , 值得关注的是NDT_OMP
,该算法通过采用多线程方式去完成NDT
算法,因此效率是传统NDT
算法的 10 倍,精度基本差不多; 这里如果选择KDTREE
,那么和传统的NDT
算法一致,剩下的速度比较:DIRECT1
>DIRECT7
, 但是DIRECT7
更稳定同时速度比KDTREE
快 - 选择完上述方法之后就开始完成匹配,这里主要是当前帧点云与
Keyframe
进行匹配 - 有两个参数比较重要:
keyframe_delta_trans
、keyframe_delta_angle
: The minimum tranlational distance and rotation angle between keyframes.If this value is zero, frames are always compared with the previous frame.
4. hdl_graph_slam_nodelet.cpp:后端概率图构建
4.1 cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg,constsensor_msgs::PointCloud2::ConstPtr& cloud_msg)
该函数主要是odom信息与cloud信息的同步,同步之后检查关键帧是否更新。
关键帧判断:这里主要看关键帧设置的这两个阈值keyframe_delta_trans
、keyframe_delta_angle
;作者认为关键帧不能设置的太近,同时也不能太远;太近的关键帧会被抛弃,如果keyframe_delta_trans=0
表示关键帧之间的距离为0,因此每一帧都是关键帧,scan_matching
函数就会变成相邻帧匹配。
变成关键帧的要求就是:/hdl_graph_slam/include/hdl_graph_slam/keyframe_updater.hpp
// calculate the delta transformation from the previous keyframe
Eigen::Isometry3d delta = prev_keypose.inverse() * pose;
double dx = delta.translation().norm();
double da = std::acos(Eigen::Quaterniond(delta.linear()).w());
// too close to the previous frame
if(dx < keyframe_delta_trans && da < keyframe_delta_angle) {
return false;
}
4.2 optimization_timer_callback(const ros::TimerEvent& event)
函数功能:将所有的位姿放在posegraph
中开始优化
loop detection
函数:主要就是将当前帧和历史帧遍历,寻找loop。
- 寻找潜在闭环帧:
/hdl_graph_slam/include/hdl_graph_slam/loop_detector.hpp
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;
}
- 由于每个关键帧都建立累计距离,当前帧对应的累计距离,即边的累计距距离 <
distance_from_last_edge_thresh
,那么就不能建立闭环:意思就是相邻两个闭环帧不能建立的过于密集 accum_distance_thresh
这里指的是当前帧与临近帧的距离阈值,假设距离阈值为5m, 那么当前帧距离5米范围内的关键帧不做考虑distance_thresh
小于该阈值范围内两个关键帧为潜在闭环帧,将所有满足条件的都存起来作为candidates
4.3 潜在闭环完成匹配(matching 函数)
Loop::Ptr matching(const std::vector<KeyFrame::Ptr>& candidate_keyframes, const KeyFrame::Ptr& new_keyframe, hdl_graph_slam::GraphSLAM& graph_slam) {
if(candidate_keyframes.empty()) {
return nullptr;
}
registration->setInputTarget(new_keyframe->cloud);
double best_score = std::numeric_limits<double>::max();
KeyFrame::Ptr best_matched;
Eigen::Matrix4f relative_pose;
std::cout << std::endl;
std::cout << "--- loop detection ---" << std::endl;
std::cout << "num_candidates: " << candidate_keyframes.size() << std::endl;
std::cout << "matching" << std::flush;
auto t1 = ros::Time::now();
pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
for(const auto& candidate : candidate_keyframes) {
registration->setInputSource(candidate->cloud);
Eigen::Matrix4f guess = (new_keyframe->node->estimate().inverse() * candidate->node->estimate()).matrix().cast<float>();
guess(2, 3) = 0.0;
registration->align(*aligned, guess);
std::cout << "." << std::flush;
double score = registration->getFitnessScore(fitness_score_max_range);
if(!registration->hasConverged() || score > best_score) {
continue;
}
best_score = score;
best_matched = candidate;
relative_pose = registration->getFinalTransformation();
}
auto t2 = ros::Time::now();
std::cout << " done" << std::endl;
std::cout << "best_score: " << boost::format("%.3f") % best_score << " time: " << boost::format("%.3f") % (t2 - t1).toSec() << "[sec]" << std::endl;
if(best_score > fitness_score_thresh) {
std::cout << "loop not found..." << std::endl;
return nullptr;
}
std::cout << "loop found!!" << std::endl;
std::cout << "relpose: " << relative_pose.block<3, 1>(0, 3) << " - " << Eigen::Quaternionf(relative_pose.block<3, 3>(0, 0)).coeffs().transpose() << std::endl;
last_edge_accum_distance = new_keyframe->accum_distance;
return std::make_shared<Loop>(new_keyframe, best_matched, relative_pose);
}
主要是当前帧与潜在闭环帧完成点云匹配,通过统计得分寻找最优匹配,并且只将最优匹配对应的transform
作为最优闭环约束。这里对应的就是fitness_score_thresh
。
4.4 全场亮点:计算不同loop的信息矩阵
calc_information_matrix(loop->key1->cloud, loop->key2->cloud, relpose)
:这里relpose
表示闭环帧之间的位姿约束。
Eigen::MatrixXd InformationMatrixCalculator::calc_information_matrix(const pcl::PointCloud<PointT>::ConstPtr& cloud1, const pcl::PointCloud<PointT>::ConstPtr& cloud2, const Eigen::Isometry3d& relpose) const {
if(use_const_inf_matrix) {
Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6);
inf.topLeftCorner(3, 3).array() /= const_stddev_x;
inf.bottomRightCorner(3, 3).array() /= const_stddev_q;
return inf;
}
- 首先判断是否使用常值信息矩阵,一般都是单位阵1/0.5=50 作为位姿, 1/0.1=10作为角度;可以看出我们更相信位姿,而非角度;这里对应变量分别是:
const_stddev_x
、const_stddev_q
。 - 这里的fitness_socre 的计算非常朴素,主要是通过:
- 建立
KD-TREE
- 在目标点云里面寻找
source
点云每个点对应的最近点,并记录两点间的距离 fitness_score += nn_dists[i];
、return (fitness_score / n);
: 上面这两个式子实际是记录所有点的距离,并将(所有点的距离/n)作为 平均距离;- 通过求所有闭环帧,
KD-TREE
查找对应点的平均距离来作为fitness_score
- 建立
4.5 gps对应的信息矩阵(这里体现g2o的强大之处)
作者主要看这几件事:
- GPS 坐标转换:主要是GPS地球坐标系->转换到大地坐标系(UTM)-> UTM坐标系再转换成局部坐标
- 这里只用gps的 X和Y信息,z 方向的信息不用,主要原因是不准,这就涉及设置信息矩阵,注意信息矩阵的类型:
graph_slam->add_se3_prior_xy_edge((*seek)->node, xyz.head<2>(), information_matrix);
//添加GPS约束,只给xy增加约束
g2o::EdgeSE3PriorXY* GraphSLAM::add_se3_prior_xy_edge(g2o::VertexSE3* v_se3, const Eigen::Vector2d& xy, const Eigen::MatrixXd& information_matrix) {
//添加该顶点
g2o::EdgeSE3PriorXY* edge(new g2o::EdgeSE3PriorXY());
//添加gps对应的测量值(xy)还是UTM坐标
edge->setMeasurement(xy);
//添加对应的信息矩阵
edge->setInformation(information_matrix);
//添加该顶点
edge->vertices()[0] = v_se3;
graph->addEdge(edge);
return edge;
}
4.6 添加查找对应的keyframe,并添加地面约束
- 检测平面,这里主要涉及
floor_detection_nodelet.cpp
里面讲的平面检测;最后通过ransac
计算平面系数 - 通过计算得到的平面系数来添加约束,主要对检测到的平面的顶点,添加平面约束,同时引进信息矩阵。
- 信息矩阵是3*3的,应该只对XYZ起到约束作用
//添加平面约束:有的设置是0.1,有的设置是100
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);
//添加平面约束
graph_slam->add_se3_plane_edge(keyframe->node, graph_slam->floor_plane_node, coeffs, information);
使用的add_se3_plane_edge
函数的代码如下:
//添加平面约束
g2o::EdgeSE3Plane* GraphSLAM::add_se3_plane_edge(g2o::VertexSE3* v_se3, g2o::VertexPlane* v_plane, const Eigen::Vector4d& plane_coeffs, const Eigen::MatrixXd& information_matrix) {
//G2O本身自带平面这种边
g2o::EdgeSE3Plane* edge(new g2o::EdgeSE3Plane());
//添加由ransssac 得到的平面系数
edge->setMeasurement(plane_coeffs);
//设置信息矩阵3*3
edge->setInformation(information_matrix);
//设置该顶点
edge->vertices()[0] = v_se3;
//以及该顶点对应的平面
edge->vertices()[1] = v_plane;
//添加平面
graph->addEdge(edge);
return edge;
}