cartographer_learn_16计算节点的全局坐标系下的姿态
续接上篇
上一篇我们正式从前端过渡到了后端，从本篇开始，我们开始正式学习讨论后端。注意我们讨论的二维的slam过程，所以对应的类是PoseGraphD。这个类再文件cartographer/cartographer/mapping/internal/2d/pose_graph_2d.h中。本篇从上篇提到过的AddNode开始讨论后端。
制作并存储节点
这里所说的节点对应的是前端的一次扫描匹配的结果。
计算节点的全局坐标系下的姿态
在data_::global_submap_poses_2d找到对应的轨迹的情况
上代码：
NodeId PoseGraph2D::AddNode(
std::shared_ptr<const TrajectoryNode::Data> constant_data,
const int trajectory_id,
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
// 计算节点在全局坐标中的位姿
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
......//一顿其他的操作
}
//调用函数1
transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform(
const int trajectory_id) const {
absl::MutexLock locker(&mutex_);
return ComputeLocalToGlobalTransform(data_.global_submap_poses_2d,
trajectory_id);
}
//调用函数2
transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
const MapById<SubmapId, optimization::SubmapSpec2D>& global_submap_poses,
const int trajectory_id) const {
//在global_submap_poses_2d中找对应轨迹
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
// 没找到
if (begin_it == end_it) {
......//其他操作
}
//找到了，并获取该轨迹最后一个子图的id
const SubmapId last_optimized_submap_id = std::prev(end_it)->id;
//wTl = wTs * sTl
return transform::Embed3D(
global_submap_poses.at(last_optimized_submap_id).global_pose) *
data_.submap_data.at(last_optimized_submap_id)
.submap->local_pose()
.inverse();
}
先看AddNode如何计算节点在全局坐标系（global）中的姿态的，它是使用某个姿态乘以节点在local坐标系下的姿态。想必这个某个姿态是local坐标系在global中的姿态。这里又有一个问题，local坐标系是什么东西？作者的理解是一次slam过程一般会产生一个轨迹，cartographer认为每个轨迹都有自己的坐标系即local坐标系（这就是为什么求local坐标系位姿时传入的参数时轨迹的id）。这里是作责自己的理解，有不正确之处请斧正。
从上面的代码可以看出AddNode实际上时调用了ComputeLocalToGlobalTransform去计算某个local坐标系的位姿。这里我们去看看ComputeLocalToGlobalTransform这个函数。这里先解释一下能在data_::global_submap_poses_2d找到对应的轨迹的情况。这里要先看EndOfTrajectory这个方法：
ConstIterator EndOfTrajectory(const int trajectory_id) const {
return BeginOfTrajectory(trajectory_id + 1);
}
这里返回的是MapById的一个迭代器，我们还是和上一篇一样画图说明。如下图，MapById的储存情况还是和上一篇一样，此时我们调用EndOfTrajectory（2）返回的迭代器中的三个指针的指向如下图所示
这就是上述代码中end_it的状态。随后对end_it调用c++标准库中的prev算法（这就是前篇中提到的要在定义迭代器时提供5个信息的原因）。prev算法其实就是调用迭代器重载的–符号。我们去看看
ConstIterator& operator--() {
while (current_trajectory_ == end_trajectory_ ||
current_data_ == current_trajectory_->second.data_.begin()) {
--current_trajectory_;
current_data_ = current_trajectory_->second.data_.end();
}
--current_data_;
return *this;
}
可以看出调用了prev后迭代器的状态如下图所示：
总结一下经过这么一波操作后我们的得到了该轨迹存储的最后一个数据，在这里，这个数据是submap的位姿。不过在调用完prev后我们并不是获取位姿，而仅仅是要获取该估计最后一个submap的id。最后的返回是先到global_submap_poses获取该submap在全局坐标系中的位姿wTs，这里使用的at和普通的stl容器的at没差别。最后去submap_data中获取子图在local坐标系中的位姿。这里我们总是说去哪拿什么东西，比如data_.global_submap_poses_2d，data_.submap_data等。这里可能有疑问这些数据是怎么加进去的，相信这个往后我们会讨论到的，这里我们先认为这些容器中就是存在这些数据。
在data_::global_submap_poses_2d找不到对应的轨迹的情况
我们再来解释在data_::global_submap_poses_2d中找不到对应轨迹id的情况。这里cartographer的思路是找到这个loacl坐标系（轨迹id假设为2）与其他local坐标系（轨迹id假设为1）的相对关系1T2，然后再去找到轨迹为1的local坐标系在全局坐标系中的位姿即wT1，这样就可以求得轨迹为2的local坐标系在全局坐标系中的位姿了。
transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
const MapById<SubmapId, optimization::SubmapSpec2D>& global_submap_poses,
const int trajectory_id) const {
......//其他操作
// 没找到
if (begin_it == end_it) {
//先找是否存在某个local坐标系和这个local坐标系之间的额相互关系
const auto it = data_.initial_trajectory_poses.find(trajectory_id);
if (it != data_.initial_trajectory_poses.end()) {
//如果存在这种关系
return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id,
it->second.time) *
it->second.relative_pose;
} else {
//如果不存在这种关系，返回单位矩阵
return transform::Rigid3d::Identity();
}
}
......//其他操作
}
我们来看看GetInterpolatedGlobalTrajectoryPose这个方法，它是如何来获取某个轨迹在全局坐标系中的位姿的
transform::Rigid3d PoseGraph2D::GetInterpolatedGlobalTrajectoryPose(
const int trajectory_id, const common::Time time) const {
CHECK_GT(data_.trajectory_nodes.SizeOfTrajectoryOrZero(trajectory_id), 0);
//先查找
const auto it = data_.trajectory_nodes.lower_bound(trajectory_id, time);
if (it == data_.trajectory_nodes.BeginOfTrajectory(trajectory_id)) {
return data_.trajectory_nodes.BeginOfTrajectory(trajectory_id)
->data.global_pose;
}
if (it == data_.trajectory_nodes.EndOfTrajectory(trajectory_id)) {
return std::prev(data_.trajectory_nodes.EndOfTrajectory(trajectory_id))
->data.global_pose;
}
//插值求取local坐标在该时间节点的位姿
return transform::Interpolate(
transform::TimestampedTransform{std::prev(it)->data.time(),
std::prev(it)->data.global_pose},
transform::TimestampedTransform{it->data.time(),
it->data.global_pose},
time)
.transform;
}
//调用函数1
ConstIterator lower_bound(const int trajectory_id,
const common::Time time) const {
//如果该轨迹没有数据，我们返回下一个轨迹第一个数据的迭代器
if (SizeOfTrajectoryOrZero(trajectory_id) == 0) {
return EndOfTrajectory(trajectory_id);
}
//该轨迹有数据，取出其所有数据
const std::map<int, DataType>& trajectory =
trajectories_.at(trajectory_id).data_;
//如果最后一个数据都没有time这个时间节点晚，我们只能无奈的返回一个指向最后一个数据的迭代器
if (internal::GetTime(std::prev(trajectory.end())->second) < time) {
return EndOfTrajectory(trajectory_id);
}
//二分法找到指向稍稍比time这个时间节点晚一点的数据的迭代器
auto left = trajectory.begin();
auto right = std::prev(trajectory.end());
while (left != right) {
// This is never 'right' which is important to guarantee progress.
const int middle = left->first + (right->first - left->first) / 2;
// This could be 'right' in the presence of gaps, so we need to use the
// previous element in this case.
auto lower_bound_middle = trajectory.lower_bound(middle);
if (lower_bound_middle->first > middle) {
CHECK(lower_bound_middle != left);
lower_bound_middle = std::prev(lower_bound_middle);
}
if (internal::GetTime(lower_bound_middle->second) < time) {
left = std::next(lower_bound_middle);
} else {
right = lower_bound_middle;
}
}
return ConstIterator(*this, IdType{trajectory_id, left->first});
}
这里作者有一点疑问，local坐标系在全局坐标系中的位姿居然是可以随时间变化？有知道的大神还请不吝赐教。我们看到GetInterpolatedGlobalTrajectoryPose的参数不仅仅有轨迹的id，还带有一个时间，实际上就是求在这个时间节点处该轨迹对应的local坐标系的位姿。但是很遗憾，我们不能保证我们正好能存储到这个时间节点的local坐标系的位姿。cartographer的做法是先找到一个比这个时间节点稍稍后一点点的的位姿的迭代器即it，然后再使用prev算法作用在这个it上，通过一前以后两个时间节点local坐标系的插值求取出当前时间节点local坐标系的位姿。即transform::Interpolate函数的功能。
那这个it迭代器是怎么找到的呢？看看lower_bound这个函数。这个函数的一些重要的步骤都已经写在备注中了。
读者阅读上面的内容是可能会有疑问，我们总是说在某某地方查找某某节点的信息，查找某某轨迹的信息，这个到底是在哪查找的呢？它们又是以什么样的结构被存储起来的呢？下面我们就来回答这个问题
后端各种数据存储
其实后端有一个类成员——data_存储了后端大多数的信息，它的类型是PoseGraphData，我们来看看这个结构
struct PoseGraphData {
// Submaps get assigned an ID and state as soon as they are seen, even
// before they take part in the background computations.
// 存储所有的submap及与该submap有约束的节点id，且按轨迹分门别类的存储
MapById<SubmapId, InternalSubmapData> submap_data;
// Global submap poses currently used for displaying data.
// 各个submap在全局坐标系下的位姿
MapById<SubmapId, optimization::SubmapSpec2D> global_submap_poses_2d;
MapById<SubmapId, optimization::SubmapSpec3D> global_submap_poses_3d;
// Data that are currently being shown.
// 所有的节点及该节点在全局坐标中的位姿，它们各自的点云时间等信息
MapById<NodeId, TrajectoryNode> trajectory_nodes;
// Global landmark poses with all observations.（还没明白这个）
std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode>
landmark_nodes;
// How our various trajectories are related.（还没搞清楚）
TrajectoryConnectivityState trajectory_connectivity_state;
// 节点的个数
int num_trajectory_nodes = 0;
// 轨迹与轨迹的状态
std::map<int, InternalTrajectoryState> trajectories_state;
// Set of all initial trajectory poses.
//上一节使用的存储轨迹的初始pose，但它存储的并不是全局坐标系中的pose，而是该轨迹与其他别的轨迹的相对位姿关系
std::map<int, PoseGraph::InitialTrajectoryPose> initial_trajectory_poses;
// 所有的约束数据
std::vector<PoseGraphInterface::Constraint> constraints;
};
值得注意的是这里大量的使用了上一篇中所说的MapById这种数据结构，凡是使用了这种数据结构的，数据均会按轨迹分类存储。虽然我们现在还不知道数据是如何被加入这里的，但是相信，随着我们讨论的深入，我们会了解到这件事情的。
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
