From eb96c91473764e41d86832741b46a76406822bc2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Wed, 8 Nov 2017 14:01:38 +0100 Subject: [PATCH] Initial pose implementation. (#606) --- cartographer/mapping/id.h | 3 +- cartographer/mapping/map_builder.cc | 9 +++ .../proto/trajectory_builder_options.proto | 8 +++ cartographer/mapping/sparse_pose_graph.h | 13 ++++ cartographer/mapping_2d/sparse_pose_graph.cc | 59 ++++++++++++++++--- cartographer/mapping_2d/sparse_pose_graph.h | 14 ++++- cartographer/mapping_3d/sparse_pose_graph.cc | 59 ++++++++++++++++--- cartographer/mapping_3d/sparse_pose_graph.h | 14 ++++- 8 files changed, 158 insertions(+), 21 deletions(-) diff --git a/cartographer/mapping/id.h b/cartographer/mapping/id.h index 359bf96..ff0b1f7 100644 --- a/cartographer/mapping/id.h +++ b/cartographer/mapping/id.h @@ -328,7 +328,8 @@ class MapById { // trajectory 'trajectory_id' whose time is not considered to go before // 'time', or EndOfTrajectory(trajectory_id) if all keys are considered to go // before 'time'. - ConstIterator lower_bound(const int trajectory_id, const common::Time& time) { + ConstIterator lower_bound(const int trajectory_id, + const common::Time time) const { if (SizeOfTrajectoryOrZero(trajectory_id) == 0) { return EndOfTrajectory(trajectory_id); } diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index 183e202..05d30dc 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -23,6 +23,7 @@ #include #include "cartographer/common/make_unique.h" +#include "cartographer/common/time.h" #include "cartographer/mapping/collated_trajectory_builder.h" #include "cartographer/mapping/global_trajectory_builder.h" #include "cartographer/mapping_2d/local_trajectory_builder.h" @@ -100,6 +101,14 @@ int MapBuilder::AddTrajectoryBuilder( sparse_pose_graph_->AddTrimmer(common::make_unique( trajectory_id, kSubmapsToKeep)); } + if (trajectory_options.has_initial_trajectory_pose()) { + const auto& initial_trajectory_pose = + trajectory_options.initial_trajectory_pose(); + sparse_pose_graph_->SetInitialTrajectoryPose( + trajectory_id, initial_trajectory_pose.to_trajectory_id(), + transform::ToRigid3(initial_trajectory_pose.relative_pose()), + common::FromUniversal(initial_trajectory_pose.timestamp())); + } return trajectory_id; } diff --git a/cartographer/mapping/proto/trajectory_builder_options.proto b/cartographer/mapping/proto/trajectory_builder_options.proto index 5366a6f..352b3df 100644 --- a/cartographer/mapping/proto/trajectory_builder_options.proto +++ b/cartographer/mapping/proto/trajectory_builder_options.proto @@ -14,15 +14,23 @@ syntax = "proto2"; +import "cartographer/transform/proto/transform.proto"; import "cartographer/mapping_2d/proto/local_trajectory_builder_options.proto"; import "cartographer/mapping_3d/proto/local_trajectory_builder_options.proto"; package cartographer.mapping.proto; +message InitialTrajectoryPose { + optional transform.proto.Rigid3d relative_pose = 1; + optional int32 to_trajectory_id = 2; + optional int64 timestamp = 3; +} + message TrajectoryBuilderOptions { optional mapping_2d.proto.LocalTrajectoryBuilderOptions trajectory_builder_2d_options = 1; optional mapping_3d.proto.LocalTrajectoryBuilderOptions trajectory_builder_3d_options = 2; optional bool pure_localization = 3; + optional InitialTrajectoryPose initial_trajectory_pose = 4; } diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index 99acf63..14f866a 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -68,6 +68,12 @@ class SparsePoseGraph { transform::Rigid3d pose; }; + struct InitialTrajectoryPose { + int to_trajectory_id; + transform::Rigid3d relative_pose; + common::Time time; + }; + SparsePoseGraph() {} virtual ~SparsePoseGraph() {} @@ -128,6 +134,13 @@ class SparsePoseGraph { // Returns the collection of constraints. virtual std::vector constraints() = 0; + + // Sets a relative initial pose 'relative_pose' for 'from_trajectory_id' with + // respect to 'to_trajectory_id' at time 'time'. + virtual void SetInitialTrajectoryPose(int from_trajectory_id, + int to_trajectory_id, + const transform::Rigid3d& pose, + const common::Time time) = 0; }; std::vector FromProto( diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 645b6e2..35b53b1 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -52,17 +52,24 @@ SparsePoseGraph::~SparsePoseGraph() { CHECK(work_queue_ == nullptr); } -std::vector SparsePoseGraph::GrowSubmapTransformsAsNeeded( - const int trajectory_id, +std::vector SparsePoseGraph::InitializeGlobalSubmapPoses( + const int trajectory_id, const common::Time time, const std::vector>& insertion_submaps) { CHECK(!insertion_submaps.empty()); const auto& submap_data = optimization_problem_.submap_data(); if (insertion_submaps.size() == 1) { // If we don't already have an entry for the first submap, add one. if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) { + if (initial_trajectory_poses_.count(trajectory_id) > 0) { + trajectory_connectivity_state_.Connect( + trajectory_id, + initial_trajectory_poses_.at(trajectory_id).to_trajectory_id, time); + } optimization_problem_.AddSubmap( - trajectory_id, - sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0])); + trajectory_id, transform::Project2D( + ComputeLocalToGlobalTransform( + optimized_submap_transforms_, trajectory_id) * + insertion_submaps[0]->local_pose())); } CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id)); const mapping::SubmapId submap_id{trajectory_id, 0}; @@ -213,11 +220,11 @@ void SparsePoseGraph::ComputeConstraintsForScan( const mapping::NodeId& node_id, std::vector> insertion_submaps, const bool newly_finished_submap) { - const std::vector submap_ids = - GrowSubmapTransformsAsNeeded(node_id.trajectory_id, insertion_submaps); + const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; + const std::vector submap_ids = InitializeGlobalSubmapPoses( + node_id.trajectory_id, constant_data->time, insertion_submaps); CHECK_EQ(submap_ids.size(), insertion_submaps.size()); const mapping::SubmapId matching_id = submap_ids.front(); - const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; const transform::Rigid2d pose = transform::Project2D( constant_data->local_pose * transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse())); @@ -549,6 +556,35 @@ std::vector SparsePoseGraph::constraints() { return result; } +void SparsePoseGraph::SetInitialTrajectoryPose(const int from_trajectory_id, + const int to_trajectory_id, + const transform::Rigid3d& pose, + const common::Time time) { + common::MutexLocker locker(&mutex_); + initial_trajectory_poses_[from_trajectory_id] = + InitialTrajectoryPose{to_trajectory_id, pose, time}; +} + +transform::Rigid3d SparsePoseGraph::GetInterpolatedGlobalTrajectoryPose( + const int trajectory_id, const common::Time time) const { + CHECK(trajectory_nodes_.SizeOfTrajectoryOrZero(trajectory_id) > 0); + const auto it = trajectory_nodes_.lower_bound(trajectory_id, time); + if (it == trajectory_nodes_.BeginOfTrajectory(trajectory_id)) { + return trajectory_nodes_.BeginOfTrajectory(trajectory_id)->data.global_pose; + } + if (it == trajectory_nodes_.EndOfTrajectory(trajectory_id)) { + return std::prev(trajectory_nodes_.EndOfTrajectory(trajectory_id)) + ->data.global_pose; + } + return transform::Interpolate( + transform::TimestampedTransform{std::prev(it)->data.time(), + std::prev(it)->data.global_pose}, + transform::TimestampedTransform{it->data.time(), + it->data.global_pose}, + time) + .transform; +} + transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( const int trajectory_id) { common::MutexLocker locker(&mutex_); @@ -585,7 +621,14 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( auto begin_it = submap_transforms.BeginOfTrajectory(trajectory_id); auto end_it = submap_transforms.EndOfTrajectory(trajectory_id); if (begin_it == end_it) { - return transform::Rigid3d::Identity(); + const auto it = initial_trajectory_poses_.find(trajectory_id); + if (it != initial_trajectory_poses_.end()) { + return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id, + it->second.time) * + it->second.relative_pose; + } else { + return transform::Rigid3d::Identity(); + } } const mapping::SubmapId last_optimized_submap_id = std::prev(end_it)->id; // Accessing 'local_pose' in Submap is okay, since the member is const. diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index eeadc20..7d57a3a 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -104,6 +104,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { mapping::MapById GetTrajectoryNodes() override EXCLUDES(mutex_); std::vector constraints() override EXCLUDES(mutex_); + void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id, + const transform::Rigid3d& pose, + const common::Time time) override + EXCLUDES(mutex_); + transform::Rigid3d GetInterpolatedGlobalTrajectoryPose( + int trajectory_id, const common::Time time) const REQUIRES(mutex_); private: // The current state of the submap in the background threads. When this @@ -129,8 +135,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Grows the optimization problem to have an entry for every element of // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'. - std::vector GrowSubmapTransformsAsNeeded( - int trajectory_id, + std::vector InitializeGlobalSubmapPoses( + int trajectory_id, const common::Time time, const std::vector>& insertion_submaps) REQUIRES(mutex_); @@ -226,6 +232,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Set of all frozen trajectories not being optimized. std::set frozen_trajectories_ GUARDED_BY(mutex_); + // Set of all initial trajectory poses. + std::map initial_trajectory_poses_ + GUARDED_BY(mutex_); + // Allows querying and manipulating the pose graph by the 'trimmers_'. The // 'mutex_' of the pose graph is held while this class is used. class TrimmingHandle : public mapping::Trimmable { diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index d7b42f0..76e9916 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -52,16 +52,23 @@ SparsePoseGraph::~SparsePoseGraph() { CHECK(work_queue_ == nullptr); } -std::vector SparsePoseGraph::GrowSubmapTransformsAsNeeded( - const int trajectory_id, +std::vector SparsePoseGraph::InitializeGlobalSubmapPoses( + const int trajectory_id, const common::Time time, const std::vector>& insertion_submaps) { CHECK(!insertion_submaps.empty()); const auto& submap_data = optimization_problem_.submap_data(); if (insertion_submaps.size() == 1) { // If we don't already have an entry for the first submap, add one. if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) { - optimization_problem_.AddSubmap(trajectory_id, - insertion_submaps[0]->local_pose()); + if (initial_trajectory_poses_.count(trajectory_id) > 0) { + trajectory_connectivity_state_.Connect( + trajectory_id, + initial_trajectory_poses_.at(trajectory_id).to_trajectory_id, time); + } + optimization_problem_.AddSubmap( + trajectory_id, ComputeLocalToGlobalTransform( + optimized_submap_transforms_, trajectory_id) * + insertion_submaps[0]->local_pose()); } CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id)); const mapping::SubmapId submap_id{trajectory_id, 0}; @@ -240,11 +247,11 @@ void SparsePoseGraph::ComputeConstraintsForScan( const mapping::NodeId& node_id, std::vector> insertion_submaps, const bool newly_finished_submap) { - const std::vector submap_ids = - GrowSubmapTransformsAsNeeded(node_id.trajectory_id, insertion_submaps); + const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; + const std::vector submap_ids = InitializeGlobalSubmapPoses( + node_id.trajectory_id, constant_data->time, insertion_submaps); CHECK_EQ(submap_ids.size(), insertion_submaps.size()); const mapping::SubmapId matching_id = submap_ids.front(); - const auto& constant_data = trajectory_nodes_.at(node_id).constant_data; const transform::Rigid3d& pose = constant_data->local_pose; const transform::Rigid3d optimized_pose = optimization_problem_.submap_data().at(matching_id).pose * @@ -571,6 +578,35 @@ std::vector SparsePoseGraph::constraints() { return constraints_; } +void SparsePoseGraph::SetInitialTrajectoryPose(const int from_trajectory_id, + const int to_trajectory_id, + const transform::Rigid3d& pose, + const common::Time time) { + common::MutexLocker locker(&mutex_); + initial_trajectory_poses_[from_trajectory_id] = + InitialTrajectoryPose{to_trajectory_id, pose, time}; +} + +transform::Rigid3d SparsePoseGraph::GetInterpolatedGlobalTrajectoryPose( + const int trajectory_id, const common::Time time) const { + CHECK(trajectory_nodes_.SizeOfTrajectoryOrZero(trajectory_id) > 0); + const auto it = trajectory_nodes_.lower_bound(trajectory_id, time); + if (it == trajectory_nodes_.BeginOfTrajectory(trajectory_id)) { + return trajectory_nodes_.BeginOfTrajectory(trajectory_id)->data.global_pose; + } + if (it == trajectory_nodes_.EndOfTrajectory(trajectory_id)) { + return std::prev(trajectory_nodes_.EndOfTrajectory(trajectory_id)) + ->data.global_pose; + } + return transform::Interpolate( + transform::TimestampedTransform{std::prev(it)->data.time(), + std::prev(it)->data.global_pose}, + transform::TimestampedTransform{it->data.time(), + it->data.global_pose}, + time) + .transform; +} + transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( const int trajectory_id) { common::MutexLocker locker(&mutex_); @@ -607,7 +643,14 @@ transform::Rigid3d SparsePoseGraph::ComputeLocalToGlobalTransform( auto begin_it = submap_transforms.BeginOfTrajectory(trajectory_id); auto end_it = submap_transforms.EndOfTrajectory(trajectory_id); if (begin_it == end_it) { - return transform::Rigid3d::Identity(); + const auto it = initial_trajectory_poses_.find(trajectory_id); + if (it != initial_trajectory_poses_.end()) { + return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id, + it->second.time) * + it->second.relative_pose; + } else { + return transform::Rigid3d::Identity(); + } } const mapping::SubmapId last_optimized_submap_id = std::prev(end_it)->id; // Accessing 'local_pose' in Submap is okay, since the member is const. diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 27117e4..38d14a8 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -104,6 +104,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { mapping::MapById GetTrajectoryNodes() override EXCLUDES(mutex_); std::vector constraints() override EXCLUDES(mutex_); + void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id, + const transform::Rigid3d& pose, + const common::Time time) override + EXCLUDES(mutex_); + transform::Rigid3d GetInterpolatedGlobalTrajectoryPose( + int trajectory_id, const common::Time time) const REQUIRES(mutex_); private: // The current state of the submap in the background threads. When this @@ -129,8 +135,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Grows the optimization problem to have an entry for every element of // 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'. - std::vector GrowSubmapTransformsAsNeeded( - int trajectory_id, + std::vector InitializeGlobalSubmapPoses( + int trajectory_id, const common::Time time, const std::vector>& insertion_submaps) REQUIRES(mutex_); @@ -230,6 +236,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Set of all frozen trajectories not being optimized. std::set frozen_trajectories_ GUARDED_BY(mutex_); + // Set of all initial trajectory poses. + std::map initial_trajectory_poses_ + GUARDED_BY(mutex_); + // Allows querying and manipulating the pose graph by the 'trimmers_'. The // 'mutex_' of the pose graph is held while this class is used. class TrimmingHandle : public mapping::Trimmable {