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

Rename mapping_{2,3}d::SparsePoseGraph. (#678) 

[RFC=0001](https://github.com/googlecartographer/rfcs/blob/master/text/0001-renaming-sparse-pose-graph.md)
master
Wolfgang Hess 2017-11-15 13:50:18 +01:00 committed by Wally B. Feed
parent 8c114d6eaf
commit c25379cd20
7 changed files with 158 additions and 168 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
cartographer/mapping/map_builder.cc
View File

@ -60,14 +60,14 @@ MapBuilder::MapBuilder(
thread_pool_(options.num_background_threads()), thread_pool_(options.num_background_threads()),
local_slam_result_callback_(local_slam_result_callback) { local_slam_result_callback_(local_slam_result_callback) {
if (options.use_trajectory_builder_2d()) { if (options.use_trajectory_builder_2d()) {
sparse_pose_graph_2d_ = common::make_unique<mapping_2d::SparsePoseGraph>( pose_graph_2d_ = common::make_unique<mapping_2d::PoseGraph>(
options_.sparse_pose_graph_options(), &thread_pool_); options_.sparse_pose_graph_options(), &thread_pool_);
pose_graph_ = sparse_pose_graph_2d_.get(); pose_graph_ = pose_graph_2d_.get();
} }
if (options.use_trajectory_builder_3d()) { if (options.use_trajectory_builder_3d()) {
sparse_pose_graph_3d_ = common::make_unique<mapping_3d::SparsePoseGraph>( pose_graph_3d_ = common::make_unique<mapping_3d::PoseGraph>(
options_.sparse_pose_graph_options(), &thread_pool_); options_.sparse_pose_graph_options(), &thread_pool_);
pose_graph_ = sparse_pose_graph_3d_.get(); pose_graph_ = pose_graph_3d_.get();
} }
} }
@ -85,9 +85,9 @@ int MapBuilder::AddTrajectoryBuilder(
common::make_unique<mapping::GlobalTrajectoryBuilder< common::make_unique<mapping::GlobalTrajectoryBuilder<
mapping_3d::LocalTrajectoryBuilder, mapping_3d::LocalTrajectoryBuilder,
mapping_3d::proto::LocalTrajectoryBuilderOptions, mapping_3d::proto::LocalTrajectoryBuilderOptions,
mapping_3d::SparsePoseGraph>>( mapping_3d::PoseGraph>>(
trajectory_options.trajectory_builder_3d_options(), trajectory_options.trajectory_builder_3d_options(),
trajectory_id, sparse_pose_graph_3d_.get(), trajectory_id, pose_graph_3d_.get(),
local_slam_result_callback_))); local_slam_result_callback_)));
} else { } else {
CHECK(trajectory_options.has_trajectory_builder_2d_options()); CHECK(trajectory_options.has_trajectory_builder_2d_options());
@ -97,9 +97,9 @@ int MapBuilder::AddTrajectoryBuilder(
common::make_unique<mapping::GlobalTrajectoryBuilder< common::make_unique<mapping::GlobalTrajectoryBuilder<
mapping_2d::LocalTrajectoryBuilder, mapping_2d::LocalTrajectoryBuilder,
mapping_2d::proto::LocalTrajectoryBuilderOptions, mapping_2d::proto::LocalTrajectoryBuilderOptions,
mapping_2d::SparsePoseGraph>>( mapping_2d::PoseGraph>>(
trajectory_options.trajectory_builder_2d_options(), trajectory_options.trajectory_builder_2d_options(),
trajectory_id, sparse_pose_graph_2d_.get(), trajectory_id, pose_graph_2d_.get(),
local_slam_result_callback_))); local_slam_result_callback_)));
} }
if (trajectory_options.pure_localization()) { if (trajectory_options.pure_localization()) {

8
cartographer/mapping/map_builder.h
View File

@ -35,8 +35,8 @@
#include "cartographer/mapping/proto/trajectory_builder_options.pb.h" #include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
#include "cartographer/mapping/submaps.h" #include "cartographer/mapping/submaps.h"
#include "cartographer/mapping/trajectory_builder.h" #include "cartographer/mapping/trajectory_builder.h"
#include "cartographer/mapping_2d/sparse_pose_graph.h" #include "cartographer/mapping_2d/pose_graph.h"
#include "cartographer/mapping_3d/sparse_pose_graph.h" #include "cartographer/mapping_3d/pose_graph.h"
#include "cartographer/sensor/collator.h" #include "cartographer/sensor/collator.h"
namespace cartographer { namespace cartographer {
@ -101,8 +101,8 @@ class MapBuilder {
const proto::MapBuilderOptions options_; const proto::MapBuilderOptions options_;
common::ThreadPool thread_pool_; common::ThreadPool thread_pool_;
std::unique_ptr<mapping_2d::SparsePoseGraph> sparse_pose_graph_2d_; std::unique_ptr<mapping_2d::PoseGraph> pose_graph_2d_;
std::unique_ptr<mapping_3d::SparsePoseGraph> sparse_pose_graph_3d_; std::unique_ptr<mapping_3d::PoseGraph> pose_graph_3d_;
mapping::PoseGraph* pose_graph_; mapping::PoseGraph* pose_graph_;
LocalSlamResultCallback local_slam_result_callback_; LocalSlamResultCallback local_slam_result_callback_;

107
cartographer/mapping_2d/sparse_pose_graph.cc → cartographer/mapping_2d/pose_graph.cc
View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping_2d/sparse_pose_graph.h" #include "cartographer/mapping_2d/pose_graph.h"
#include <algorithm> #include <algorithm>
#include <cmath> #include <cmath>
@ -39,20 +39,19 @@
namespace cartographer { namespace cartographer {
namespace mapping_2d { namespace mapping_2d {
SparsePoseGraph::SparsePoseGraph( PoseGraph::PoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
const mapping::proto::SparsePoseGraphOptions& 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) {}
SparsePoseGraph::~SparsePoseGraph() { PoseGraph::~PoseGraph() {
WaitForAllComputations(); WaitForAllComputations();
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
CHECK(work_queue_ == nullptr); CHECK(work_queue_ == nullptr);
} }
std::vector<mapping::SubmapId> SparsePoseGraph::InitializeGlobalSubmapPoses( std::vector<mapping::SubmapId> PoseGraph::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 Submap>>& insertion_submaps) {
CHECK(!insertion_submaps.empty()); CHECK(!insertion_submaps.empty());
@ -99,7 +98,7 @@ std::vector<mapping::SubmapId> SparsePoseGraph::InitializeGlobalSubmapPoses(
return {front_submap_id, last_submap_id}; return {front_submap_id, last_submap_id};
} }
mapping::NodeId SparsePoseGraph::AddNode( mapping::NodeId PoseGraph::AddNode(
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data, std::shared_ptr<const mapping::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 Submap>>& insertion_submaps) {
@ -133,7 +132,7 @@ mapping::NodeId SparsePoseGraph::AddNode(
return node_id; return node_id;
} }
void SparsePoseGraph::AddWorkItem(const std::function<void()>& work_item) { void PoseGraph::AddWorkItem(const std::function<void()>& work_item) {
if (work_queue_ == nullptr) { if (work_queue_ == nullptr) {
work_item(); work_item();
} else { } else {
@ -141,7 +140,7 @@ void SparsePoseGraph::AddWorkItem(const std::function<void()>& work_item) {
} }
} }
void SparsePoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) { void PoseGraph::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]) {
@ -151,30 +150,30 @@ void SparsePoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) {
} }
} }
void SparsePoseGraph::AddImuData(const int trajectory_id, void PoseGraph::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 SparsePoseGraph::AddOdometryData( void PoseGraph::AddOdometryData(const int trajectory_id,
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 SparsePoseGraph::AddFixedFramePoseData( void PoseGraph::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 SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) { const mapping::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);
@ -206,7 +205,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
} }
} }
void SparsePoseGraph::ComputeConstraintsForOldNodes( void PoseGraph::ComputeConstraintsForOldNodes(
const mapping::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()) {
@ -217,7 +216,7 @@ void SparsePoseGraph::ComputeConstraintsForOldNodes(
} }
} }
void SparsePoseGraph::ComputeConstraintsForNode( void PoseGraph::ComputeConstraintsForNode(
const mapping::NodeId& node_id, const mapping::NodeId& node_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps, std::vector<std::shared_ptr<const Submap>> insertion_submaps,
const bool newly_finished_submap) { const bool newly_finished_submap) {
@ -282,7 +281,7 @@ void SparsePoseGraph::ComputeConstraintsForNode(
} }
} }
common::Time SparsePoseGraph::GetLatestNodeTime( common::Time PoseGraph::GetLatestNodeTime(
const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) const { const mapping::NodeId& node_id, const mapping::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);
@ -295,8 +294,7 @@ common::Time SparsePoseGraph::GetLatestNodeTime(
return time; return time;
} }
void SparsePoseGraph::UpdateTrajectoryConnectivity( void PoseGraph::UpdateTrajectoryConnectivity(const Constraint& constraint) {
const Constraint& constraint) {
CHECK_EQ(constraint.tag, mapping::PoseGraph::Constraint::INTER_SUBMAP); CHECK_EQ(constraint.tag, mapping::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);
@ -305,7 +303,7 @@ void SparsePoseGraph::UpdateTrajectoryConnectivity(
time); time);
} }
void SparsePoseGraph::HandleWorkQueue() { void PoseGraph::HandleWorkQueue() {
constraint_builder_.WhenDone( constraint_builder_.WhenDone(
[this](const pose_graph::ConstraintBuilder::Result& result) { [this](const pose_graph::ConstraintBuilder::Result& result) {
{ {
@ -339,7 +337,7 @@ void SparsePoseGraph::HandleWorkQueue() {
}); });
} }
void SparsePoseGraph::WaitForAllComputations() { void PoseGraph::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 =
@ -370,12 +368,12 @@ void SparsePoseGraph::WaitForAllComputations() {
locker.Await([&notification]() { return notification; }); locker.Await([&notification]() { return notification; });
} }
void SparsePoseGraph::FinishTrajectory(const int trajectory_id) { void PoseGraph::FinishTrajectory(const int trajectory_id) {
// TODO(jihoonl): Add a logic to notify trimmers to finish the given // TODO(jihoonl): Add a logic to notify trimmers to finish the given
// trajectory. // trajectory.
} }
void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) { void PoseGraph::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_) {
@ -384,9 +382,8 @@ void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
}); });
} }
void SparsePoseGraph::AddSubmapFromProto( void PoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
const transform::Rigid3d& global_submap_pose, const mapping::proto::Submap& submap) {
const mapping::proto::Submap& submap) {
if (!submap.has_submap_2d()) { if (!submap.has_submap_2d()) {
return; return;
} }
@ -412,8 +409,8 @@ void SparsePoseGraph::AddSubmapFromProto(
}); });
} }
void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose, void PoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Node& node) { const mapping::proto::Node& node) {
const mapping::NodeId node_id = {node.node_id().trajectory_id(), const mapping::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 mapping::TrajectoryNode::Data> constant_data =
@ -439,15 +436,15 @@ void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
}); });
} }
void SparsePoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id, void PoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) { const mapping::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 SparsePoseGraph::AddSerializedConstraints( void PoseGraph::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_) {
@ -479,8 +476,7 @@ void SparsePoseGraph::AddSerializedConstraints(
}); });
} }
void SparsePoseGraph::AddTrimmer( void PoseGraph::AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) {
std::unique_ptr<mapping::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(); mapping::PoseGraphTrimmer* const trimmer_ptr = trimmer.release();
@ -488,7 +484,7 @@ void SparsePoseGraph::AddTrimmer(
REQUIRES(mutex_) { trimmers_.emplace_back(trimmer_ptr); }); REQUIRES(mutex_) { trimmers_.emplace_back(trimmer_ptr); });
} }
void SparsePoseGraph::RunFinalOptimization() { void PoseGraph::RunFinalOptimization() {
WaitForAllComputations(); WaitForAllComputations();
optimization_problem_.SetMaxNumIterations( optimization_problem_.SetMaxNumIterations(
options_.max_num_final_iterations()); options_.max_num_final_iterations());
@ -499,7 +495,7 @@ void SparsePoseGraph::RunFinalOptimization() {
.max_num_iterations()); .max_num_iterations());
} }
void SparsePoseGraph::RunOptimization() { void PoseGraph::RunOptimization() {
if (optimization_problem_.submap_data().empty()) { if (optimization_problem_.submap_data().empty()) {
return; return;
} }
@ -544,22 +540,22 @@ void SparsePoseGraph::RunOptimization() {
} }
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode> mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
SparsePoseGraph::GetTrajectoryNodes() { PoseGraph::GetTrajectoryNodes() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return trajectory_nodes_; return trajectory_nodes_;
} }
sensor::MapByTime<sensor::ImuData> SparsePoseGraph::GetImuData() { sensor::MapByTime<sensor::ImuData> PoseGraph::GetImuData() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return optimization_problem_.imu_data(); return optimization_problem_.imu_data();
} }
sensor::MapByTime<sensor::OdometryData> SparsePoseGraph::GetOdometryData() { sensor::MapByTime<sensor::OdometryData> PoseGraph::GetOdometryData() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return optimization_problem_.odometry_data(); return optimization_problem_.odometry_data();
} }
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() { std::vector<PoseGraph::Constraint> PoseGraph::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_) {
@ -576,16 +572,16 @@ std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() {
return result; return result;
} }
void SparsePoseGraph::SetInitialTrajectoryPose(const int from_trajectory_id, void PoseGraph::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 SparsePoseGraph::GetInterpolatedGlobalTrajectoryPose( transform::Rigid3d PoseGraph::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);
@ -605,25 +601,25 @@ transform::Rigid3d SparsePoseGraph::GetInterpolatedGlobalTrajectoryPose(
.transform; .transform;
} }
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( transform::Rigid3d PoseGraph::GetLocalToGlobalTransform(
const int trajectory_id) { const int trajectory_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return ComputeLocalToGlobalTransform(optimized_submap_transforms_, return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
trajectory_id); trajectory_id);
} }
std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() { std::vector<std::vector<int>> PoseGraph::GetConnectedTrajectories() {
return trajectory_connectivity_state_.Components(); return trajectory_connectivity_state_.Components();
} }
mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapData( mapping::PoseGraph::SubmapData PoseGraph::GetSubmapData(
const mapping::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::PoseGraph::SubmapData> mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData>
SparsePoseGraph::GetAllSubmapData() { PoseGraph::GetAllSubmapData() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData> submaps; mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData> submaps;
for (const auto& submap_id_data : submap_data_) { for (const auto& submap_id_data : submap_data_) {
@ -633,7 +629,7 @@ SparsePoseGraph::GetAllSubmapData() {
return submaps; return submaps;
} }
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( transform::Rigid3d PoseGraph::ComputeLocalToGlobalTransform(
const mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>& const mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>&
submap_transforms, submap_transforms,
const int trajectory_id) const { const int trajectory_id) const {
@ -658,7 +654,7 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
.inverse(); .inverse();
} }
mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( mapping::PoseGraph::SubmapData PoseGraph::GetSubmapDataUnderLock(
const mapping::SubmapId& submap_id) { const mapping::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()) {
@ -677,16 +673,15 @@ mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
submap->local_pose()}; submap->local_pose()};
} }
SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent) PoseGraph::TrimmingHandle::TrimmingHandle(PoseGraph* const parent)
: parent_(parent) {} : parent_(parent) {}
int SparsePoseGraph::TrimmingHandle::num_submaps( int PoseGraph::TrimmingHandle::num_submaps(const int trajectory_id) const {
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);
} }
void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( void PoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
const mapping::SubmapId& submap_id) { const mapping::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.

22
cartographer/mapping_2d/sparse_pose_graph.h → cartographer/mapping_2d/pose_graph.h
View File

@ -14,8 +14,8 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_ #ifndef CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_H_
#define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_ #define CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_H_
#include <deque> #include <deque>
#include <functional> #include <functional>
@ -56,14 +56,14 @@ 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 SparsePoseGraph : public mapping::PoseGraph { class PoseGraph : public mapping::PoseGraph {
public: public:
SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options, PoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
common::ThreadPool* thread_pool); common::ThreadPool* thread_pool);
~SparsePoseGraph() override; ~PoseGraph() override;
SparsePoseGraph(const SparsePoseGraph&) = delete; PoseGraph(const PoseGraph&) = delete;
SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; PoseGraph& operator=(const PoseGraph&) = 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
@ -245,7 +245,7 @@ class SparsePoseGraph : public mapping::PoseGraph {
// '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 mapping::Trimmable {
public: public:
TrimmingHandle(SparsePoseGraph* parent); TrimmingHandle(PoseGraph* parent);
~TrimmingHandle() override {} ~TrimmingHandle() override {}
int num_submaps(int trajectory_id) const override; int num_submaps(int trajectory_id) const override;
@ -253,11 +253,11 @@ class SparsePoseGraph : public mapping::PoseGraph {
REQUIRES(parent_->mutex_) override; REQUIRES(parent_->mutex_) override;
private: private:
SparsePoseGraph* const parent_; PoseGraph* const parent_;
}; };
}; };
} // namespace mapping_2d } // namespace mapping_2d
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_H_ #endif // CARTOGRAPHER_MAPPING_2D_POSE_GRAPH_H_

42
cartographer/mapping_2d/sparse_pose_graph_test.cc → cartographer/mapping_2d/pose_graph_test.cc
View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping_2d/sparse_pose_graph.h" #include "cartographer/mapping_2d/pose_graph.h"
#include <cmath> #include <cmath>
#include <memory> #include <memory>
@ -35,9 +35,9 @@ namespace cartographer {
namespace mapping_2d { namespace mapping_2d {
namespace { namespace {
class SparsePoseGraphTest : public ::testing::Test { class PoseGraphTest : public ::testing::Test {
protected: protected:
SparsePoseGraphTest() : thread_pool_(1) { PoseGraphTest() : 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 (>
@ -132,7 +132,7 @@ class SparsePoseGraphTest : 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");
sparse_pose_graph_ = common::make_unique<SparsePoseGraph>( pose_graph_ = common::make_unique<PoseGraph>(
mapping::CreateSparsePoseGraphOptions(parameter_dictionary.get()), mapping::CreateSparsePoseGraphOptions(parameter_dictionary.get()),
&thread_pool_); &thread_pool_);
} }
@ -157,7 +157,7 @@ class SparsePoseGraphTest : public ::testing::Test {
active_submaps_->InsertRangeData(TransformRangeData( active_submaps_->InsertRangeData(TransformRangeData(
range_data, transform::Embed3D(pose_estimate.cast<float>()))); range_data, transform::Embed3D(pose_estimate.cast<float>())));
sparse_pose_graph_->AddNode( pose_graph_->AddNode(
std::make_shared<const mapping::TrajectoryNode::Data>( std::make_shared<const mapping::TrajectoryNode::Data>(
mapping::TrajectoryNode::Data{common::FromUniversal(0), mapping::TrajectoryNode::Data{common::FromUniversal(0),
Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
@ -181,22 +181,22 @@ class SparsePoseGraphTest : public ::testing::Test {
sensor::PointCloud point_cloud_; sensor::PointCloud point_cloud_;
std::unique_ptr<ActiveSubmaps> active_submaps_; std::unique_ptr<ActiveSubmaps> active_submaps_;
common::ThreadPool thread_pool_; common::ThreadPool thread_pool_;
std::unique_ptr<SparsePoseGraph> sparse_pose_graph_; std::unique_ptr<PoseGraph> pose_graph_;
transform::Rigid2d current_pose_; transform::Rigid2d current_pose_;
}; };
TEST_F(SparsePoseGraphTest, EmptyMap) { TEST_F(PoseGraphTest, EmptyMap) {
sparse_pose_graph_->RunFinalOptimization(); pose_graph_->RunFinalOptimization();
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); const auto nodes = pose_graph_->GetTrajectoryNodes();
EXPECT_TRUE(nodes.empty()); EXPECT_TRUE(nodes.empty());
} }
TEST_F(SparsePoseGraphTest, NoMovement) { TEST_F(PoseGraphTest, 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());
sparse_pose_graph_->RunFinalOptimization(); pose_graph_->RunFinalOptimization();
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); const auto nodes = pose_graph_->GetTrajectoryNodes();
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));
@ -208,7 +208,7 @@ TEST_F(SparsePoseGraphTest, NoMovement) {
transform::IsNearly(transform::Rigid3d::Identity(), 1e-2)); transform::IsNearly(transform::Rigid3d::Identity(), 1e-2));
} }
TEST_F(SparsePoseGraphTest, NoOverlappingNodes) { TEST_F(PoseGraphTest, 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;
@ -216,8 +216,8 @@ TEST_F(SparsePoseGraphTest, NoOverlappingNodes) {
MoveRelative(transform::Rigid2d({0.25 * distribution(rng), 5.}, 0.)); MoveRelative(transform::Rigid2d({0.25 * distribution(rng), 5.}, 0.));
poses.emplace_back(current_pose_); poses.emplace_back(current_pose_);
} }
sparse_pose_graph_->RunFinalOptimization(); pose_graph_->RunFinalOptimization();
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); const auto nodes = pose_graph_->GetTrajectoryNodes();
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) {
@ -229,7 +229,7 @@ TEST_F(SparsePoseGraphTest, NoOverlappingNodes) {
} }
} }
TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingNodes) { TEST_F(PoseGraphTest, 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;
@ -237,8 +237,8 @@ TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingNodes) {
MoveRelative(transform::Rigid2d({0.25 * distribution(rng), 2.}, 0.)); MoveRelative(transform::Rigid2d({0.25 * distribution(rng), 2.}, 0.));
poses.emplace_back(current_pose_); poses.emplace_back(current_pose_);
} }
sparse_pose_graph_->RunFinalOptimization(); pose_graph_->RunFinalOptimization();
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); const auto nodes = pose_graph_->GetTrajectoryNodes();
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) {
@ -250,7 +250,7 @@ TEST_F(SparsePoseGraphTest, ConsecutivelyOverlappingNodes) {
} }
} }
TEST_F(SparsePoseGraphTest, OverlappingNodes) { TEST_F(PoseGraphTest, 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;
@ -265,8 +265,8 @@ TEST_F(SparsePoseGraphTest, OverlappingNodes) {
ground_truth.emplace_back(current_pose_); ground_truth.emplace_back(current_pose_);
poses.emplace_back(noise * current_pose_); poses.emplace_back(noise * current_pose_);
} }
sparse_pose_graph_->RunFinalOptimization(); pose_graph_->RunFinalOptimization();
const auto nodes = sparse_pose_graph_->GetTrajectoryNodes(); const auto nodes = pose_graph_->GetTrajectoryNodes();
ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()), ASSERT_THAT(ToVectorInt(nodes.trajectory_ids()),
::testing::ContainerEq(std::vector<int>{0})); ::testing::ContainerEq(std::vector<int>{0}));
transform::Rigid2d true_movement = transform::Rigid2d true_movement =

109
cartographer/mapping_3d/sparse_pose_graph.cc → cartographer/mapping_3d/pose_graph.cc
View File

@ -14,7 +14,7 @@
* limitations under the License. * limitations under the License.
*/ */
#include "cartographer/mapping_3d/sparse_pose_graph.h" #include "cartographer/mapping_3d/pose_graph.h"
#include <algorithm> #include <algorithm>
#include <cmath> #include <cmath>
@ -38,21 +38,20 @@
namespace cartographer { namespace cartographer {
namespace mapping_3d { namespace mapping_3d {
SparsePoseGraph::SparsePoseGraph( PoseGraph::PoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
const mapping::proto::SparsePoseGraphOptions& 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(),
pose_graph::OptimizationProblem::FixZ::kNo), pose_graph::OptimizationProblem::FixZ::kNo),
constraint_builder_(options_.constraint_builder_options(), thread_pool) {} constraint_builder_(options_.constraint_builder_options(), thread_pool) {}
SparsePoseGraph::~SparsePoseGraph() { PoseGraph::~PoseGraph() {
WaitForAllComputations(); WaitForAllComputations();
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
CHECK(work_queue_ == nullptr); CHECK(work_queue_ == nullptr);
} }
std::vector<mapping::SubmapId> SparsePoseGraph::InitializeGlobalSubmapPoses( std::vector<mapping::SubmapId> PoseGraph::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 Submap>>& insertion_submaps) {
CHECK(!insertion_submaps.empty()); CHECK(!insertion_submaps.empty());
@ -97,7 +96,7 @@ std::vector<mapping::SubmapId> SparsePoseGraph::InitializeGlobalSubmapPoses(
return {front_submap_id, last_submap_id}; return {front_submap_id, last_submap_id};
} }
mapping::NodeId SparsePoseGraph::AddNode( mapping::NodeId PoseGraph::AddNode(
std::shared_ptr<const mapping::TrajectoryNode::Data> constant_data, std::shared_ptr<const mapping::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 Submap>>& insertion_submaps) {
@ -131,7 +130,7 @@ mapping::NodeId SparsePoseGraph::AddNode(
return node_id; return node_id;
} }
void SparsePoseGraph::AddWorkItem(const std::function<void()>& work_item) { void PoseGraph::AddWorkItem(const std::function<void()>& work_item) {
if (work_queue_ == nullptr) { if (work_queue_ == nullptr) {
work_item(); work_item();
} else { } else {
@ -139,7 +138,7 @@ void SparsePoseGraph::AddWorkItem(const std::function<void()>& work_item) {
} }
} }
void SparsePoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) { void PoseGraph::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]) {
@ -149,23 +148,23 @@ void SparsePoseGraph::AddTrajectoryIfNeeded(const int trajectory_id) {
} }
} }
void SparsePoseGraph::AddImuData(const int trajectory_id, void PoseGraph::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 SparsePoseGraph::AddOdometryData( void PoseGraph::AddOdometryData(const int trajectory_id,
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 SparsePoseGraph::AddFixedFramePoseData( void PoseGraph::AddFixedFramePoseData(
const int trajectory_id, const int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) { const sensor::FixedFramePoseData& fixed_frame_pose_data) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
@ -175,8 +174,8 @@ void SparsePoseGraph::AddFixedFramePoseData(
}); });
} }
void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) { const mapping::SubmapId& submap_id) {
CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished); CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished);
const transform::Rigid3d global_node_pose = const transform::Rigid3d global_node_pose =
@ -227,7 +226,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
} }
} }
void SparsePoseGraph::ComputeConstraintsForOldNodes( void PoseGraph::ComputeConstraintsForOldNodes(
const mapping::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()) {
@ -238,7 +237,7 @@ void SparsePoseGraph::ComputeConstraintsForOldNodes(
} }
} }
void SparsePoseGraph::ComputeConstraintsForNode( void PoseGraph::ComputeConstraintsForNode(
const mapping::NodeId& node_id, const mapping::NodeId& node_id,
std::vector<std::shared_ptr<const Submap>> insertion_submaps, std::vector<std::shared_ptr<const Submap>> insertion_submaps,
const bool newly_finished_submap) { const bool newly_finished_submap) {
@ -299,7 +298,7 @@ void SparsePoseGraph::ComputeConstraintsForNode(
} }
} }
common::Time SparsePoseGraph::GetLatestNodeTime( common::Time PoseGraph::GetLatestNodeTime(
const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) const { const mapping::NodeId& node_id, const mapping::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);
@ -312,8 +311,7 @@ common::Time SparsePoseGraph::GetLatestNodeTime(
return time; return time;
} }
void SparsePoseGraph::UpdateTrajectoryConnectivity( void PoseGraph::UpdateTrajectoryConnectivity(const Constraint& constraint) {
const Constraint& constraint) {
CHECK_EQ(constraint.tag, mapping::PoseGraph::Constraint::INTER_SUBMAP); CHECK_EQ(constraint.tag, mapping::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);
@ -322,7 +320,7 @@ void SparsePoseGraph::UpdateTrajectoryConnectivity(
time); time);
} }
void SparsePoseGraph::HandleWorkQueue() { void PoseGraph::HandleWorkQueue() {
constraint_builder_.WhenDone( constraint_builder_.WhenDone(
[this](const pose_graph::ConstraintBuilder::Result& result) { [this](const pose_graph::ConstraintBuilder::Result& result) {
{ {
@ -356,7 +354,7 @@ void SparsePoseGraph::HandleWorkQueue() {
}); });
} }
void SparsePoseGraph::WaitForAllComputations() { void PoseGraph::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 =
@ -387,12 +385,12 @@ void SparsePoseGraph::WaitForAllComputations() {
locker.Await([&notification]() { return notification; }); locker.Await([&notification]() { return notification; });
} }
void SparsePoseGraph::FinishTrajectory(const int trajectory_id) { void PoseGraph::FinishTrajectory(const int trajectory_id) {
// TODO(jihoonl): Add a logic to notify trimmers to finish the given // TODO(jihoonl): Add a logic to notify trimmers to finish the given
// trajectory. // trajectory.
} }
void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) { void PoseGraph::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_) {
@ -401,9 +399,8 @@ void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
}); });
} }
void SparsePoseGraph::AddSubmapFromProto( void PoseGraph::AddSubmapFromProto(const transform::Rigid3d& global_submap_pose,
const transform::Rigid3d& global_submap_pose, const mapping::proto::Submap& submap) {
const mapping::proto::Submap& submap) {
if (!submap.has_submap_3d()) { if (!submap.has_submap_3d()) {
return; return;
} }
@ -427,8 +424,8 @@ void SparsePoseGraph::AddSubmapFromProto(
}); });
} }
void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose, void PoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
const mapping::proto::Node& node) { const mapping::proto::Node& node) {
const mapping::NodeId node_id = {node.node_id().trajectory_id(), const mapping::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 mapping::TrajectoryNode::Data> constant_data =
@ -448,15 +445,15 @@ void SparsePoseGraph::AddNodeFromProto(const transform::Rigid3d& global_pose,
}); });
} }
void SparsePoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id, void PoseGraph::AddNodeToSubmap(const mapping::NodeId& node_id,
const mapping::SubmapId& submap_id) { const mapping::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 SparsePoseGraph::AddSerializedConstraints( void PoseGraph::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_) {
@ -481,8 +478,7 @@ void SparsePoseGraph::AddSerializedConstraints(
}); });
} }
void SparsePoseGraph::AddTrimmer( void PoseGraph::AddTrimmer(std::unique_ptr<mapping::PoseGraphTrimmer> trimmer) {
std::unique_ptr<mapping::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(); mapping::PoseGraphTrimmer* const trimmer_ptr = trimmer.release();
@ -490,7 +486,7 @@ void SparsePoseGraph::AddTrimmer(
REQUIRES(mutex_) { trimmers_.emplace_back(trimmer_ptr); }); REQUIRES(mutex_) { trimmers_.emplace_back(trimmer_ptr); });
} }
void SparsePoseGraph::RunFinalOptimization() { void PoseGraph::RunFinalOptimization() {
WaitForAllComputations(); WaitForAllComputations();
optimization_problem_.SetMaxNumIterations( optimization_problem_.SetMaxNumIterations(
options_.max_num_final_iterations()); options_.max_num_final_iterations());
@ -501,7 +497,7 @@ void SparsePoseGraph::RunFinalOptimization() {
.max_num_iterations()); .max_num_iterations());
} }
void SparsePoseGraph::LogResidualHistograms() { void PoseGraph::LogResidualHistograms() {
common::Histogram rotational_residual; common::Histogram rotational_residual;
common::Histogram translational_residual; common::Histogram translational_residual;
for (const Constraint& constraint : constraints_) { for (const Constraint& constraint : constraints_) {
@ -527,7 +523,7 @@ void SparsePoseGraph::LogResidualHistograms() {
<< rotational_residual.ToString(10); << rotational_residual.ToString(10);
} }
void SparsePoseGraph::RunOptimization() { void PoseGraph::RunOptimization() {
if (optimization_problem_.submap_data().empty()) { if (optimization_problem_.submap_data().empty()) {
return; return;
} }
@ -573,36 +569,36 @@ void SparsePoseGraph::RunOptimization() {
} }
mapping::MapById<mapping::NodeId, mapping::TrajectoryNode> mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
SparsePoseGraph::GetTrajectoryNodes() { PoseGraph::GetTrajectoryNodes() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return trajectory_nodes_; return trajectory_nodes_;
} }
sensor::MapByTime<sensor::ImuData> SparsePoseGraph::GetImuData() { sensor::MapByTime<sensor::ImuData> PoseGraph::GetImuData() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return optimization_problem_.imu_data(); return optimization_problem_.imu_data();
} }
sensor::MapByTime<sensor::OdometryData> SparsePoseGraph::GetOdometryData() { sensor::MapByTime<sensor::OdometryData> PoseGraph::GetOdometryData() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return optimization_problem_.odometry_data(); return optimization_problem_.odometry_data();
} }
std::vector<SparsePoseGraph::Constraint> SparsePoseGraph::constraints() { std::vector<PoseGraph::Constraint> PoseGraph::constraints() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return constraints_; return constraints_;
} }
void SparsePoseGraph::SetInitialTrajectoryPose(const int from_trajectory_id, void PoseGraph::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 SparsePoseGraph::GetInterpolatedGlobalTrajectoryPose( transform::Rigid3d PoseGraph::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);
@ -622,25 +618,25 @@ transform::Rigid3d SparsePoseGraph::GetInterpolatedGlobalTrajectoryPose(
.transform; .transform;
} }
transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( transform::Rigid3d PoseGraph::GetLocalToGlobalTransform(
const int trajectory_id) { const int trajectory_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
return ComputeLocalToGlobalTransform(optimized_submap_transforms_, return ComputeLocalToGlobalTransform(optimized_submap_transforms_,
trajectory_id); trajectory_id);
} }
std::vector<std::vector<int>> SparsePoseGraph::GetConnectedTrajectories() { std::vector<std::vector<int>> PoseGraph::GetConnectedTrajectories() {
return trajectory_connectivity_state_.Components(); return trajectory_connectivity_state_.Components();
} }
mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapData( mapping::PoseGraph::SubmapData PoseGraph::GetSubmapData(
const mapping::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::PoseGraph::SubmapData> mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData>
SparsePoseGraph::GetAllSubmapData() { PoseGraph::GetAllSubmapData() {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData> submaps; mapping::MapById<mapping::SubmapId, mapping::PoseGraph::SubmapData> submaps;
for (const auto& submap_id_data : submap_data_) { for (const auto& submap_id_data : submap_data_) {
@ -650,7 +646,7 @@ SparsePoseGraph::GetAllSubmapData() {
return submaps; return submaps;
} }
transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( transform::Rigid3d PoseGraph::ComputeLocalToGlobalTransform(
const mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>& const mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>&
submap_transforms, submap_transforms,
const int trajectory_id) const { const int trajectory_id) const {
@ -674,7 +670,7 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform(
.inverse(); .inverse();
} }
mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock( mapping::PoseGraph::SubmapData PoseGraph::GetSubmapDataUnderLock(
const mapping::SubmapId& submap_id) { const mapping::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()) {
@ -691,16 +687,15 @@ mapping::PoseGraph::SubmapData SparsePoseGraph::GetSubmapDataUnderLock(
submap->local_pose()}; submap->local_pose()};
} }
SparsePoseGraph::TrimmingHandle::TrimmingHandle(SparsePoseGraph* const parent) PoseGraph::TrimmingHandle::TrimmingHandle(PoseGraph* const parent)
: parent_(parent) {} : parent_(parent) {}
int SparsePoseGraph::TrimmingHandle::num_submaps( int PoseGraph::TrimmingHandle::num_submaps(const int trajectory_id) const {
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);
} }
void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( void PoseGraph::TrimmingHandle::MarkSubmapAsTrimmed(
const mapping::SubmapId& submap_id) { const mapping::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.

22
cartographer/mapping_3d/sparse_pose_graph.h → cartographer/mapping_3d/pose_graph.h
View File

@ -14,8 +14,8 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_ #ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_H_
#define CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_ #define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_H_
#include <deque> #include <deque>
#include <functional> #include <functional>
@ -56,14 +56,14 @@ namespace mapping_3d {
// 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 SparsePoseGraph : public mapping::PoseGraph { class PoseGraph : public mapping::PoseGraph {
public: public:
SparsePoseGraph(const mapping::proto::SparsePoseGraphOptions& options, PoseGraph(const mapping::proto::SparsePoseGraphOptions& options,
common::ThreadPool* thread_pool); common::ThreadPool* thread_pool);
~SparsePoseGraph() override; ~PoseGraph() override;
SparsePoseGraph(const SparsePoseGraph&) = delete; PoseGraph(const PoseGraph&) = delete;
SparsePoseGraph& operator=(const SparsePoseGraph&) = delete; PoseGraph& operator=(const PoseGraph&) = 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
@ -249,7 +249,7 @@ class SparsePoseGraph : public mapping::PoseGraph {
// '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 mapping::Trimmable {
public: public:
TrimmingHandle(SparsePoseGraph* parent); TrimmingHandle(PoseGraph* parent);
~TrimmingHandle() override {} ~TrimmingHandle() override {}
int num_submaps(int trajectory_id) const override; int num_submaps(int trajectory_id) const override;
@ -257,11 +257,11 @@ class SparsePoseGraph : public mapping::PoseGraph {
REQUIRES(parent_->mutex_) override; REQUIRES(parent_->mutex_) override;
private: private:
SparsePoseGraph* const parent_; PoseGraph* const parent_;
}; };
}; };
} // namespace mapping_3d } // namespace mapping_3d
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_3D_SPARSE_POSE_GRAPH_H_ #endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_H_