12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Rename mapping_2d::PoseGraph to mapping::PoseGraph2D. (#917) 

[rCode structure RFC](e11bca586f/text/0000-code-structure.md)
master
Alexander Belyaev 2018-02-19 20:01:29 +01:00 committed by GitHub
parent a338b2e339
commit 43544f0fbc
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 263 additions and 277 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
cartographer/mapping/local_slam_result_data.cc
View File

@ -20,53 +20,52 @@
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping {
LocalSlamResultData::LocalSlamResultData(const std::string &sensor_id, LocalSlamResultData::LocalSlamResultData(const std::string& sensor_id,
common::Time time) common::Time time)
: Data(sensor_id), time_(time) {} : Data(sensor_id), time_(time) {}
LocalSlamResult2D::LocalSlamResult2D( LocalSlamResult2D::LocalSlamResult2D(
const std::string &sensor_id, common::Time time, const std::string& sensor_id, common::Time time,
std::shared_ptr<const mapping::TrajectoryNode::Data> node_data, std::shared_ptr<const mapping::TrajectoryNode::Data> node_data,
const std::vector<std::shared_ptr<const mapping_2d::Submap>> const std::vector<std::shared_ptr<const mapping_2d::Submap>>&
&insertion_submaps) insertion_submaps)
: LocalSlamResultData(sensor_id, time), : LocalSlamResultData(sensor_id, time),
node_data_(node_data), node_data_(node_data),
insertion_submaps_(insertion_submaps) {} insertion_submaps_(insertion_submaps) {}
void LocalSlamResult2D::AddToTrajectoryBuilder( void LocalSlamResult2D::AddToTrajectoryBuilder(
mapping::TrajectoryBuilderInterface *const trajectory_builder) { mapping::TrajectoryBuilderInterface* const trajectory_builder) {
trajectory_builder->AddLocalSlamResultData( trajectory_builder->AddLocalSlamResultData(
common::make_unique<LocalSlamResult2D>(*this)); common::make_unique<LocalSlamResult2D>(*this));
} }
void LocalSlamResult2D::AddToPoseGraph(int trajectory_id, void LocalSlamResult2D::AddToPoseGraph(int trajectory_id,
mapping::PoseGraph *pose_graph) const { mapping::PoseGraph* pose_graph) const {
DCHECK(dynamic_cast<mapping_2d::PoseGraph *>(pose_graph)); DCHECK(dynamic_cast<PoseGraph2D*>(pose_graph));
mapping_2d::PoseGraph *pose_graph_2d = PoseGraph2D* pose_graph_2d = static_cast<PoseGraph2D*>(pose_graph);
static_cast<mapping_2d::PoseGraph *>(pose_graph);
pose_graph_2d->AddNode(node_data_, trajectory_id, insertion_submaps_); pose_graph_2d->AddNode(node_data_, trajectory_id, insertion_submaps_);
} }
LocalSlamResult3D::LocalSlamResult3D( LocalSlamResult3D::LocalSlamResult3D(
const std::string &sensor_id, common::Time time, const std::string& sensor_id, common::Time time,
std::shared_ptr<const mapping::TrajectoryNode::Data> node_data, std::shared_ptr<const mapping::TrajectoryNode::Data> node_data,
const std::vector<std::shared_ptr<const mapping_3d::Submap>> const std::vector<std::shared_ptr<const mapping_3d::Submap>>&
&insertion_submaps) insertion_submaps)
: LocalSlamResultData(sensor_id, time), : LocalSlamResultData(sensor_id, time),
node_data_(node_data), node_data_(node_data),
insertion_submaps_(insertion_submaps) {} insertion_submaps_(insertion_submaps) {}
void LocalSlamResult3D::AddToTrajectoryBuilder( void LocalSlamResult3D::AddToTrajectoryBuilder(
mapping::TrajectoryBuilderInterface *const trajectory_builder) { mapping::TrajectoryBuilderInterface* const trajectory_builder) {
trajectory_builder->AddLocalSlamResultData( trajectory_builder->AddLocalSlamResultData(
common::make_unique<LocalSlamResult3D>(*this)); common::make_unique<LocalSlamResult3D>(*this));
} }
void LocalSlamResult3D::AddToPoseGraph(int trajectory_id, void LocalSlamResult3D::AddToPoseGraph(int trajectory_id,
mapping::PoseGraph *pose_graph) const { mapping::PoseGraph* pose_graph) const {
DCHECK(dynamic_cast<mapping_3d::PoseGraph *>(pose_graph)); DCHECK(dynamic_cast<mapping_3d::PoseGraph*>(pose_graph));
mapping_3d::PoseGraph *pose_graph_3d = mapping_3d::PoseGraph* pose_graph_3d =
static_cast<mapping_3d::PoseGraph *>(pose_graph); static_cast<mapping_3d::PoseGraph*>(pose_graph);
pose_graph_3d->AddNode(node_data_, trajectory_id, insertion_submaps_); pose_graph_3d->AddNode(node_data_, trajectory_id, insertion_submaps_);
} }

26
cartographer/mapping/local_slam_result_data.h
View File

@ -17,7 +17,7 @@
#ifndef CARTOGRAPHER_MAPPING_LOCAL_SLAM_RESULT_DATA_H #ifndef CARTOGRAPHER_MAPPING_LOCAL_SLAM_RESULT_DATA_H
#define CARTOGRAPHER_MAPPING_LOCAL_SLAM_RESULT_DATA_H #define CARTOGRAPHER_MAPPING_LOCAL_SLAM_RESULT_DATA_H
#include "cartographer/mapping_2d/pose_graph.h" #include "cartographer/mapping_2d/pose_graph_2d.h"
#include "cartographer/mapping_3d/pose_graph.h" #include "cartographer/mapping_3d/pose_graph.h"
#include "cartographer/sensor/data.h" #include "cartographer/sensor/data.h"
@ -27,11 +27,11 @@ namespace mapping {
class TrajectoryBuilderInterface; class TrajectoryBuilderInterface;
class LocalSlamResultData : public cartographer::sensor::Data { class LocalSlamResultData : public cartographer::sensor::Data {
public: public:
LocalSlamResultData(const std::string &sensor_id, common::Time time); LocalSlamResultData(const std::string& sensor_id, common::Time time);
common::Time GetTime() const override { return time_; } common::Time GetTime() const override { return time_; }
virtual void AddToPoseGraph(int trajectory_id, virtual void AddToPoseGraph(int trajectory_id,
mapping::PoseGraph *pose_graph) const = 0; mapping::PoseGraph* pose_graph) const = 0;
private: private:
common::Time time_; common::Time time_;
@ -40,15 +40,15 @@ class LocalSlamResultData : public cartographer::sensor::Data {
class LocalSlamResult2D : public LocalSlamResultData { class LocalSlamResult2D : public LocalSlamResultData {
public: public:
LocalSlamResult2D( LocalSlamResult2D(
const std::string &sensor_id, common::Time time, const std::string& sensor_id, common::Time time,
std::shared_ptr<const mapping::TrajectoryNode::Data> node_data, std::shared_ptr<const mapping::TrajectoryNode::Data> node_data,
const std::vector<std::shared_ptr<const mapping_2d::Submap>> const std::vector<std::shared_ptr<const mapping_2d::Submap>>&
&insertion_submaps); insertion_submaps);
void AddToTrajectoryBuilder( void AddToTrajectoryBuilder(
mapping::TrajectoryBuilderInterface *const trajectory_builder) override; mapping::TrajectoryBuilderInterface* const trajectory_builder) override;
void AddToPoseGraph(int trajectory_id, void AddToPoseGraph(int trajectory_id,
mapping::PoseGraph *pose_graph) const override; mapping::PoseGraph* pose_graph) const override;
private: private:
std::shared_ptr<const mapping::TrajectoryNode::Data> node_data_; std::shared_ptr<const mapping::TrajectoryNode::Data> node_data_;
@ -58,15 +58,15 @@ class LocalSlamResult2D : public LocalSlamResultData {
class LocalSlamResult3D : public LocalSlamResultData { class LocalSlamResult3D : public LocalSlamResultData {
public: public:
LocalSlamResult3D( LocalSlamResult3D(
const std::string &sensor_id, common::Time time, const std::string& sensor_id, common::Time time,
std::shared_ptr<const mapping::TrajectoryNode::Data> node_data, std::shared_ptr<const mapping::TrajectoryNode::Data> node_data,
const std::vector<std::shared_ptr<const mapping_3d::Submap>> const std::vector<std::shared_ptr<const mapping_3d::Submap>>&
&insertion_submaps); insertion_submaps);
void AddToTrajectoryBuilder( void AddToTrajectoryBuilder(
mapping::TrajectoryBuilderInterface *const trajectory_builder) override; mapping::TrajectoryBuilderInterface* const trajectory_builder) override;
void AddToPoseGraph(int trajectory_id, void AddToPoseGraph(int trajectory_id,
mapping::PoseGraph *pose_graph) const override; mapping::PoseGraph* pose_graph) const override;
private: private:
std::shared_ptr<const mapping::TrajectoryNode::Data> node_data_; std::shared_ptr<const mapping::TrajectoryNode::Data> node_data_;

11
cartographer/mapping/map_builder.cc
View File

@ -58,7 +58,7 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
MapBuilder::MapBuilder(const proto::MapBuilderOptions& options) MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
: options_(options), thread_pool_(options.num_background_threads()) { : options_(options), thread_pool_(options.num_background_threads()) {
if (options.use_trajectory_builder_2d()) { if (options.use_trajectory_builder_2d()) {
pose_graph_2d_ = common::make_unique<mapping_2d::PoseGraph>( pose_graph_2d_ = common::make_unique<PoseGraph2D>(
options_.pose_graph_options(), &thread_pool_); options_.pose_graph_options(), &thread_pool_);
pose_graph_ = pose_graph_2d_.get(); pose_graph_ = pose_graph_2d_.get();
} }
@ -74,8 +74,6 @@ MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
} }
} }
MapBuilder::~MapBuilder() {}
int MapBuilder::AddTrajectoryBuilder( int MapBuilder::AddTrajectoryBuilder(
const std::set<SensorId>& expected_sensor_ids, const std::set<SensorId>& expected_sensor_ids,
const proto::TrajectoryBuilderOptions& trajectory_options, const proto::TrajectoryBuilderOptions& trajectory_options,
@ -111,10 +109,9 @@ int MapBuilder::AddTrajectoryBuilder(
sensor_collator_.get(), trajectory_id, expected_sensor_ids, sensor_collator_.get(), trajectory_id, expected_sensor_ids,
common::make_unique<mapping::GlobalTrajectoryBuilder< common::make_unique<mapping::GlobalTrajectoryBuilder<
mapping_2d::LocalTrajectoryBuilder, mapping_2d::LocalTrajectoryBuilder,
mapping_2d::proto::LocalTrajectoryBuilderOptions, mapping_2d::proto::LocalTrajectoryBuilderOptions, PoseGraph2D>>(
mapping_2d::PoseGraph>>(std::move(local_trajectory_builder), std::move(local_trajectory_builder), trajectory_id,
trajectory_id, pose_graph_2d_.get(), pose_graph_2d_.get(), local_slam_result_callback)));
local_slam_result_callback)));
} }
if (trajectory_options.pure_localization()) { if (trajectory_options.pure_localization()) {
constexpr int kSubmapsToKeep = 3; constexpr int kSubmapsToKeep = 3;

7
cartographer/mapping/map_builder.h
View File

@ -23,9 +23,8 @@
#include <set> #include <set>
#include "cartographer/common/thread_pool.h" #include "cartographer/common/thread_pool.h"
#include "cartographer/mapping/pose_graph_interface.h"
#include "cartographer/mapping/proto/map_builder_options.pb.h" #include "cartographer/mapping/proto/map_builder_options.pb.h"
#include "cartographer/mapping_2d/pose_graph.h" #include "cartographer/mapping_2d/pose_graph_2d.h"
#include "cartographer/mapping_3d/pose_graph.h" #include "cartographer/mapping_3d/pose_graph.h"
#include "cartographer/sensor/collator_interface.h" #include "cartographer/sensor/collator_interface.h"
@ -40,7 +39,7 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
class MapBuilder : public MapBuilderInterface { class MapBuilder : public MapBuilderInterface {
public: public:
explicit MapBuilder(const proto::MapBuilderOptions& options); explicit MapBuilder(const proto::MapBuilderOptions& options);
~MapBuilder() override; ~MapBuilder() override {}
MapBuilder(const MapBuilder&) = delete; MapBuilder(const MapBuilder&) = delete;
MapBuilder& operator=(const MapBuilder&) = delete; MapBuilder& operator=(const MapBuilder&) = delete;
@ -77,7 +76,7 @@ class MapBuilder : public MapBuilderInterface {
const proto::MapBuilderOptions options_; const proto::MapBuilderOptions options_;
common::ThreadPool thread_pool_; common::ThreadPool thread_pool_;
std::unique_ptr<mapping_2d::PoseGraph> pose_graph_2d_; std::unique_ptr<PoseGraph2D> pose_graph_2d_;
std::unique_ptr<mapping_3d::PoseGraph> pose_graph_3d_; std::unique_ptr<mapping_3d::PoseGraph> pose_graph_3d_;
mapping::PoseGraph* pose_graph_; mapping::PoseGraph* pose_graph_;

269
cartographer/mapping_2d/pose_graph.cc → cartographer/mapping_2d/pose_graph_2d.cc
View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping_2d/pose_graph.h" #include "cartographer/mapping_2d/pose_graph_2d.h"
#include <algorithm> #include <algorithm>
#include <cmath> #include <cmath>
@ -37,23 +37,24 @@
#include "glog/logging.h" #include "glog/logging.h"
namespace cartographer { namespace cartographer {
namespace mapping_2d { namespace mapping {
PoseGraph::PoseGraph(const mapping::proto::PoseGraphOptions& options, PoseGraph2D::PoseGraph2D(const proto::PoseGraphOptions& options,
common::ThreadPool* thread_pool) common::ThreadPool* thread_pool)
: options_(options), : options_(options),
optimization_problem_(options_.optimization_problem_options()), optimization_problem_(options_.optimization_problem_options()),
constraint_builder_(options_.constraint_builder_options(), thread_pool) {} constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
PoseGraph::~PoseGraph() { PoseGraph2D::~PoseGraph2D() {
WaitForAllComputations(); WaitForAllComputations();
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
CHECK(work_queue_ == nullptr); CHECK(work_queue_ == nullptr);
} }
std::vector<mapping::SubmapId> PoseGraph::InitializeGlobalSubmapPoses( std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(
const int trajectory_id, const common::Time time, const int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) { const std::vector<std::shared_ptr<const mapping_2d::Submap>>&
insertion_submaps) {
CHECK(!insertion_submaps.empty()); CHECK(!insertion_submaps.empty());
const auto& submap_data = optimization_problem_.submap_data(); const auto& submap_data = optimization_problem_.submap_data();
if (insertion_submaps.size() == 1) { if (insertion_submaps.size() == 1) {
@ -71,14 +72,14 @@ std::vector<mapping::SubmapId> PoseGraph::InitializeGlobalSubmapPoses(
insertion_submaps[0]->local_pose())); insertion_submaps[0]->local_pose()));
} }
CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id)); CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
const mapping::SubmapId submap_id{trajectory_id, 0}; const SubmapId submap_id{trajectory_id, 0};
CHECK(submap_data_.at(submap_id).submap == insertion_submaps.front()); CHECK(submap_data_.at(submap_id).submap == insertion_submaps.front());
return {submap_id}; return {submap_id};
} }
CHECK_EQ(2, insertion_submaps.size()); CHECK_EQ(2, insertion_submaps.size());
const auto end_it = submap_data.EndOfTrajectory(trajectory_id); const auto end_it = submap_data.EndOfTrajectory(trajectory_id);
CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it); CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);
const mapping::SubmapId last_submap_id = std::prev(end_it)->id; const SubmapId last_submap_id = std::prev(end_it)->id;
if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) { if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) {
// In this case, 'last_submap_id' is the ID of // In this case, 'last_submap_id' is the ID of
// 'insertions_submaps.front()' and 'insertions_submaps.back()' is new. // 'insertions_submaps.front()' and 'insertions_submaps.back()' is new.
@ -86,29 +87,31 @@ std::vector<mapping::SubmapId> PoseGraph::InitializeGlobalSubmapPoses(
optimization_problem_.AddSubmap( optimization_problem_.AddSubmap(
trajectory_id, trajectory_id,
first_submap_pose * first_submap_pose *
pose_graph::ComputeSubmapPose(*insertion_submaps[0]).inverse() * mapping_2d::pose_graph::ComputeSubmapPose(*insertion_submaps[0])
pose_graph::ComputeSubmapPose(*insertion_submaps[1])); .inverse() *
mapping_2d::pose_graph::ComputeSubmapPose(*insertion_submaps[1]));
return {last_submap_id, return {last_submap_id,
mapping::SubmapId{trajectory_id, last_submap_id.submap_index + 1}}; SubmapId{trajectory_id, last_submap_id.submap_index + 1}};
} }
CHECK(submap_data_.at(last_submap_id).submap == insertion_submaps.back()); CHECK(submap_data_.at(last_submap_id).submap == insertion_submaps.back());
const mapping::SubmapId front_submap_id{trajectory_id, const SubmapId front_submap_id{trajectory_id,
last_submap_id.submap_index - 1}; last_submap_id.submap_index - 1};
CHECK(submap_data_.at(front_submap_id).submap == insertion_submaps.front()); CHECK(submap_data_.at(front_submap_id).submap == insertion_submaps.front());
return {front_submap_id, last_submap_id}; return {front_submap_id, last_submap_id};
} }
mapping::NodeId PoseGraph::AddNode( NodeId PoseGraph2D::AddNode(
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data, std::shared_ptr<const TrajectoryNode::Data> constant_data,
const int trajectory_id, const int trajectory_id,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) { const std::vector<std::shared_ptr<const mapping_2d::Submap>>&
insertion_submaps) {
const transform::Rigid3d optimized_pose( const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose); GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddTrajectoryIfNeeded(trajectory_id); AddTrajectoryIfNeeded(trajectory_id);
const mapping::NodeId node_id = trajectory_nodes_.Append( const NodeId node_id = trajectory_nodes_.Append(
trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose}); trajectory_id, TrajectoryNode{constant_data, optimized_pose});
++num_trajectory_nodes_; ++num_trajectory_nodes_;
// Test if the 'insertion_submap.back()' is one we never saw before. // Test if the 'insertion_submap.back()' is one we never saw before.
@ -117,8 +120,7 @@ mapping::NodeId PoseGraph::AddNode(
insertion_submaps.back()) { insertion_submaps.back()) {
// We grow 'submap_data_' as needed. This code assumes that the first // We grow 'submap_data_' as needed. This code assumes that the first
// time we see a new submap is as 'insertion_submaps.back()'. // time we see a new submap is as 'insertion_submaps.back()'.
const mapping::SubmapId submap_id = const SubmapId submap_id = submap_data_.Append(trajectory_id, SubmapData());
submap_data_.Append(trajectory_id, SubmapData());
submap_data_.at(submap_id).submap = insertion_submaps.back(); submap_data_.at(submap_id).submap = insertion_submaps.back();
} }
@ -132,7 +134,7 @@ mapping::NodeId PoseGraph::AddNode(
return node_id; return node_id;
} }
void PoseGraph::AddWorkItem(const std::function<void()>& work_item) { void PoseGraph2D::AddWorkItem(const std::function<void()>& work_item) {
if (work_queue_ == nullptr) { if (work_queue_ == nullptr) {
work_item(); work_item();
} else { } else {
@ -140,7 +142,7 @@ void PoseGraph::AddWorkItem(const std::function<void()>& work_item) {
} }
} }
void PoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) { void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id) {
trajectory_connectivity_state_.Add(trajectory_id); trajectory_connectivity_state_.Add(trajectory_id);
// Make sure we have a sampler for this trajectory. // Make sure we have a sampler for this trajectory.
if (!global_localization_samplers_[trajectory_id]) { if (!global_localization_samplers_[trajectory_id]) {
@ -150,30 +152,30 @@ void PoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) {
} }
} }
void PoseGraph::AddImuData(const int trajectory_id, void PoseGraph2D::AddImuData(const int trajectory_id,
const sensor::ImuData& imu_data) { const sensor::ImuData& imu_data) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddImuData(trajectory_id, imu_data); optimization_problem_.AddImuData(trajectory_id, imu_data);
}); });
} }
void PoseGraph::AddOdometryData(const int trajectory_id, void PoseGraph2D::AddOdometryData(const int trajectory_id,
const sensor::OdometryData& odometry_data) { const sensor::OdometryData& odometry_data) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
optimization_problem_.AddOdometryData(trajectory_id, odometry_data); optimization_problem_.AddOdometryData(trajectory_id, odometry_data);
}); });
} }
void PoseGraph::AddFixedFramePoseData( void PoseGraph2D::AddFixedFramePoseData(
const int trajectory_id, const int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) { const sensor::FixedFramePoseData& fixed_frame_pose_data) {
LOG(FATAL) << "Not yet implemented for 2D."; LOG(FATAL) << "Not yet implemented for 2D.";
} }
void PoseGraph::AddLandmarkData(int trajectory_id, void PoseGraph2D::AddLandmarkData(int trajectory_id,
const sensor::LandmarkData& landmark_data) const sensor::LandmarkData& landmark_data)
EXCLUDES(mutex_) { EXCLUDES(mutex_) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([=]() REQUIRES(mutex_) { AddWorkItem([=]() REQUIRES(mutex_) {
@ -187,8 +189,8 @@ void PoseGraph::AddLandmarkData(int trajectory_id,
}); });
} }
void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id, void PoseGraph2D::ComputeConstraint(const NodeId& node_id,
const mapping::SubmapId& submap_id) { const SubmapId& submap_id) {
CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished); CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);
const common::Time node_time = GetLatestNodeTime(node_id, submap_id); const common::Time node_time = GetLatestNodeTime(node_id, submap_id);
@ -220,44 +222,46 @@ void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
} }
} }
void PoseGraph::ComputeConstraintsForOldNodes( void PoseGraph2D::ComputeConstraintsForOldNodes(const SubmapId& submap_id) {
const mapping::SubmapId& submap_id) {
const auto& submap_data = submap_data_.at(submap_id); const auto& submap_data = submap_data_.at(submap_id);
for (const auto& node_id_data : optimization_problem_.node_data()) { for (const auto& node_id_data : optimization_problem_.node_data()) {
const mapping::NodeId& node_id = node_id_data.id; const NodeId& node_id = node_id_data.id;
if (submap_data.node_ids.count(node_id) == 0) { if (submap_data.node_ids.count(node_id) == 0) {
ComputeConstraint(node_id, submap_id); ComputeConstraint(node_id, submap_id);
} }
} }
} }
void PoseGraph::ComputeConstraintsForNode( void PoseGraph2D::ComputeConstraintsForNode(
const mapping::NodeId& node_id, const NodeId& node_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps, std::vector<std::shared_ptr<const mapping_2d::Submap>> insertion_submaps,
const bool newly_finished_submap) { const bool newly_finished_submap) {
const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
const std::vector<mapping::SubmapId> submap_ids = InitializeGlobalSubmapPoses( const std::vector<SubmapId> submap_ids = InitializeGlobalSubmapPoses(
node_id.trajectory_id, constant_data->time, insertion_submaps); node_id.trajectory_id, constant_data->time, insertion_submaps);
CHECK_EQ(submap_ids.size(), insertion_submaps.size()); CHECK_EQ(submap_ids.size(), insertion_submaps.size());
const mapping::SubmapId matching_id = submap_ids.front(); const SubmapId matching_id = submap_ids.front();
const transform::Rigid2d pose = transform::Project2D( const transform::Rigid2d pose = transform::Project2D(
constant_data->local_pose * constant_data->local_pose *
transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse())); transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
const transform::Rigid2d optimized_pose = const transform::Rigid2d optimized_pose =
optimization_problem_.submap_data().at(matching_id).global_pose * optimization_problem_.submap_data().at(matching_id).global_pose *
pose_graph::ComputeSubmapPose(*insertion_submaps.front()).inverse() * mapping_2d::pose_graph::ComputeSubmapPose(*insertion_submaps.front())
.inverse() *
pose; pose;
optimization_problem_.AddTrajectoryNode( optimization_problem_.AddTrajectoryNode(
matching_id.trajectory_id, constant_data->time, pose, optimized_pose, matching_id.trajectory_id, constant_data->time, pose, optimized_pose,
constant_data->gravity_alignment); constant_data->gravity_alignment);
for (size_t i = 0; i < insertion_submaps.size(); ++i) { for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const mapping::SubmapId submap_id = submap_ids[i]; const SubmapId submap_id = submap_ids[i];
// Even if this was the last node added to 'submap_id', the submap will // Even if this was the last node added to 'submap_id', the submap will
// only be marked as finished in 'submap_data_' further below. // only be marked as finished in 'submap_data_' further below.
CHECK(submap_data_.at(submap_id).state == SubmapState::kActive); CHECK(submap_data_.at(submap_id).state == SubmapState::kActive);
submap_data_.at(submap_id).node_ids.emplace(node_id); submap_data_.at(submap_id).node_ids.emplace(node_id);
const transform::Rigid2d constraint_transform = const transform::Rigid2d constraint_transform =
pose_graph::ComputeSubmapPose(*insertion_submaps[i]).inverse() * pose; mapping_2d::pose_graph::ComputeSubmapPose(*insertion_submaps[i])
.inverse() *
pose;
constraints_.push_back(Constraint{submap_id, constraints_.push_back(Constraint{submap_id,
node_id, node_id,
{transform::Embed3D(constraint_transform), {transform::Embed3D(constraint_transform),
@ -274,7 +278,7 @@ void PoseGraph::ComputeConstraintsForNode(
} }
if (newly_finished_submap) { if (newly_finished_submap) {
const mapping::SubmapId finished_submap_id = submap_ids.front(); const SubmapId finished_submap_id = submap_ids.front();
SubmapData& finished_submap_data = submap_data_.at(finished_submap_id); SubmapData& finished_submap_data = submap_data_.at(finished_submap_id);
CHECK(finished_submap_data.state == SubmapState::kActive); CHECK(finished_submap_data.state == SubmapState::kActive);
finished_submap_data.state = SubmapState::kFinished; finished_submap_data.state = SubmapState::kFinished;
@ -291,7 +295,7 @@ void PoseGraph::ComputeConstraintsForNode(
} }
} }
void PoseGraph::DispatchOptimization() { void PoseGraph2D::DispatchOptimization() {
run_loop_closure_ = true; run_loop_closure_ = true;
// If there is a 'work_queue_' already, some other thread will take care. // If there is a 'work_queue_' already, some other thread will take care.
if (work_queue_ == nullptr) { if (work_queue_ == nullptr) {
@ -299,12 +303,12 @@ void PoseGraph::DispatchOptimization() {
HandleWorkQueue(); HandleWorkQueue();
} }
} }
common::Time PoseGraph::GetLatestNodeTime( common::Time PoseGraph2D::GetLatestNodeTime(const NodeId& node_id,
const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) const { const SubmapId& submap_id) const {
common::Time time = trajectory_nodes_.at(node_id).constant_data->time; common::Time time = trajectory_nodes_.at(node_id).constant_data->time;
const SubmapData& submap_data = submap_data_.at(submap_id); const SubmapData& submap_data = submap_data_.at(submap_id);
if (!submap_data.node_ids.empty()) { if (!submap_data.node_ids.empty()) {
const mapping::NodeId last_submap_node_id = const NodeId last_submap_node_id =
*submap_data_.at(submap_id).node_ids.rbegin(); *submap_data_.at(submap_id).node_ids.rbegin();
time = std::max( time = std::max(
time, trajectory_nodes_.at(last_submap_node_id).constant_data->time); time, trajectory_nodes_.at(last_submap_node_id).constant_data->time);
@ -312,8 +316,8 @@ common::Time PoseGraph::GetLatestNodeTime(
return time; return time;
} }
void PoseGraph::UpdateTrajectoryConnectivity(const Constraint& constraint) { void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint& constraint) {
CHECK_EQ(constraint.tag, mapping::PoseGraph::Constraint::INTER_SUBMAP); CHECK_EQ(constraint.tag, PoseGraph::Constraint::INTER_SUBMAP);
const common::Time time = const common::Time time =
GetLatestNodeTime(constraint.node_id, constraint.submap_id); GetLatestNodeTime(constraint.node_id, constraint.submap_id);
trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id, trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
@ -321,9 +325,9 @@ void PoseGraph::UpdateTrajectoryConnectivity(const Constraint& constraint) {
time); time);
} }
void PoseGraph::HandleWorkQueue() { void PoseGraph2D::HandleWorkQueue() {
constraint_builder_.WhenDone( constraint_builder_.WhenDone(
[this](const pose_graph::ConstraintBuilder::Result& result) { [this](const mapping_2d::pose_graph::ConstraintBuilder::Result& result) {
{ {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end()); constraints_.insert(constraints_.end(), result.begin(), result.end());
@ -339,11 +343,10 @@ void PoseGraph::HandleWorkQueue() {
trimmer->Trim(&trimming_handle); trimmer->Trim(&trimming_handle);
} }
trimmers_.erase( trimmers_.erase(
std::remove_if( std::remove_if(trimmers_.begin(), trimmers_.end(),
trimmers_.begin(), trimmers_.end(), [](std::unique_ptr<PoseGraphTrimmer>& trimmer) {
[](std::unique_ptr<mapping::PoseGraphTrimmer>& trimmer) { return trimmer->IsFinished();
return trimmer->IsFinished(); }),
}),
trimmers_.end()); trimmers_.end());
num_nodes_since_last_loop_closure_ = 0; num_nodes_since_last_loop_closure_ = 0;
@ -362,7 +365,7 @@ void PoseGraph::HandleWorkQueue() {
}); });
} }
void PoseGraph::WaitForAllComputations() { void PoseGraph2D::WaitForAllComputations() {
bool notification = false; bool notification = false;
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
const int num_finished_nodes_at_start = const int num_finished_nodes_at_start =
@ -385,8 +388,8 @@ void PoseGraph::WaitForAllComputations() {
} }
std::cout << "\r\x1b[KOptimizing: Done. " << std::endl; std::cout << "\r\x1b[KOptimizing: Done. " << std::endl;
constraint_builder_.WhenDone( constraint_builder_.WhenDone(
[this, [this, &notification](
&notification](const pose_graph::ConstraintBuilder::Result& result) { const mapping_2d::pose_graph::ConstraintBuilder::Result& result) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
constraints_.insert(constraints_.end(), result.begin(), result.end()); constraints_.insert(constraints_.end(), result.begin(), result.end());
notification = true; notification = true;
@ -394,7 +397,7 @@ void PoseGraph::WaitForAllComputations() {
locker.Await([&notification]() { return notification; }); locker.Await([&notification]() { return notification; });
} }
void PoseGraph::FinishTrajectory(const int trajectory_id) { void PoseGraph2D::FinishTrajectory(const int trajectory_id) {
AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
CHECK_EQ(finished_trajectories_.count(trajectory_id), 0); CHECK_EQ(finished_trajectories_.count(trajectory_id), 0);
finished_trajectories_.insert(trajectory_id); finished_trajectories_.insert(trajectory_id);
@ -407,11 +410,11 @@ void PoseGraph::FinishTrajectory(const int trajectory_id) {
}); });
} }
bool PoseGraph::IsTrajectoryFinished(const int trajectory_id) { bool PoseGraph2D::IsTrajectoryFinished(const int trajectory_id) {
return finished_trajectories_.count(trajectory_id) > 0; return finished_trajectories_.count(trajectory_id) > 0;
} }
void PoseGraph::FreezeTrajectory(const int trajectory_id) { void PoseGraph2D::FreezeTrajectory(const int trajectory_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
trajectory_connectivity_state_.Add(trajectory_id); trajectory_connectivity_state_.Add(trajectory_id);
AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
@ -420,16 +423,16 @@ void PoseGraph::FreezeTrajectory(const int trajectory_id) {
}); });
} }
void PoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, void PoseGraph2D::AddSubmapFromProto(
const mapping::proto::Submap& submap) { const transform::Rigid3d& global_submap_pose, const proto::Submap& submap) {
if (!submap.has_submap_2d()) { if (!submap.has_submap_2d()) {
return; return;
} }
const mapping::SubmapId submap_id = {submap.submap_id().trajectory_id(), const SubmapId submap_id = {submap.submap_id().trajectory_id(),
submap.submap_id().submap_index()}; submap.submap_id().submap_index()};
std::shared_ptr<const Submap> submap_ptr = std::shared_ptr<const mapping_2d::Submap> submap_ptr =
std::make_shared<const Submap>(submap.submap_2d()); std::make_shared<const mapping_2d::Submap>(submap.submap_2d());
const transform::Rigid2d global_submap_pose_2d = const transform::Rigid2d global_submap_pose_2d =
transform::Project2D(global_submap_pose); transform::Project2D(global_submap_pose);
@ -438,8 +441,8 @@ void PoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
submap_data_.Insert(submap_id, SubmapData()); submap_data_.Insert(submap_id, SubmapData());
submap_data_.at(submap_id).submap = submap_ptr; submap_data_.at(submap_id).submap = submap_ptr;
// Immediately show the submap at the 'global_submap_pose'. // Immediately show the submap at the 'global_submap_pose'.
global_submap_poses_.Insert(submap_id, global_submap_poses_.Insert(
pose_graph::SubmapData{global_submap_pose_2d}); submap_id, mapping_2d::pose_graph::SubmapData{global_submap_pose_2d});
AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) { AddWorkItem([this, submap_id, global_submap_pose_2d]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1); CHECK_EQ(frozen_trajectories_.count(submap_id.trajectory_id), 1);
submap_data_.at(submap_id).state = SubmapState::kFinished; submap_data_.at(submap_id).state = SubmapState::kFinished;
@ -447,18 +450,16 @@ void PoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
}); });
} }
void PoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose, void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Node& node) { const proto::Node& node) {
const mapping::NodeId node_id = {node.node_id().trajectory_id(), const NodeId node_id = {node.node_id().trajectory_id(),
node.node_id().node_index()}; node.node_id().node_index()};
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data = std::shared_ptr<const TrajectoryNode::Data> constant_data =
std::make_shared<const mapping::TrajectoryNode::Data>( std::make_shared<const TrajectoryNode::Data>(FromProto(node.node_data()));
mapping::FromProto(node.node_data()));
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddTrajectoryIfNeeded(node_id.trajectory_id); AddTrajectoryIfNeeded(node_id.trajectory_id);
trajectory_nodes_.Insert(node_id, trajectory_nodes_.Insert(node_id, TrajectoryNode{constant_data, global_pose});
mapping::TrajectoryNode{constant_data, global_pose});
AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) { AddWorkItem([this, node_id, global_pose]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(node_id.trajectory_id), 1); CHECK_EQ(frozen_trajectories_.count(node_id.trajectory_id), 1);
@ -474,20 +475,20 @@ void PoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
}); });
} }
void PoseGraph::SetTrajectoryDataFromProto( void PoseGraph2D::SetTrajectoryDataFromProto(
const mapping::proto::TrajectoryData& data) { const proto::TrajectoryData& data) {
// Not implemented yet in 2D. // Not implemented yet in 2D.
} }
void PoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id, void PoseGraph2D::AddNodeToSubmap(const NodeId& node_id,
const mapping::SubmapId& submap_id) { const SubmapId& submap_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([this, node_id, submap_id]() REQUIRES(mutex_) { AddWorkItem([this, node_id, submap_id]() REQUIRES(mutex_) {
submap_data_.at(submap_id).node_ids.insert(node_id); submap_data_.at(submap_id).node_ids.insert(node_id);
}); });
} }
void PoseGraph::AddSerializedConstraints( void PoseGraph2D::AddSerializedConstraints(
const std::vector<Constraint>& constraints) { const std::vector<Constraint>& constraints) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([this, constraints]() REQUIRES(mutex_) { AddWorkItem([this, constraints]() REQUIRES(mutex_) {
@ -519,15 +520,15 @@ void PoseGraph::AddSerializedConstraints(
}); });
} }
void PoseGraph::AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) { void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
// C++11 does not allow us to move a unique_ptr into a lambda. // C++11 does not allow us to move a unique_ptr into a lambda.
mapping::PoseGraphTrimmer* const trimmer_ptr = trimmer.release(); PoseGraphTrimmer* const trimmer_ptr = trimmer.release();
AddWorkItem([this, trimmer_ptr]() AddWorkItem([this, trimmer_ptr]()
REQUIRES(mutex_) { trimmers_.emplace_back(trimmer_ptr); }); REQUIRES(mutex_) { trimmers_.emplace_back(trimmer_ptr); });
} }
void PoseGraph::RunFinalOptimization() { void PoseGraph2D::RunFinalOptimization() {
{ {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
AddWorkItem([this]() REQUIRES(mutex_) { AddWorkItem([this]() REQUIRES(mutex_) {
@ -545,7 +546,7 @@ void PoseGraph::RunFinalOptimization() {
WaitForAllComputations(); WaitForAllComputations();
} }
void PoseGraph::RunOptimization() { void PoseGraph2D::RunOptimization() {
if (optimization_problem_.submap_data().empty()) { if (optimization_problem_.submap_data().empty()) {
return; return;
} }
@ -578,7 +579,7 @@ void PoseGraph::RunOptimization() {
const transform::Rigid3d old_global_to_new_global = const transform::Rigid3d old_global_to_new_global =
local_to_new_global * local_to_old_global.inverse(); local_to_new_global * local_to_old_global.inverse();
const mapping::NodeId last_optimized_node_id = const NodeId last_optimized_node_id =
std::prev(node_data.EndOfTrajectory(trajectory_id))->id; std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
auto node_it = std::next(trajectory_nodes_.find(last_optimized_node_id)); auto node_it = std::next(trajectory_nodes_.find(last_optimized_node_id));
for (; node_it != trajectory_nodes_.EndOfTrajectory(trajectory_id); for (; node_it != trajectory_nodes_.EndOfTrajectory(trajectory_id);
@ -594,26 +595,24 @@ void PoseGraph::RunOptimization() {
global_submap_poses_ = submap_data; global_submap_poses_ = submap_data;
} }
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode> MapById<NodeId, TrajectoryNode> PoseGraph2D::GetTrajectoryNodes() {
PoseGraph::GetTrajectoryNodes() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return trajectory_nodes_; return trajectory_nodes_;
} }
mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose> MapById<NodeId, TrajectoryNodePose> PoseGraph2D::GetTrajectoryNodePoses() {
PoseGraph::GetTrajectoryNodePoses() { MapById<NodeId, TrajectoryNodePose> node_poses;
mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose> node_poses;
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
for (const auto& node_id_data : trajectory_nodes_) { for (const auto& node_id_data : trajectory_nodes_) {
node_poses.Insert( node_poses.Insert(
node_id_data.id, node_id_data.id,
mapping::TrajectoryNodePose{node_id_data.data.constant_data != nullptr, TrajectoryNodePose{node_id_data.data.constant_data != nullptr,
node_id_data.data.global_pose}); node_id_data.data.global_pose});
} }
return node_poses; return node_poses;
} }
std::map<std::string, transform::Rigid3d> PoseGraph::GetLandmarkPoses() { std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses() {
std::map<std::string, transform::Rigid3d> landmark_poses; std::map<std::string, transform::Rigid3d> landmark_poses;
for (const auto& landmark : landmark_nodes_) { for (const auto& landmark : landmark_nodes_) {
// Landmark without value has not been optimized yet. // Landmark without value has not been optimized yet.
@ -624,27 +623,27 @@ std::map<std::string, transform::Rigid3d> PoseGraph::GetLandmarkPoses() {
return landmark_poses; return landmark_poses;
} }
sensor::MapByTime<sensor::ImuData> PoseGraph::GetImuData() { sensor::MapByTime<sensor::ImuData> PoseGraph2D::GetImuData() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return optimization_problem_.imu_data(); return optimization_problem_.imu_data();
} }
sensor::MapByTime<sensor::OdometryData> PoseGraph::GetOdometryData() { sensor::MapByTime<sensor::OdometryData> PoseGraph2D::GetOdometryData() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return optimization_problem_.odometry_data(); return optimization_problem_.odometry_data();
} }
std::map<int, mapping::PoseGraphInterface::TrajectoryData> std::map<int, PoseGraphInterface::TrajectoryData>
PoseGraph::GetTrajectoryData() { PoseGraph2D::GetTrajectoryData() {
return {}; // Not implemented yet in 2D. return {}; // Not implemented yet in 2D.
} }
sensor::MapByTime<sensor::FixedFramePoseData> sensor::MapByTime<sensor::FixedFramePoseData>
PoseGraph::GetFixedFramePoseData() { PoseGraph2D::GetFixedFramePoseData() {
return {}; // Not implemented yet in 2D. return {}; // Not implemented yet in 2D.
} }
std::vector<PoseGraph::Constraint> PoseGraph::constraints() { std::vector<PoseGraph2D::Constraint> PoseGraph2D::constraints() {
std::vector<Constraint> result; std::vector<Constraint> result;
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
for (const Constraint& constraint : constraints_) { for (const Constraint& constraint : constraints_) {
@ -661,16 +660,16 @@ std::vector<PoseGraph::Constraint> PoseGraph::constraints() {
return result; return result;
} }
void PoseGraph::SetInitialTrajectoryPose(const int from_trajectory_id, void PoseGraph2D::SetInitialTrajectoryPose(const int from_trajectory_id,
const int to_trajectory_id, const int to_trajectory_id,
const transform::Rigid3d& pose, const transform::Rigid3d& pose,
const common::Time time) { const common::Time time) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
initial_trajectory_poses_[from_trajectory_id] = initial_trajectory_poses_[from_trajectory_id] =
InitialTrajectoryPose{to_trajectory_id, pose, time}; InitialTrajectoryPose{to_trajectory_id, pose, time};
} }
transform::Rigid3d PoseGraph::GetInterpolatedGlobalTrajectoryPose( transform::Rigid3d PoseGraph2D::GetInterpolatedGlobalTrajectoryPose(
const int trajectory_id, const common::Time time) const { const int trajectory_id, const common::Time time) const {
CHECK(trajectory_nodes_.SizeOfTrajectoryOrZero(trajectory_id) > 0); CHECK(trajectory_nodes_.SizeOfTrajectoryOrZero(trajectory_id) > 0);
const auto it = trajectory_nodes_.lower_bound(trajectory_id, time); const auto it = trajectory_nodes_.lower_bound(trajectory_id, time);
@ -690,27 +689,25 @@ transform::Rigid3d PoseGraph::GetInterpolatedGlobalTrajectoryPose(
.transform; .transform;
} }
transform::Rigid3d PoseGraph::GetLocalToGlobalTransform( transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform(
const int trajectory_id) { const int trajectory_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id); return ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id);
} }
std::vector<std::vector<int>> PoseGraph::GetConnectedTrajectories() { std::vector<std::vector<int>> PoseGraph2D::GetConnectedTrajectories() {
return trajectory_connectivity_state_.Components(); return trajectory_connectivity_state_.Components();
} }
mapping::PoseGraph::SubmapData PoseGraph::GetSubmapData( PoseGraph::SubmapData PoseGraph2D::GetSubmapData(const SubmapId& submap_id) {
const mapping::SubmapId& submap_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return GetSubmapDataUnderLock(submap_id); return GetSubmapDataUnderLock(submap_id);
} }
mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapData> MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph::GetAllSubmapData() { PoseGraph2D::GetAllSubmapData() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapData> MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
submaps;
for (const auto& submap_id_data : submap_data_) { for (const auto& submap_id_data : submap_data_) {
submaps.Insert(submap_id_data.id, submaps.Insert(submap_id_data.id,
GetSubmapDataUnderLock(submap_id_data.id)); GetSubmapDataUnderLock(submap_id_data.id));
@ -718,22 +715,22 @@ PoseGraph::GetAllSubmapData() {
return submaps; return submaps;
} }
mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapPose> MapById<SubmapId, PoseGraphInterface::SubmapPose>
PoseGraph::GetAllSubmapPoses() { PoseGraph2D::GetAllSubmapPoses() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
mapping::MapById<mapping::SubmapId, SubmapPose> submap_poses; MapById<SubmapId, SubmapPose> submap_poses;
for (const auto& submap_id_data : submap_data_) { for (const auto& submap_id_data : submap_data_) {
auto submap_data = GetSubmapDataUnderLock(submap_id_data.id); auto submap_data = GetSubmapDataUnderLock(submap_id_data.id);
submap_poses.Insert( submap_poses.Insert(
submap_id_data.id, submap_id_data.id,
mapping::PoseGraph::SubmapPose{submap_data.submap->num_range_data(), PoseGraph::SubmapPose{submap_data.submap->num_range_data(),
submap_data.pose}); submap_data.pose});
} }
return submap_poses; return submap_poses;
} }
transform::Rigid3d PoseGraph::ComputeLocalToGlobalTransform( transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
const mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>& const MapById<SubmapId, mapping_2d::pose_graph::SubmapData>&
global_submap_poses, global_submap_poses,
const int trajectory_id) const { const int trajectory_id) const {
auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id); auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
@ -748,7 +745,7 @@ transform::Rigid3d PoseGraph::ComputeLocalToGlobalTransform(
return transform::Rigid3d::Identity(); return transform::Rigid3d::Identity();
} }
} }
const mapping::SubmapId last_optimized_submap_id = std::prev(end_it)->id; const SubmapId last_optimized_submap_id = std::prev(end_it)->id;
// Accessing 'local_pose' in Submap is okay, since the member is const. // Accessing 'local_pose' in Submap is okay, since the member is const.
return transform::Embed3D( return transform::Embed3D(
global_submap_poses.at(last_optimized_submap_id).global_pose) * global_submap_poses.at(last_optimized_submap_id).global_pose) *
@ -757,8 +754,8 @@ transform::Rigid3d PoseGraph::ComputeLocalToGlobalTransform(
.inverse(); .inverse();
} }
mapping::PoseGraph::SubmapData PoseGraph::GetSubmapDataUnderLock( PoseGraph::SubmapData PoseGraph2D::GetSubmapDataUnderLock(
const mapping::SubmapId& submap_id) { const SubmapId& submap_id) {
const auto it = submap_data_.find(submap_id); const auto it = submap_data_.find(submap_id);
if (it == submap_data_.end()) { if (it == submap_data_.end()) {
return {}; return {};
@ -775,27 +772,27 @@ mapping::PoseGraph::SubmapData PoseGraph::GetSubmapDataUnderLock(
submap->local_pose()}; submap->local_pose()};
} }
PoseGraph::TrimmingHandle::TrimmingHandle(PoseGraph* const parent) PoseGraph2D::TrimmingHandle::TrimmingHandle(PoseGraph2D* const parent)
: parent_(parent) {} : parent_(parent) {}
int PoseGraph::TrimmingHandle::num_submaps(const int trajectory_id) const { int PoseGraph2D::TrimmingHandle::num_submaps(const int trajectory_id) const {
const auto& submap_data = parent_->optimization_problem_.submap_data(); const auto& submap_data = parent_->optimization_problem_.submap_data();
return submap_data.SizeOfTrajectoryOrZero(trajectory_id); return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
} }
bool PoseGraph::TrimmingHandle::IsFinished(const int trajectory_id) const { bool PoseGraph2D::TrimmingHandle::IsFinished(const int trajectory_id) const {
return parent_->IsTrajectoryFinished(trajectory_id); return parent_->IsTrajectoryFinished(trajectory_id);
} }
void PoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( void PoseGraph2D::TrimmingHandle::MarkSubmapAsTrimmed(
const mapping::SubmapId& submap_id) { const SubmapId& submap_id) {
// TODO(hrapp): We have to make sure that the trajectory has been finished // TODO(hrapp): We have to make sure that the trajectory has been finished
// if we want to delete the last submaps. // if we want to delete the last submaps.
CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished); CHECK(parent_->submap_data_.at(submap_id).state == SubmapState::kFinished);
// Compile all nodes that are still INTRA_SUBMAP constrained once the submap // Compile all nodes that are still INTRA_SUBMAP constrained once the submap
// with 'submap_id' is gone. // with 'submap_id' is gone.
std::set<mapping::NodeId> nodes_to_retain; std::set<NodeId> nodes_to_retain;
for (const Constraint& constraint : parent_->constraints_) { for (const Constraint& constraint : parent_->constraints_) {
if (constraint.tag == Constraint::Tag::INTRA_SUBMAP && if (constraint.tag == Constraint::Tag::INTRA_SUBMAP &&
constraint.submap_id != submap_id) { constraint.submap_id != submap_id) {
@ -803,7 +800,7 @@ void PoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
} }
} }
// Remove all 'constraints_' related to 'submap_id'. // Remove all 'constraints_' related to 'submap_id'.
std::set<mapping::NodeId> nodes_to_remove; std::set<NodeId> nodes_to_remove;
{ {
std::vector<Constraint> constraints; std::vector<Constraint> constraints;
for (const Constraint& constraint : parent_->constraints_) { for (const Constraint& constraint : parent_->constraints_) {
@ -839,11 +836,11 @@ void PoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
// Remove the 'nodes_to_remove' from the pose graph and the optimization // Remove the 'nodes_to_remove' from the pose graph and the optimization
// problem. // problem.
for (const mapping::NodeId& node_id : nodes_to_remove) { for (const NodeId& node_id : nodes_to_remove) {
parent_->trajectory_nodes_.Trim(node_id); parent_->trajectory_nodes_.Trim(node_id);
parent_->optimization_problem_.TrimTrajectoryNode(node_id); parent_->optimization_problem_.TrimTrajectoryNode(node_id);
} }
} }
} // namespace mapping_2d } // namespace mapping
} // namespace cartographer } // namespace cartographer

121
cartographer/mapping_2d/pose_graph.h → cartographer/mapping_2d/pose_graph_2d.h
View File

@ -14,8 +14,8 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_H_ #ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_2D_H_
#define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_H_ #define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_2D_H_
#include <deque> #include <deque>
#include <functional> #include <functional>
@ -46,7 +46,7 @@
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
namespace cartographer { namespace cartographer {
namespace mapping_2d { namespace mapping {
// Implements the loop closure method called Sparse Pose Adjustment (SPA) from // Implements the loop closure method called Sparse Pose Adjustment (SPA) from
// Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping." // Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping."
@ -57,25 +57,24 @@ namespace mapping_2d {
// Each node has been matched against one or more submaps (adding a constraint // Each node has been matched against one or more submaps (adding a constraint
// for each match), both poses of nodes and of submaps are to be optimized. // for each match), both poses of nodes and of submaps are to be optimized.
// All constraints are between a submap i and a node j. // All constraints are between a submap i and a node j.
class PoseGraph : public mapping::PoseGraph { class PoseGraph2D : public PoseGraph {
public: public:
PoseGraph(const mapping::proto::PoseGraphOptions& options, PoseGraph2D(const proto::PoseGraphOptions& options,
common::ThreadPool* thread_pool); common::ThreadPool* thread_pool);
~PoseGraph() override; ~PoseGraph2D() override;
PoseGraph(const PoseGraph&) = delete; PoseGraph2D(const PoseGraph2D&) = delete;
PoseGraph& operator=(const PoseGraph&) = delete; PoseGraph2D& operator=(const PoseGraph2D&) = delete;
// Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was // Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was
// determined by scan matching against 'insertion_submaps.front()' and the // determined by scan matching against 'insertion_submaps.front()' and the
// node data was inserted into the 'insertion_submaps'. If // node data was inserted into the 'insertion_submaps'. If
// 'insertion_submaps.front().finished()' is 'true', data was inserted into // 'insertion_submaps.front().finished()' is 'true', data was inserted into
// this submap for the last time. // this submap for the last time.
mapping::NodeId AddNode( NodeId AddNode(std::shared_ptr<const TrajectoryNode::Data> constant_data,
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data, int trajectory_id,
int trajectory_id, const std::vector<std::shared_ptr<const mapping_2d::Submap>>&
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) insertion_submaps) EXCLUDES(mutex_);
EXCLUDES(mutex_);
void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override void AddImuData(int trajectory_id, const sensor::ImuData& imu_data) override
EXCLUDES(mutex_); EXCLUDES(mutex_);
@ -94,30 +93,28 @@ class PoseGraph : public mapping::PoseGraph {
bool IsTrajectoryFinished(int trajectory_id) override; bool IsTrajectoryFinished(int trajectory_id) override;
void FreezeTrajectory(int trajectory_id) override; void FreezeTrajectory(int trajectory_id) override;
void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose, void AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
const mapping::proto::Submap& submap) override; const proto::Submap& submap) override;
void AddNodeFromProto(const transform::Rigid3d& global_pose, void AddNodeFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Node& node) override; const proto::Node& node) override;
void SetTrajectoryDataFromProto( void SetTrajectoryDataFromProto(const proto::TrajectoryData& data) override;
const mapping::proto::TrajectoryData& data) override; void AddNodeToSubmap(const NodeId& node_id,
void AddNodeToSubmap(const mapping::NodeId& node_id, const SubmapId& submap_id) override;
const mapping::SubmapId& submap_id) override;
void AddSerializedConstraints( void AddSerializedConstraints(
const std::vector<Constraint>& constraints) override; const std::vector<Constraint>& constraints) override;
void AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) override; void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) override;
void RunFinalOptimization() override; void RunFinalOptimization() override;
std::vector<std::vector<int>> GetConnectedTrajectories() override; std::vector<std::vector<int>> GetConnectedTrajectories() override;
mapping::PoseGraph::SubmapData GetSubmapData( PoseGraph::SubmapData GetSubmapData(const SubmapId& submap_id)
const mapping::SubmapId& submap_id) EXCLUDES(mutex_) override;
mapping::MapById<mapping::SubmapId, mapping::PoseGraphInterface::SubmapData>
GetAllSubmapData() EXCLUDES(mutex_) override;
mapping::MapById<mapping::SubmapId, SubmapPose> GetAllSubmapPoses()
EXCLUDES(mutex_) override; EXCLUDES(mutex_) override;
MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData()
EXCLUDES(mutex_) override;
MapById<SubmapId, SubmapPose> GetAllSubmapPoses() EXCLUDES(mutex_) override;
transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id)
EXCLUDES(mutex_) override; EXCLUDES(mutex_) override;
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode> MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() override
GetTrajectoryNodes() override EXCLUDES(mutex_); EXCLUDES(mutex_);
mapping::MapById<mapping::NodeId, mapping::TrajectoryNodePose> MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() override
GetTrajectoryNodePoses() override EXCLUDES(mutex_); EXCLUDES(mutex_);
std::map<std::string, transform::Rigid3d> GetLandmarkPoses() override std::map<std::string, transform::Rigid3d> GetLandmarkPoses() override
EXCLUDES(mutex_); EXCLUDES(mutex_);
sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_); sensor::MapByTime<sensor::ImuData> GetImuData() override EXCLUDES(mutex_);
@ -140,12 +137,12 @@ class PoseGraph : public mapping::PoseGraph {
// Likewise, all new nodes are matched against submaps which are finished. // Likewise, all new nodes are matched against submaps which are finished.
enum class SubmapState { kActive, kFinished }; enum class SubmapState { kActive, kFinished };
struct SubmapData { struct SubmapData {
std::shared_ptr<const Submap> submap; std::shared_ptr<const mapping_2d::Submap> submap;
// IDs of the nodes that were inserted into this map together with // IDs of the nodes that were inserted into this map together with
// constraints for them. They are not to be matched again when this submap // constraints for them. They are not to be matched again when this submap
// becomes 'finished'. // becomes 'finished'.
std::set<mapping::NodeId> node_ids; std::set<NodeId> node_ids;
SubmapState state = SubmapState::kActive; SubmapState state = SubmapState::kActive;
}; };
@ -158,23 +155,23 @@ class PoseGraph : public mapping::PoseGraph {
// Grows the optimization problem to have an entry for every element of // Grows the optimization problem to have an entry for every element of
// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'. // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
std::vector<mapping::SubmapId> InitializeGlobalSubmapPoses( std::vector<SubmapId> InitializeGlobalSubmapPoses(
int trajectory_id, const common::Time time, int trajectory_id, const common::Time time,
const std::vector<std::shared_ptr<const Submap>>& insertion_submaps) const std::vector<std::shared_ptr<const mapping_2d::Submap>>&
REQUIRES(mutex_); insertion_submaps) REQUIRES(mutex_);
// Adds constraints for a node, and starts scan matching in the background. // Adds constraints for a node, and starts scan matching in the background.
void ComputeConstraintsForNode( void ComputeConstraintsForNode(
const mapping::NodeId& node_id, const NodeId& node_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps, std::vector<std::shared_ptr<const mapping_2d::Submap>> insertion_submaps,
bool newly_finished_submap) REQUIRES(mutex_); bool newly_finished_submap) REQUIRES(mutex_);
// Computes constraints for a node and submap pair. // Computes constraints for a node and submap pair.
void ComputeConstraint(const mapping::NodeId& node_id, void ComputeConstraint(const NodeId& node_id, const SubmapId& submap_id)
const mapping::SubmapId& submap_id) REQUIRES(mutex_); REQUIRES(mutex_);
// Adds constraints for older nodes whenever a new submap is finished. // Adds constraints for older nodes whenever a new submap is finished.
void ComputeConstraintsForOldNodes(const mapping::SubmapId& submap_id) void ComputeConstraintsForOldNodes(const SubmapId& submap_id)
REQUIRES(mutex_); REQUIRES(mutex_);
// Registers the callback to run the optimization once all constraints have // Registers the callback to run the optimization once all constraints have
@ -192,22 +189,22 @@ class PoseGraph : public mapping::PoseGraph {
// Computes the local to global map frame transform based on the given // Computes the local to global map frame transform based on the given
// 'global_submap_poses'. // 'global_submap_poses'.
transform::Rigid3d ComputeLocalToGlobalTransform( transform::Rigid3d ComputeLocalToGlobalTransform(
const mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>& const MapById<SubmapId, mapping_2d::pose_graph::SubmapData>&
global_submap_poses, global_submap_poses,
int trajectory_id) const REQUIRES(mutex_); int trajectory_id) const REQUIRES(mutex_);
mapping::PoseGraph::SubmapData GetSubmapDataUnderLock( PoseGraph::SubmapData GetSubmapDataUnderLock(const SubmapId& submap_id)
const mapping::SubmapId& submap_id) REQUIRES(mutex_); REQUIRES(mutex_);
common::Time GetLatestNodeTime(const mapping::NodeId& node_id, common::Time GetLatestNodeTime(const NodeId& node_id,
const mapping::SubmapId& submap_id) const const SubmapId& submap_id) const
REQUIRES(mutex_); REQUIRES(mutex_);
// Updates the trajectory connectivity structure with a new constraint. // Updates the trajectory connectivity structure with a new constraint.
void UpdateTrajectoryConnectivity(const Constraint& constraint) void UpdateTrajectoryConnectivity(const Constraint& constraint)
REQUIRES(mutex_); REQUIRES(mutex_);
const mapping::proto::PoseGraphOptions options_; const proto::PoseGraphOptions options_;
common::Mutex mutex_; common::Mutex mutex_;
// If it exists, further work items must be added to this queue, and will be // If it exists, further work items must be added to this queue, and will be
@ -216,7 +213,7 @@ class PoseGraph : public mapping::PoseGraph {
GUARDED_BY(mutex_); GUARDED_BY(mutex_);
// How our various trajectories are related. // How our various trajectories are related.
mapping::TrajectoryConnectivityState trajectory_connectivity_state_; TrajectoryConnectivityState trajectory_connectivity_state_;
// We globally localize a fraction of the nodes from each trajectory. // We globally localize a fraction of the nodes from each trajectory.
std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>> std::unordered_map<int, std::unique_ptr<common::FixedRatioSampler>>
@ -232,31 +229,29 @@ class PoseGraph : public mapping::PoseGraph {
void DispatchOptimization() REQUIRES(mutex_); void DispatchOptimization() REQUIRES(mutex_);
// Current optimization problem. // Current optimization problem.
pose_graph::OptimizationProblem optimization_problem_; mapping_2d::pose_graph::OptimizationProblem optimization_problem_;
pose_graph::ConstraintBuilder constraint_builder_ GUARDED_BY(mutex_); mapping_2d::pose_graph::ConstraintBuilder constraint_builder_
GUARDED_BY(mutex_);
std::vector<Constraint> constraints_ GUARDED_BY(mutex_); std::vector<Constraint> constraints_ GUARDED_BY(mutex_);
// Submaps get assigned an ID and state as soon as they are seen, even // Submaps get assigned an ID and state as soon as they are seen, even
// before they take part in the background computations. // before they take part in the background computations.
mapping::MapById<mapping::SubmapId, SubmapData> submap_data_ MapById<SubmapId, SubmapData> submap_data_ GUARDED_BY(mutex_);
GUARDED_BY(mutex_);
// Data that are currently being shown. // Data that are currently being shown.
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode> trajectory_nodes_ MapById<NodeId, TrajectoryNode> trajectory_nodes_ GUARDED_BY(mutex_);
GUARDED_BY(mutex_);
int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0; int num_trajectory_nodes_ GUARDED_BY(mutex_) = 0;
// Global submap poses currently used for displaying data. // Global submap poses currently used for displaying data.
mapping::MapById<mapping::SubmapId, pose_graph::SubmapData> MapById<SubmapId, mapping_2d::pose_graph::SubmapData> global_submap_poses_
global_submap_poses_ GUARDED_BY(mutex_); GUARDED_BY(mutex_);
// Global landmark poses with all observations. // Global landmark poses with all observations.
std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode> std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
landmark_nodes_ GUARDED_BY(mutex_); landmark_nodes_ GUARDED_BY(mutex_);
// List of all trimmers to consult when optimizations finish. // List of all trimmers to consult when optimizations finish.
std::vector<std::unique_ptr<mapping::PoseGraphTrimmer>> trimmers_ std::vector<std::unique_ptr<PoseGraphTrimmer>> trimmers_ GUARDED_BY(mutex_);
GUARDED_BY(mutex_);
// Set of all frozen trajectories not being optimized. // Set of all frozen trajectories not being optimized.
std::set<int> frozen_trajectories_ GUARDED_BY(mutex_); std::set<int> frozen_trajectories_ GUARDED_BY(mutex_);
@ -270,22 +265,22 @@ class PoseGraph : public mapping::PoseGraph {
// Allows querying and manipulating the pose graph by the 'trimmers_'. The // Allows querying and manipulating the pose graph by the 'trimmers_'. The
// 'mutex_' of the pose graph is held while this class is used. // 'mutex_' of the pose graph is held while this class is used.
class TrimmingHandle : public mapping::Trimmable { class TrimmingHandle : public Trimmable {
public: public:
TrimmingHandle(PoseGraph* parent); TrimmingHandle(PoseGraph2D* parent);
~TrimmingHandle() override {} ~TrimmingHandle() override {}
int num_submaps(int trajectory_id) const override; int num_submaps(int trajectory_id) const override;
void MarkSubmapAsTrimmed(const mapping::SubmapId& submap_id) void MarkSubmapAsTrimmed(const SubmapId& submap_id)
REQUIRES(parent_->mutex_) override; REQUIRES(parent_->mutex_) override;
bool IsFinished(int trajectory_id) const override; bool IsFinished(int trajectory_id) const override;
private: private:
PoseGraph* const parent_; PoseGraph2D* const parent_;
}; };
}; };
} // namespace mapping_2d } // namespace mapping
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_H_ #endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_2D_H_

73
cartographer/mapping_2d/pose_graph_test.cc → cartographer/mapping_2d/pose_graph_2d_test.cc
View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping_2d/pose_graph.h" #include "cartographer/mapping_2d/pose_graph_2d.h"
#include <cmath> #include <cmath>
#include <memory> #include <memory>
@ -32,12 +32,12 @@
#include "gmock/gmock.h" #include "gmock/gmock.h"
namespace cartographer { namespace cartographer {
namespace mapping_2d { namespace mapping {
namespace { namespace {
class PoseGraphTest : public ::testing::Test { class PoseGraph2DTest : public ::testing::Test {
protected: protected:
PoseGraphTest() : thread_pool_(1) { PoseGraph2DTest() : thread_pool_(1) {
// Builds a wavy, irregularly circular point cloud that is unique // Builds a wavy, irregularly circular point cloud that is unique
// rotationally. This gives us good rotational texture and avoids any // rotationally. This gives us good rotational texture and avoids any
// possibility of the CeresScanMatcher preferring free space (> // possibility of the CeresScanMatcher preferring free space (>
@ -58,8 +58,8 @@ class PoseGraphTest : public ::testing::Test {
miss_probability = 0.495, miss_probability = 0.495,
}, },
})text"); })text");
active_submaps_ = common::make_unique<ActiveSubmaps>( active_submaps_ = common::make_unique<mapping_2d::ActiveSubmaps>(
CreateSubmapsOptions(parameter_dictionary.get())); mapping_2d::CreateSubmapsOptions(parameter_dictionary.get()));
} }
{ {
@ -132,9 +132,8 @@ class PoseGraphTest : public ::testing::Test {
log_residual_histograms = true, log_residual_histograms = true,
global_constraint_search_after_n_seconds = 10.0, global_constraint_search_after_n_seconds = 10.0,
})text"); })text");
pose_graph_ = common::make_unique<PoseGraph>( pose_graph_ = common::make_unique<PoseGraph2D>(
mapping::CreatePoseGraphOptions(parameter_dictionary.get()), CreatePoseGraphOptions(parameter_dictionary.get()), &thread_pool_);
&thread_pool_);
} }
current_pose_ = transform::Rigid2d::Identity(); current_pose_ = transform::Rigid2d::Identity();
@ -146,7 +145,7 @@ class PoseGraphTest : public ::testing::Test {
const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud( const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud(
point_cloud_, point_cloud_,
transform::Embed3D(current_pose_.inverse().cast<float>())); transform::Embed3D(current_pose_.inverse().cast<float>()));
std::vector<std::shared_ptr<const Submap>> insertion_submaps; std::vector<std::shared_ptr<const mapping_2d::Submap>> insertion_submaps;
for (const auto& submap : active_submaps_->submaps()) { for (const auto& submap : active_submaps_->submaps()) {
insertion_submaps.push_back(submap); insertion_submaps.push_back(submap);
} }
@ -158,14 +157,14 @@ class PoseGraphTest : public ::testing::Test {
range_data, transform::Embed3D(pose_estimate.cast<float>()))); range_data, transform::Embed3D(pose_estimate.cast<float>())));
pose_graph_->AddNode( pose_graph_->AddNode(
std::make_shared<const mapping::TrajectoryNode::Data>( std::make_shared<const TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{common::FromUniversal(0), TrajectoryNode::Data{common::FromUniversal(0),
Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
range_data.returns, range_data.returns,
{}, {},
{}, {},
{}, {},
transform::Embed3D(pose_estimate)}), transform::Embed3D(pose_estimate)}),
kTrajectoryId, insertion_submaps); kTrajectoryId, insertion_submaps);
} }
@ -179,19 +178,19 @@ class PoseGraphTest : public ::testing::Test {
} }
sensor::PointCloud point_cloud_; sensor::PointCloud point_cloud_;
std::unique_ptr<ActiveSubmaps> active_submaps_; std::unique_ptr<mapping_2d::ActiveSubmaps> active_submaps_;
common::ThreadPool thread_pool_; common::ThreadPool thread_pool_;
std::unique_ptr<PoseGraph> pose_graph_; std::unique_ptr<PoseGraph2D> pose_graph_;
transform::Rigid2d current_pose_; transform::Rigid2d current_pose_;
}; };
TEST_F(PoseGraphTest, EmptyMap) { TEST_F(PoseGraph2DTest, EmptyMap) {
pose_graph_->RunFinalOptimization(); pose_graph_->RunFinalOptimization();
const auto nodes = pose_graph_->GetTrajectoryNodes(); const auto nodes = pose_graph_->GetTrajectoryNodes();
EXPECT_TRUE(nodes.empty()); EXPECT_TRUE(nodes.empty());
} }
TEST_F(PoseGraphTest, NoMovement) { TEST_F(PoseGraph2DTest, NoMovement) {
MoveRelative(transform::Rigid2d::Identity()); MoveRelative(transform::Rigid2d::Identity());
MoveRelative(transform::Rigid2d::Identity()); MoveRelative(transform::Rigid2d::Identity());
MoveRelative(transform::Rigid2d::Identity()); MoveRelative(transform::Rigid2d::Identity());
@ -200,15 +199,15 @@ TEST_F(PoseGraphTest, NoMovement) {
ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()), ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
::testing::ContainerEq(std::vector<int>{0})); ::testing::ContainerEq(std::vector<int>{0}));
EXPECT_THAT(nodes.SizeOfTrajectoryOrZero(0), ::testing::Eq(3u)); EXPECT_THAT(nodes.SizeOfTrajectoryOrZero(0), ::testing::Eq(3u));
EXPECT_THAT(nodes.at(mapping::NodeId{0, 0}).global_pose, EXPECT_THAT(nodes.at(NodeId{0, 0}).global_pose,
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
EXPECT_THAT(nodes.at(mapping::NodeId{0, 1}).global_pose, EXPECT_THAT(nodes.at(NodeId{0, 1}).global_pose,
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
EXPECT_THAT(nodes.at(mapping::NodeId{0, 2}).global_pose, EXPECT_THAT(nodes.at(NodeId{0, 2}).global_pose,
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
} }
TEST_F(PoseGraphTest, NoOverlappingNodes) { TEST_F(PoseGraph2DTest, NoOverlappingNodes) {
std::mt19937 rng(0); std::mt19937 rng(0);
std::uniform_real_distribution<double> distribution(-1., 1.); std::uniform_real_distribution<double> distribution(-1., 1.);
std::vector<transform::Rigid2d> poses; std::vector<transform::Rigid2d> poses;
@ -221,15 +220,15 @@ TEST_F(PoseGraphTest, NoOverlappingNodes) {
ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()), ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
::testing::ContainerEq(std::vector<int>{0})); ::testing::ContainerEq(std::vector<int>{0}));
for (int i = 0; i != 4; ++i) { for (int i = 0; i != 4; ++i) {
EXPECT_THAT(poses[i], EXPECT_THAT(
IsNearly(transform::Project2D( poses[i],
nodes.at(mapping::NodeId{0, i}).global_pose), IsNearly(transform::Project2D(nodes.at(NodeId{0, i}).global_pose),
1e-2)) 1e-2))
<< i; << i;
} }
} }
TEST_F(PoseGraphTest, ConsecutivelyOverlappingNodes) { TEST_F(PoseGraph2DTest, ConsecutivelyOverlappingNodes) {
std::mt19937 rng(0); std::mt19937 rng(0);
std::uniform_real_distribution<double> distribution(-1., 1.); std::uniform_real_distribution<double> distribution(-1., 1.);
std::vector<transform::Rigid2d> poses; std::vector<transform::Rigid2d> poses;
@ -242,15 +241,15 @@ TEST_F(PoseGraphTest, ConsecutivelyOverlappingNodes) {
ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()), ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
::testing::ContainerEq(std::vector<int>{0})); ::testing::ContainerEq(std::vector<int>{0}));
for (int i = 0; i != 5; ++i) { for (int i = 0; i != 5; ++i) {
EXPECT_THAT(poses[i], EXPECT_THAT(
IsNearly(transform::Project2D( poses[i],
nodes.at(mapping::NodeId{0, i}).global_pose), IsNearly(transform::Project2D(nodes.at(NodeId{0, i}).global_pose),
1e-2)) 1e-2))
<< i; << i;
} }
} }
TEST_F(PoseGraphTest, OverlappingNodes) { TEST_F(PoseGraph2DTest, OverlappingNodes) {
std::mt19937 rng(0); std::mt19937 rng(0);
std::uniform_real_distribution<double> distribution(-1., 1.); std::uniform_real_distribution<double> distribution(-1., 1.);
std::vector<transform::Rigid2d> ground_truth; std::vector<transform::Rigid2d> ground_truth;
@ -285,5 +284,5 @@ TEST_F(PoseGraphTest, OverlappingNodes) {
} }
} // namespace } // namespace
} // namespace mapping_2d } // namespace mapping
} // namespace cartographer } // namespace cartographer