续接上篇
上一篇我们讨论到了如何将一个求一个节点在global坐标系中的位姿。总结一下大致分成两种情况。一种是这个节点所属轨迹存在子图s了,我们通过wTs * sTl来求得该轨迹得local坐标系在global坐标系中得位姿,再通过wTl *lTn来求得节点再global坐标系中得位姿。另一种是没有找到这个节点所属得轨迹存储得子图,cartographer得做法是先看看这个轨迹和其他轨迹有没有相对位姿关系,如果没有只能返回单位矩阵(这里是指wTl),如果有就通过别的local坐标系与这个local坐标系得位姿求得这个local坐标系得位姿。
接下来我们往下看看cartographer得后续处理过程。
节点和子图得添加
先上代码:
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) {
// 将节点在local坐标系下的坐标转成global坐标系下的坐标
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
// 向节点列表加入节点,并得到节点的id
const NodeId node_id = AppendNode(constant_data, trajectory_id,
insertion_submaps, optimized_pose);
......//其他操作
}
//调用函数1
NodeId PoseGraph2D::AppendNode(
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) {
absl::MutexLock locker(&mutex_);
// 如果轨迹不存在, 则为轨迹添加连接性和采样器
AddTrajectoryIfNeeded(trajectory_id);
// 根据轨迹状态判断是否可以添加任务
if (!CanAddWorkItemModifying(trajectory_id)) {
LOG(WARNING) << "AddNode was called for finished or deleted trajectory.";
}
// 向节点列表中添加一个新的节点
const NodeId node_id = data_.trajectory_nodes.Append(
trajectory_id, TrajectoryNode{constant_data, optimized_pose});
++data_.num_trajectory_nodes;
......//其他操作
}
//调用函数2
void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id) {
// 如果不存在就添加,map的特性
data_.trajectories_state[trajectory_id];
CHECK(data_.trajectories_state.at(trajectory_id).state !=
TrajectoryState::FINISHED);
CHECK(data_.trajectories_state.at(trajectory_id).state !=
TrajectoryState::DELETED);
CHECK(data_.trajectories_state.at(trajectory_id).deletion_state ==
InternalTrajectoryState::DeletionState::NORMAL);
// 为轨迹添加连接状态
data_.trajectory_connectivity_state.Add(trajectory_id);
// Make sure we have a sampler for this trajectory.
// 添加采样器
if (!global_localization_samplers_[trajectory_id]) {
global_localization_samplers_[trajectory_id] =
absl::make_unique<common::FixedRatioSampler>(
options_.global_sampling_ratio());
}
}
// 轨迹的状态
struct InternalTrajectoryState {
enum class DeletionState {
NORMAL,
SCHEDULED_FOR_DELETION,
WAIT_FOR_DELETION
};
PoseGraphInterface::TrajectoryState state =
PoseGraphInterface::TrajectoryState::ACTIVE;
DeletionState deletion_state = DeletionState::NORMAL;
};
先从调用函数1说起,首先一步是先看看这个轨迹是不是以前被记录过,如果没有要先设置一下这个轨迹一些初始状态(即调用函数2),这些轨迹的状态的存储的地方是上一篇最后一节我们提到的std::map<int, InternalTrajectoryState> PoseGraphData::trajectories_state。从上述代码最后一个结构可以看出轨迹的状态有两个变量表示state和deletion_state,它们的初始值分别是ACTIVE和NORMAL,至于其他的状态怎么表示即是什么意思,相信后面我们会讨论到。在设置完轨迹的初始状态后,cartographer还为每个轨迹生成一个采样器(调用函数2的后半部分),至于这个采样器有啥用,作者目前还没有发现。
节点的生成和存储
上面稍微提了一下轨迹状态及采样器的事情,接下来我们回到调用函数1中“向节点列表中添加一个新的节点”这一行。可以看到这里调用的是PoseGraphData::trajectory_nodes中的类方法实现的,trajectory_nodes也是我们上一篇最后一节中提到的后端用来存储节点的地方。我们去看看Append这个函数是如何实现的:
IdType Append(const int trajectory_id, const DataType& data) {
CHECK_GE(trajectory_id, 0);
//先找到存储该轨迹数据的map
auto& trajectory = trajectories_[trajectory_id];
CHECK(trajectory.can_append_);
//注意这里是rbegin,是最后一个元素的迭代器,这里是找到最后一个元素的索引
const int index =
trajectory.data_.empty() ? 0 : trajectory.data_.rbegin()->first + 1;
//最后一个元素索引+1就是当前元素的索引。
trajectory.data_.emplace(index, data);
return IdType{trajectory_id, index};
}
这个存储结构是我们前面几篇多次遇见的MapById,这里只不过是一个我们之前没有遇见过的类方法而已。具体的过程就略过了。我们来看看在这里都存储了节点的哪些信息,我们看到的是一个TrajectoryNode的结构体:
struct TrajectoryNode {
common::Time time() const { return constant_data->time; }
std::shared_ptr<const Data> constant_data;
// 节点在global坐标系中的位姿
transform::Rigid3d global_pose;
};
其中这个Data是我们第15篇提到过的Data。
子图的存储
回到调用函数1,下面是这个函数被上面省略掉的部分:
NodeId PoseGraph2D::AppendNode(
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) {
......//生成添加节点的一些操作
if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
std::prev(data_.submap_data.EndOfTrajectory(trajectory_id))
->data.submap != insertion_submaps.back()) {
// We grow 'data_.submap_data' as needed. This code assumes that the first
// time we see a new submap is as 'insertion_submaps.back()'.
//为即将添加的submap先预留空间
const SubmapId submap_id =
data_.submap_data.Append(trajectory_id, InternalSubmapData());
// 将地图赋值 到新生成的 data_.submap_data.at(submap_id) 的 submap 中
data_.submap_data.at(submap_id).submap = insertion_submaps.back();
LOG(INFO) << "Inserted submap " << submap_id << ".";
kActiveSubmapsMetric->Increment();
}
return node_id;
}
struct InternalSubmapData {
std::shared_ptr<const Submap> submap;
SubmapState state = SubmapState::kNoConstraintSearch;
//与子图相关联的节点,这里的理解是这些个节点在前端时曾经被插入到submap中过
std::set<NodeId> node_ids;
};
首先一点我们可以看到,submap的存储同样时上篇中最后一节提到的PoseGraphData::submap_data。并不是任何时候都会去存储submap,根据那个if判断我们可以发现当该轨迹中没有子图的存储时要存submao,或者是insertion_submaps中第二个submap没有被储存过的话就要存储它。和上一小节类似,同样的使用Append的方法实现。这里我们看看存储的内容是写什么?首先是一根指向该submap的指针,还有就是一个set,里面记录了和该submap有关联的节点的id。什么是有关联,备注中写了作者自己的理解。
线程池
在看完AddNode计算节点在global中的位姿,如何添加节点和子图及后端如何存储这些节点子图后,我们继续来看看这个函数的后续操作
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) {
......//计算节点在global中的位姿
......//创建节点存储节点,存储子图
//看看旧的submap是否完成插入
const bool newly_finished_submap =
insertion_submaps.front()->insertion_finished();
// 把计算约束的工作加入线程池
AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
return ComputeConstraintsForNode(node_id, insertion_submaps,
newly_finished_submap);
});
return node_id;
}
这里我们遇到了一个陌生的东西——线程池。本节我们来讨论线程池,作者也是初学,有错误请指出。先看看AddWorkItem这个函数:
void PoseGraph2D::AddWorkItem(
const std::function<WorkItem::Result()>& work_item) {
absl::MutexLock locker(&work_queue_mutex_);
if (work_queue_ == nullptr) {
// work_queue_是指向deque<WorkItem>的指针
work_queue_ = absl::make_unique<WorkQueue>();
//貌似是先往线程池中添加一个任务
auto task = absl::make_unique<common::Task>();
task->SetWorkItem([this]() { DrainWorkQueue(); });
//thread_pool_的类型是指向ThreadPool的指针
thread_pool_->Schedule(std::move(task));
}
const auto now = std::chrono::steady_clock::now();
//将任务放入work_queue_队列中
work_queue_->push_back({now, work_item});
kWorkQueueSizeMetric->Set(work_queue_->size());
kWorkQueueDelayMetric->Set(
std::chrono::duration_cast<std::chrono::duration<double>>(
now - work_queue_->front().time)
.count());
}
可以看到这个函数的的参数有点点少见——std::function,这里作者推荐一篇博客:
https://blog.csdn.net/p942005405/article/details/84760715
在了解完function后我们跳过Task这个类,去直接看看线程池这个类,即ThreadPool。上代码:
class ThreadPool : public ThreadPoolInterface {
public:
explicit ThreadPool(int num_threads);
std::weak_ptr<Task> Schedule(std::unique_ptr<Task> task)
LOCKS_EXCLUDED(mutex_) override;
private:
void DoWork();
void NotifyDependenciesCompleted(Task* task) LOCKS_EXCLUDED(mutex_) override;
absl::Mutex mutex_;
// 结束线程的标志
bool running_ GUARDED_BY(mutex_) = true;
// 线程池
std::vector<std::thread> pool_ GUARDED_BY(mutex_);
std::deque<std::shared_ptr<Task>> task_queue_ GUARDED_BY(mutex_);
absl::flat_hash_map<Task*, std::shared_ptr<Task>> tasks_not_ready_
GUARDED_BY(mutex_);
};
先看类成员一来就是一个互斥量,接下来是一个标志位,然后一个vector里面装着很多线程thread,再是一个双向数组deque,里面装着很多Task的指针(从这个命名来看,想必这里面装的就是线程池要执行的任务了),最后一个是一个类似哈希表的东西,目前还不知道作用。
再去看看构造函数:
ThreadPool::ThreadPool(int num_threads) {
CHECK_GT(num_threads, 0) << "ThreadPool requires a positive num_threads!";
absl::MutexLock locker(&mutex_);
//依据传入线程的数量,在vector中创建对应数量的线程
for (int i = 0; i != num_threads; ++i) {
pool_.emplace_back([this]() { ThreadPool::DoWork(); });
}
}
可以看到这里使用lambda表达式去创建线程,且线程执行的是这个类的私有方法ThreadPool::DoWork(),我们去看看这个方法:
void ThreadPool::DoWork() {
//这里比较高深,貌似是linux系统的话要做什么检查,不过不太影响代码的阅读
#ifdef __linux__
// This changes the per-thread nice level of the current thread on Linux. We
// do this so that the background work done by the thread pool is not taking
// away CPU resources from more important foreground threads.
CHECK_NE(nice(10), -1);
#endif
//一个lambda表达式
const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
return !task_queue_.empty() || !running_;
};
// 一直循环
for (;;) {
std::shared_ptr<Task> task;
{
absl::MutexLock locker(&mutex_);
//作者对这里的理解是这个东西类似c++中的条件变量
mutex_.Await(absl::Condition(&predicate));
// 如果任务队列不为空, 那就取出第一个task
if (!task_queue_.empty()) {
task = std::move(task_queue_.front());
task_queue_.pop_front();
} else if (!running_) {
return;
}
}
CHECK(task);
CHECK_EQ(task->GetState(), common::Task::DEPENDENCIES_COMPLETED);
// 执行task
Execute(task.get());
}
先看看那个lambda表达式的意思,任务队列为空时返回false,标志位设置为停止时返回的时false。再看循环,那就容易理解了,当任务队列为空时无法上锁,线程被阻塞。当线程队列不为空时取出队列前面的Task。当标志位为false直接返回杀死线程。接下来我们先去看看Execute这个方法,再去看看Task这个类。这里值得注意的是Execute是ThreadPool这个类基类的类方法。
//Execute
void ThreadPoolInterface::Execute(Task* task) { task->Execute(); }
class Task {
public:
friend class ThreadPoolInterface;
using WorkItem = std::function<void()>;
enum State { NEW, DISPATCHED, DEPENDENCIES_COMPLETED, RUNNING, COMPLETED };
......//一堆类方法
private:
......//一些类方法
void Execute() LOCKS_EXCLUDED(mutex_);
......//一堆类方法
//注意这是std::function<void()>
WorkItem work_item_ GUARDED_BY(mutex_);
//应该是指向线程池的指针,基类指向派生类
ThreadPoolInterface* thread_pool_to_notify_ GUARDED_BY(mutex_) = nullptr;
//当前任务的状态
State state_ GUARDED_BY(mutex_) = NEW;
//未完成的依赖的数量
unsigned int uncompleted_dependencies_ GUARDED_BY(mutex_) = 0;
//本任务的依赖
std::set<Task*> dependent_tasks_ GUARDED_BY(mutex_);
absl::Mutex mutex_;
};
这个类没有一开始想象的那么简单,不仅仅只有一个function执行一下就好了,我们看到任务是带有依赖的,这些所谓的依赖也是一种Task,想必是要确保任务之间的相对顺序,必须执行完依赖的任务后才能执行本任务。同时也要注意任务的状态State。
我们再去看看Task的类方法Execute()
void Task::Execute() {
//检查依赖是否都完成,通过任务的转台
{
absl::MutexLock locker(&mutex_);
CHECK_EQ(state_, DEPENDENCIES_COMPLETED);
state_ = RUNNING;
}
// Execute the work item.
if (work_item_) {
work_item_();
}
absl::MutexLock locker(&mutex_);
//把任务的状态设置为完成状态
state_ = COMPLETED;
// 通知依赖本任务的其他任务, 本任务执行完了
for (Task* dependent_task : dependent_tasks_) {
dependent_task->OnDependenyCompleted();
}
}
这个类的其他函数就暂时不介绍了,不过它使用到了一种指针weak_ptr,这里作者提供一篇文章以供学习:
http://c.biancheng.net/view/7918.html
我们继续返回去看看线程池这个类的类方法——Schedule:
std::weak_ptr<Task> ThreadPool::Schedule(std::unique_ptr<Task> task) {
std::shared_ptr<Task> shared_task;
{
absl::MutexLock locker(&mutex_);
// map::insert() 会返回pair<map::iterator,bool> 类型,
// 第一个值为迭代器, 第二个值为插入操作是否成功
auto insert_result =
tasks_not_ready_.insert(std::make_pair(task.get(), std::move(task)));
CHECK(insert_result.second) << "Schedule called twice";
shared_task = insert_result.first->second;
}
//设置Task类中线程池的指针
SetThreadPool(shared_task.get());
return shared_task;
}
可见任务被加入线程池中时并不是直接加入task_queue_中等待执行,可能时因为我们之前所说的每个任务都可能有自己的依赖任务。它们先是被加入tasks_not_ready_中。再来看看另一个类方法NotifyDependenciesCompleted:
void ThreadPool::NotifyDependenciesCompleted(Task* task) {
absl::MutexLock locker(&mutex_);
// 找到task的索引
auto it = tasks_not_ready_.find(task);
CHECK(it != tasks_not_ready_.end());
// 加入到任务队列中
task_queue_.push_back(it->second);
// 从tasks_not_ready_中删除task
tasks_not_ready_.erase(it);
}
可以看到NotifyDependenciesCompleted才会把任务从tasks_not_ready_转移到task_queue_中等待线程的执行。想必时等待任务的依赖执行完后才会调用这个函数。
好了本篇就到这里了。