全局优化
SLAM过程全局优化基本原理，参考：https://zhuanlan.zhihu.com/p/50055546
：第i个Submap相对于世界坐标系的位姿，其中上角标
表示这是Submap的位姿,
;
：第j个Scan相对于世界坐标系的位姿，其中上角标
表示这是Scan的位姿，
;
:第j个Scan插入第i个Submap的相对位姿。
: 第j个Scan插入第i个Submap的质量。可以理解为对ScanMatching效果的一个评分值。
那么，我们可以把 看做是约束(Constraints)，我们的总体目标就是优化
和
这两类量，使总体的误差最小。其中，误差函数的定义过程如下：
首先，考虑其中一对儿约束，对于 、
和
，定义误差为：
ceres使用说明：https://blog.csdn.net/u011178262/article/details/88774577
简单理解，node-submap-odom等都有相互关系，全局优化optimization_problem，要综合计算这些约束之间关系，最终通过调整node和submap的位置，让这些约束相互妥协，实现综合最小误差。这个原理，就像下面通过最小二乘法，调节直线参数位置，拟合直线，让每个约束互相妥协，达到最小误差。
因此，OptimizationProblem2D类主要有两种函数：
AddResidualBlock（）加入约束信息，solve（）计算优化结果。
AddResidualBlock(cost_funcfution, loss_function, &x)
其中：cost_funcfution,误差函数，loss_function（可以为NULL） x，为cost_function 待优化变量的初始值。比如，在优化submap和node（scan）位置的过程中，x为submap和scan的初始值，cost_function与下面e有关。
因此，关于node和submap，AddResidualBlock相关程序如下。
// Add cost functions for intra- and inter-submap constraints.
for (const Constraint& constraint : constraints) {
problem.AddResidualBlock(
CreateAutoDiffSpaCostFunction(constraint.pose),
// Loop closure constraints should have a loss function.
constraint.tag == Constraint::INTER_SUBMAP
? new ceres::HuberLoss(options_.huber_scale())
: nullptr,
C_submaps.at(constraint.submap_id).data(),
C_nodes.at(constraint.node_id).data());
}
cartographer/mapping/internal/optimization/optimization_problem_2d.cc
namespace cartographer {
namespace mapping {
namespace optimization {
struct NodeSpec2D {
common::Time time;
transform::Rigid2d local_pose_2d;
transform::Rigid2d global_pose_2d;
Eigen::Quaterniond gravity_alignment; //重力校准
};
struct SubmapSpec2D {
transform::Rigid2d global_pose;
};
class OptimizationProblem2D
: public OptimizationProblemInterface<NodeSpec2D, SubmapSpec2D,
transform::Rigid2d> {
public:
explicit OptimizationProblem2D(
const optimization::proto::OptimizationProblemOptions& options);
// add data OptimizationProblem2D needed 把与优化相关的数据加入OptimizationProblem2D类中
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override;
void AddOdometryData(int trajectory_id,
const sensor::OdometryData& odometry_data) override;
void AddTrajectoryNode(int trajectory_id,
const NodeSpec2D& node_data) override;
void InsertTrajectoryNode(const NodeId& node_id,
const NodeSpec2D& node_data) override;
void TrimTrajectoryNode(const NodeId& node_id) override;
void AddSubmap(int trajectory_id,
const transform::Rigid2d& global_submap_pose) override;
void InsertSubmap(const SubmapId& submap_id,
const transform::Rigid2d& global_submap_pose) override;
void TrimSubmap(const SubmapId& submap_id) override;
void SetMaxNumIterations(int32 max_num_iterations) override;
// from constrains of the data ,AddResidualBlock() and solve
// 根据数据之间的约束关系，加入costfunction函数，并优化
void Solve(
const std::vector<Constraint>& constraints,
const std::map<int, PoseGraphInterface::TrajectoryState>&
trajectories_state,
const std::map<std::string, LandmarkNode>& landmark_nodes) override;
const MapById<NodeId, NodeSpec2D>& node_data() const override {
return node_data_;
}
const MapById<SubmapId, SubmapSpec2D>& submap_data() const override {
return submap_data_;
}
const std::map<std::string, transform::Rigid3d>& landmark_data()
const override {
return landmark_data_;
}
const sensor::MapByTime<sensor::ImuData>& imu_data() const override {
return imu_data_;
}
const sensor::MapByTime<sensor::OdometryData>& odometry_data()
const override {
return odometry_data_;
}
private:
std::unique_ptr<transform::Rigid3d> InterpolateOdometry(
int trajectory_id, common::Time time) const;
// Computes the relative pose between two nodes based on odometry data.
std::unique_ptr<transform::Rigid3d> CalculateOdometryBetweenNodes(
int trajectory_id, const NodeSpec2D& first_node_data,
const NodeSpec2D& second_node_data) const;
optimization::proto::OptimizationProblemOptions options_;
MapById<NodeId, NodeSpec2D> node_data_;
MapById<SubmapId, SubmapSpec2D> submap_data_;
std::map<std::string, transform::Rigid3d> landmark_data_;
sensor::MapByTime<sensor::ImuData> imu_data_;
sensor::MapByTime<sensor::OdometryData> odometry_data_;
};
} // namespace optimization
} // namespace mapping
} // namespace cartographer
/cartographer/mapping/internal/2d/pose_graph.cc
主要函数如下：
总结：通过addNode等主要函数，计算node和submap之间constraint并传给optimization_problem_，optimization_problem_根据要求阶段性的进行优化。当然optimization_problem_优化还可能用到imu和odom数据（addImuData，addOdomData），通过相关函数加入到optimization_problem。
其中ComputeConstraintsForNode以及runOptimization等工作通过压入work_queue，通过thread_pool处理。
// AddNode的主要工作是把一个TrajectoryNode的数据加入到PoseGraph维护的trajectory_nodes_这个容器中。并返回加入的节点的Node.
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) {
// 生成一个新的Pose，这个Pose是由trajectory相对于世界坐标系的Pose乘以节点数据中包含的该节点相对于该trajectory的LocalTrajectoryBuilder Pose
// 所以生成的optimized_pose就是该节点的绝对位姿
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
common::MutexLocker locker(&mutex_);
// 必要时新增一条trajectory。怎么判断是否必要，查看AddTrajectoryIfNeeded的代码
AddTrajectoryIfNeeded(trajectory_id);
// TrajectoryNode定义在/mapping/id.h.解读见：https://zhuanlan.zhihu.com/p/48790984
// 把该节点加入到PoseGraph2D维护的trajectory_nodes_这个容器中
const NodeId node_id = trajectory_nodes_.Append(
trajectory_id, TrajectoryNode{constant_data, optimized_pose});
// 节点数加1.
++num_trajectory_nodes_;
// 跟上一个情况类似，如果submap_data_还没有元素或最后一个元素不等于insertion_submaps
// 的第二个元素
// 说明insertion_submaps的第二个元素还不被PoseGraph所知。
if (submap_data_.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
std::prev(submap_data_.EndOfTrajectory(trajectory_id))->data.submap !=
insertion_submaps.back()) {
// We grow 'submap_data_' as needed. This code assumes that the first
// time we see a new submap is as 'insertion_submaps.back()'.
// 生成一个SubmapId,把insertion_submaps的第二个元素加到submap_data_中
const SubmapId submap_id =
submap_data_.Append(trajectory_id, InternalSubmapData());
submap_data_.at(submap_id).submap = insertion_submaps.back();
}
// We have to check this here, because it might have changed by the time we
// execute the lambda.
// 检查insertion_submaps的第一个submap是否已经被finished了。
// 如果被finished，那么我们需要计算一下约束并且进行一下全局优化了。
// 这里是把这个工作交到了workItem中等待处理
const bool newly_finished_submap = insertion_submaps.front()->finished();
AddWorkItem([=]() REQUIRES(mutex_) {
ComputeConstraintsForNode(node_id, insertion_submaps,
newly_finished_submap);
});
// 上述都做完了，返回node_id.
return node_id;
}
void PoseGraph2D::AddWorkItem(
const std::function<WorkItem::Result()>& work_item) {
absl::MutexLock locker(&work_queue_mutex_);
if (work_queue_ == nullptr) {
work_queue_ = absl::make_unique<WorkQueue>();
auto task = absl::make_unique<common::Task>();
task->SetWorkItem([this]() { DrainWorkQueue(); });
thread_pool_->Schedule(std::move(task));
}
const auto now = std::chrono::steady_clock::now();
work_queue_->push_back({now, work_item});
kWorkQueueDelayMetric->Set(
std::chrono::duration_cast<std::chrono::duration<double>>(
now - work_queue_->front().time)
.count());
}
// imu 数据加入到 optimization_problem_
void PoseGraph2D::AddImuData(const int trajectory_id,
const sensor::ImuData& imu_data) {
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->AddImuData(trajectory_id, imu_data);
}
return WorkItem::Result::kDoNotRunOptimization;
});
}
// odometry_data 数据加入到 optimization_problem_
void PoseGraph2D::AddOdometryData(const int trajectory_id,
const sensor::OdometryData& odometry_data) {
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) {
optimization_problem_->AddOdometryData(trajectory_id, odometry_data);
}
return WorkItem::Result::kDoNotRunOptimization;
});
}
//AddFixedFramePoseData是处理GPS等能够测量机器人完整位姿的传感器输入，
//目前2D情况下并没有用到该函数。
void PoseGraph2D::AddFixedFramePoseData(
const int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) {
LOG(FATAL) << "Not yet implemented for 2D.";
}
void PoseGraph2D::AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data) {
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
if (CanAddWorkItemModifying(trajectory_id)) {
//根据数据生成一个LandmarkNode::LandmarkObservation，压入landmark_nodes_这个容器中
for (const auto& observation : landmark_data.landmark_observations) {
data_.landmark_nodes[observation.id].landmark_observations.emplace_back(
PoseGraphInterface::LandmarkNode::LandmarkObservation{
trajectory_id, landmark_data.time,
observation.landmark_to_tracking_transform,
observation.translation_weight, observation.rotation_weight});
}
}
return WorkItem::Result::kDoNotRunOptimization;
});
}
// 计算node和submap之间的约束
void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
const SubmapId& submap_id) {
...
}
// 三个参数，一个是刚加入的节点ID；一个是Local Slam返回的insertion_submaps；
// 一个是是否有新finished的submap的判断标志
// 四步：
//1.把该节点的信息加入到OptimizationProblem中，方便进行优化
//2.计算节点和新加入的submap之间的约束
//3.计算其它submap与节点之间约束
//4.计算新的submap和旧的节点的约束
void PoseGraph2D::ComputeConstraintsForNode(
const NodeId& node_id,
std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
const bool newly_finished_submap) {
// 获取节点数据
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
// 根据节点数据的时间获取最新的submap的id
const std::vector<SubmapId> submap_ids = InitializeGlobalSubmapPoses(
node_id.trajectory_id, constant_data->time, insertion_submaps);
CHECK_EQ(submap_ids.size(), insertion_submaps.size());// 检查两者大小是否相等
// 获取这两个submap中前一个的id
const SubmapId matching_id = submap_ids.front();
// 计算该Node经过重力align后的相对位姿，即在submap中的位姿
const transform::Rigid2d local_pose_2d = transform::Project2D(
constant_data->local_pose *
transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
// 计算该Node在世界坐标系中的绝对位姿；但中间为啥要乘一个constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse()呢？
const transform::Rigid2d global_pose_2d =
optimization_problem_->submap_data().at(matching_id).global_pose *
constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
local_pose_2d;//该submap的绝对位姿乘以在相应submap的相对位姿不就可以了吗？中间那一项是啥呢？
// 把该节点的信息加入到OptimizationProblem中，方便进行优化
optimization_problem_->AddTrajectoryNode(
matching_id.trajectory_id,
optimization::NodeSpec2D{constant_data->time, local_pose_2d,
global_pose_2d,
constant_data->gravity_alignment});
//遍历处理每一个insertion_submaps
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const SubmapId submap_id = submap_ids[i];
// Even if this was the last node added to 'submap_id', the submap will
// only be marked as finished in 'submap_data_' further below.
CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);//检查指定id是否是kActive
// 加入到PoseGraph维护的容器中
submap_data_.at(submap_id).node_ids.emplace(node_id);
// 计算相对位姿
const transform::Rigid2d constraint_transform =
constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
local_pose_2d;
// 把约束压入约束集合中
constraints_.push_back(Constraint{submap_id,
node_id,
{transform::Embed3D(constraint_transform),
options_.matcher_translation_weight(),
options_.matcher_rotation_weight()},
Constraint::INTRA_SUBMAP});
}
// 遍历历史中的submap，计算新的Node与每个submap的约束
for (const auto& submap_id_data : submap_data_) {
if (submap_id_data.data.state == SubmapState::kFinished) {//确认submap已经被finished了
CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);//检查该submap中还没有跟该节点产生约束
ComputeConstraint(node_id, submap_id_data.id);//计算该节点与submap的约束
}
}
// 如果有新的刚被finished的submap
if (newly_finished_submap) {
//insertion_maps中的第一个是Old的那个，如果有刚被finished的submap，那一定是他
const SubmapId finished_submap_id = submap_ids.front();
// 获取该submap的数据
InternalSubmapData& finished_submap_data =
submap_data_.at(finished_submap_id);
// 检查它还是不是kActive。
CHECK(finished_submap_data.state == SubmapState::kActive);
// 把它设置成finished
finished_submap_data.state = SubmapState::kFinished;
// We have a new completed submap, so we look into adding constraints for
// old nodes.
//计算新的submap和旧的节点的约束
ComputeConstraintsForOldNodes(finished_submap_id);
}
// 结束构建约束
constraint_builder_.NotifyEndOfNode();
// 计数器加1
absl::MutexLock locker(&mutex_);
++num_nodes_since_last_loop_closure_;
if (options_.optimize_every_n_nodes() > 0 &&
num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes()) {
return WorkItem::Result::kRunOptimization;
}
return WorkItem::Result::kDoNotRunOptimization;
}
// 获取该node和该submap中的node中较新的时间
common::Time PoseGraph2D::GetLatestNodeTime(const NodeId& node_id,
const SubmapId& submap_id) const {
// 获取指定id的时间
common::Time time = data_.trajectory_nodes.at(node_id).constant_data->time;
// 获取指定id的submap的数据
const InternalSubmapData& submap_data = data_.submap_data.at(submap_id);
if (!submap_data.node_ids.empty()) {//如果该submap相关的node_ids不为空。
// 获取指定id的submap相关的node_ids列表中的最后一个元素
const NodeId last_submap_node_id =
*data_.submap_data.at(submap_id).node_ids.rbegin(); //c.rbegin() 返回一个逆序迭代器，它指向容器c的最后一个元素
// 把时间更新为节点建立时间与submap中最后一个节点时间中较晚的那个
time = std::max(
time,
data_.trajectory_nodes.at(last_submap_node_id).constant_data->time);
}
return time;
}
// 在每有一条新的约束constraint加入时，用来更新不同trajectory之间的connectivity的关系。
void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
//检查constraint的标签类型是否是INTER_SUBMAP类型.
CHECK_EQ(constraint.tag, Constraint::INTER_SUBMAP);
//根据constraint中产生约束的一对儿node_id和submap_id获取两者上次建立约束的最晚时间
const common::Time time =
GetLatestNodeTime(constraint.node_id, constraint.submap_id);
//对两者所在trajectory_id建立connectivity
trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
constraint.submap_id.trajectory_id,
time);
}
// 输入是由ConstraintBuilder2D建立起来的约束向量
void PoseGraph2D::HandleWorkQueue(
const constraints::ConstraintBuilder2D::Result& result) {
{
// 在处理数据时加上互斥锁，防止出现数据访问错误
absl::MutexLock locker(&mutex_);
// 把result中的所有约束加入到constraints_向量的末尾处
data_.constraints.insert(data_.constraints.end(), result.begin(),
result.end());
//data_.constraints 是RunOptimization() 依据
}
// 执行优化。这里调用了PoseGraph2D的另一个成员函数RunOptimization()来处理
RunOptimization();
// 如果已经设置了全局优化的回调函数。进行一些参数设置后调用该函数。
if (global_slam_optimization_callback_) {
// 设置回调函数的两个参数
std::map<int, NodeId> trajectory_id_to_last_optimized_node_id;
std::map<int, SubmapId> trajectory_id_to_last_optimized_submap_id;
{
absl::MutexLock locker(&mutex_);
const auto& submap_data = optimization_problem_->submap_data();
const auto& node_data = optimization_problem_->node_data();
// 把optimization_problem_中的node和submap的数据拷贝到两个参数中
for (const int trajectory_id : node_data.trajectory_ids()) {
if (node_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
continue;
}
trajectory_id_to_last_optimized_node_id.emplace(
trajectory_id,
std::prev(node_data.EndOfTrajectory(trajectory_id))->id);
trajectory_id_to_last_optimized_submap_id.emplace(
trajectory_id,
std::prev(submap_data.EndOfTrajectory(trajectory_id))->id);
}
}
// 调用该回调函数进行处理。猜测应该是更新一下地图结果之类的操作。
global_slam_optimization_callback_(
trajectory_id_to_last_optimized_submap_id,
trajectory_id_to_last_optimized_node_id);
}
{
// 更新trajectory之间的connectivity信息
absl::MutexLock locker(&mutex_);
for (const Constraint& constraint : result) {
UpdateTrajectoryConnectivity(constraint);
}
DeleteTrajectoriesIfNeeded();
// 调用trimmers_中每一个trimmer的Trim函数进行处理。但还是不清楚这个trimming的含义是什么。
TrimmingHandle trimming_handle(this);
for (auto& trimmer : trimmers_) {
trimmer->Trim(&trimming_handle);
}
// Trim之后把他们从trimmers_这个向量中清除，trimmers_将重新记录等待新一轮的Loop Closure过程中产生的数据
trimmers_.erase(
std::remove_if(trimmers_.begin(), trimmers_.end(),
[](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
return trimmer->IsFinished();
}),
trimmers_.end());
// 重新把“上次Loop Closure之后新加入的节点数”置为0，run_loop_closure置为false
num_nodes_since_last_loop_closure_ = 0;
// Update the gauges that count the current number of constraints.
double inter_constraints_same_trajectory = 0;
double inter_constraints_different_trajectory = 0;
for (const auto& constraint : data_.constraints) {
if (constraint.tag ==
cartographer::mapping::PoseGraph::Constraint::INTRA_SUBMAP) {
continue;
}
if (constraint.node_id.trajectory_id ==
constraint.submap_id.trajectory_id) {
++inter_constraints_same_trajectory;
} else {
++inter_constraints_different_trajectory;
}
}
kConstraintsSameTrajectoryMetric->Set(inter_constraints_same_trajectory);
kConstraintsDifferentTrajectoryMetric->Set(
inter_constraints_different_trajectory);
}
DrainWorkQueue();
}
// 处理workqueue中的task
void PoseGraph2D::DrainWorkQueue() {
bool process_work_queue = true;
size_t work_queue_size;
while (process_work_queue) {
std::function<WorkItem::Result()> work_item;
{
absl::MutexLock locker(&work_queue_mutex_);
if (work_queue_->empty()) {
work_queue_.reset();
return;
}
work_item = work_queue_->front().task;
work_queue_->pop_front();
work_queue_size = work_queue_->size();
}
process_work_queue = work_item() == WorkItem::Result::kDoNotRunOptimization;
}
LOG(INFO) << "Remaining work items in queue: " << work_queue_size;
// We have to optimize again.
constraint_builder_.WhenDone(
[this](const constraints::ConstraintBuilder2D::Result& result) {
HandleWorkQueue(result);
});
}
//建图后最终优化
void PoseGraph2D::RunFinalOptimization() {
{
//该任务仅仅起到参数设置作用
AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
optimization_problem_->SetMaxNumIterations(
options_.max_num_final_iterations());
return WorkItem::Result::kRunOptimization;
});
AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) {
absl::MutexLock locker(&mutex_);
optimization_problem_->SetMaxNumIterations(
options_.optimization_problem_options()
.ceres_solver_options()
.max_num_iterations());
return WorkItem::Result::kDoNotRunOptimization;
});
}
WaitForAllComputations();
}
void PoseGraph2D::RunOptimization() {
if (optimization_problem_->submap_data().empty()) {
return;
}
// No other thread is accessing the optimization_problem_,
// data_.constraints, data_.frozen_trajectories and data_.landmark_nodes
// when executing the Solve. Solve is time consuming, so not taking the mutex
// before Solve to avoid blocking foreground processing.
optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
data_.landmark_nodes);
absl::MutexLock locker(&mutex_);
...
}
// 该函数的主要工作就是指定一个trajectory_id的情况下，返回当前正处于活跃状态下的submap的id，
// 也就是系统正在维护的insertion_submaps的两个submap的id。insertion_submaps可能为空，
// 也可能当前只有一个元素，也可能已经有两个元素了。
std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
const int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {
CHECK(!insertion_submaps.empty());
// 返回OptimizationProblem2D的成员变量： MapById<SubmapId, SubmapSpec2D> submap_data_;
// submap_data_中存的就是以SubmapId为key values值管理的所有Submap的全局位姿
const auto& submap_data = optimization_problem_->submap_data();
if (insertion_submaps.size() == 1) {
// If we don't already have an entry for the first submap, add one.
// SizeOfTrajectoryOrZero: 返回一个指定Id的元素（这里的元素是SubmapSpec2D）的数据的size，如果该id不存在，则返回0
// 如果判断指定id的submap_data的size为0，说明该trajectory_id上还没有submap数据，那么就需要建立一个submap
// If we don't already have an entry for the first submap, add one.
if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {
if (data_.initial_trajectory_poses.count(trajectory_id) > 0) {
// TrajectoryConnectivityState 定义在/mapping/internal/trajectory_connectivity_state.h中
// initial_trajectory_poses_中存的是一系列带有int型id的InitialTrajectoryPose，
// 一个InitialTrajectoryPose定义了与该trajectory相关联的trajectory_id的值。
// 因此，下面这句是把该trajectory_id与其应该关联的id(存在InitialTrajectoryPose的to_trajectory_id中)关联起来
trajectory_connectivity_state_.Connect( //把这个表里数据与指定id关联
trajectory_id,
data_.initial_trajectory_poses.at(trajectory_id).to_trajectory_id,
time);
}
// 将该submap的global pose加入到optimization_problem_中，方便以后优化
optimization_problem_->AddSubmap(
trajectory_id,
transform::Project2D(ComputeLocalToGlobalTransform(
global_submap_poses_, trajectory_id) *
insertion_submaps[0]->local_pose()));
}
// 看一下存submap的这个容器的size是否确实等于1
CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
// 因为是第一个submap，所以该submap的ID是trajectory_id+0，其中0是submap的index.
const SubmapId submap_id{trajectory_id, 0};
// 检查这个SubmapId下的submap是否等于insertion_submaps的第一个元素。因为我们初始化第一个submap肯定是要被插入的那个submap
CHECK(submap_data_.at(submap_id).submap == insertion_submaps.front());
// 如果是第一个submap，那么把刚刚建立的submap的id返回
return {submap_id};
}
CHECK_EQ(2, insertion_submaps.size());//检查insertion_submaps的size等于2.
// 获取submap_data的末尾trajectory_id
const auto end_it = submap_data.EndOfTrajectory(trajectory_id);
// 检查开头是否不等于末尾。如果等于，说明这个容器里没有一个元素
CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);
// end_it并没有实际的元素，所以它之前的一个submap的id就是submap_data中的最后一个元素
const SubmapId last_submap_id = std::prev(end_it)->id;
// 如果是等于insertion_submaps的第一个元素，说明insertion_submaps的第二个元素还没有分配了id
if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) {
// In this case, 'last_submap_id' is the ID of
// 'insertions_submaps.front()' and 'insertions_submaps.back()' is new.
// 这种情况下，要给新的submap分配id，并把它加到OptimizationProblem的submap_data_这个容器中
const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;
optimization_problem_->AddSubmap(//解算新的submap的pose插入到OptimizationProblem2D::submap_data_中
trajectory_id,
//然后把该pose乘以insertion_submaps中第一个pose的逆再乘第二个pose
first_submap_pose *
constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
constraints::ComputeSubmapPose(*insertion_submaps[1]));
// 返回一个包含两个Submap的向量。
return {last_submap_id,
SubmapId{trajectory_id, last_submap_id.submap_index + 1}};
}
// 如果是等于insertion_submaps的第二个元素，
// 说明insertion_submaps的第二个元素也已经分配了id并加入到了OptimizationProblem的submap_data_中
CHECK(submap_data_.at(last_submap_id).submap == insertion_submaps.back());
// 生成一个submap的Id, 这个id是submap_data_的倒数第二个。
const SubmapId front_submap_id{trajectory_id,
last_submap_id.submap_index - 1};
// 检查倒数第二个是否等于insertion_submaps的第一个。
CHECK(submap_data_.at(front_submap_id).submap == insertion_submaps.front());
// 把这两个submap的id返回 AddNode
return {front_submap_id, last_submap_id};
} namespace cartographer