节点与submap
cartogrpher 在每一次添加节点的时候进行判断是否优化。cartogrpher 的全局优化算法在 sovle( 函数里面,在 PoseGraph2D::RunOptimization() 的判断 逻辑里面,调用堆栈到 AddNode 逻辑。
1. cartogrpher的AddNode与优化过程
2. range数据到submap的嵌入更新过程
cartographer并非使用3d或者视觉slam的经典poseLBA过程,而是通过网格优化来进行计算的。
range
数据添加过程:
ros
端的数据添加,对接参考章节
1
的
Ros
端的数据添加过程。
Track
和定位:
Track
过程参考章节
2
的
track
过程;
Match
:
real_time_correlative_scan_matcher
_
.
Match
和
ceres_scan_matcher
_
过程。
submap
更新过程:使用了
votex
优化
。
时间分析
:
Filte
至多
消耗
1
毫秒之上,
1-2ms
;
InsertIntoSubmap
可以消耗
18ms
。
3.优化过程
cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(
const common::Time time,
const sensor::RangeData& gravity_aligned_range_data,
const transform::Rigid3d& gravity_alignment)
{
if (gravity_aligned_range_data.returns.empty()) {
..............................
std::unique_ptr<InsertionResult> insertion_result =
InsertIntoSubmap(time, range_data_in_local, gravity_aligned_range_data,
pose_estimate, gravity_alignment.rotation());
........................
优化过程:
std::unique_ptr<LocalTrajectoryBuilder2D::InsertionResult>
LocalTrajectoryBuilder2D::InsertIntoSubmap(
const common::Time time, const sensor::RangeData& rang..)
{
..................
active_submaps_.InsertRangeData(range_data_in_local);
...................
const sensor::PointCloud filtered_gravity_aligned_point_cloud =
adaptive_voxel_filter.Filter(gravity_aligned_range_data.returns);
....................}