Initial pose implementation. (#606)

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);
     }

cartographer/mapping/map_builder.cc

@@ -23,6 +23,7 @@
 #include <utility>
 
 #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<PureLocalizationTrimmer>(
         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;
 }
 

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;
 }

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<Constraint> 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<SparsePoseGraph::Constraint> FromProto(

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -52,17 +52,24 @@ SparsePoseGraph::~SparsePoseGraph() {
   CHECK(work_queue_ == nullptr);
 }
 
-std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
+std::vector<mapping::SubmapId> SparsePoseGraph::InitializeGlobalSubmapPoses(
-    const int trajectory_id,
+    const int trajectory_id, const common::Time time,
     const std::vector<std::shared_ptr<const Submap>>& 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,
+          trajectory_id, transform::Project2D(
-          sparse_pose_graph::ComputeSubmapPose(*insertion_submaps[0]));
+                             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<std::shared_ptr<const Submap>> insertion_submaps,
     const bool newly_finished_submap) {
-  const std::vector<mapping::SubmapId> submap_ids =
+  const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
-      GrowSubmapTransformsAsNeeded(node_id.trajectory_id, insertion_submaps);
+  const std::vector<mapping::SubmapId> 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::Constraint> 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.

cartographer/mapping_2d/sparse_pose_graph.h

@@ -104,6 +104,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
   GetTrajectoryNodes() override EXCLUDES(mutex_);
   std::vector<Constraint> 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<mapping::SubmapId> GrowSubmapTransformsAsNeeded(
+  std::vector<mapping::SubmapId> InitializeGlobalSubmapPoses(
-      int trajectory_id,
+      int trajectory_id, const common::Time time,
       const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
       REQUIRES(mutex_);
 
@@ -226,6 +232,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Set of all frozen trajectories not being optimized.
   std::set<int> frozen_trajectories_ GUARDED_BY(mutex_);
 
+  // Set of all initial trajectory poses.
+  std::map<int, InitialTrajectoryPose> 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 {

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -52,16 +52,23 @@ SparsePoseGraph::~SparsePoseGraph() {
   CHECK(work_queue_ == nullptr);
 }
 
-std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
+std::vector<mapping::SubmapId> SparsePoseGraph::InitializeGlobalSubmapPoses(
-    const int trajectory_id,
+    const int trajectory_id, const common::Time time,
     const std::vector<std::shared_ptr<const Submap>>& 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,
+      if (initial_trajectory_poses_.count(trajectory_id) > 0) {
-                                      insertion_submaps[0]->local_pose());
+        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<std::shared_ptr<const Submap>> insertion_submaps,
     const bool newly_finished_submap) {
-  const std::vector<mapping::SubmapId> submap_ids =
+  const auto& constant_data = trajectory_nodes_.at(node_id).constant_data;
-      GrowSubmapTransformsAsNeeded(node_id.trajectory_id, insertion_submaps);
+  const std::vector<mapping::SubmapId> 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::Constraint> 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.

cartographer/mapping_3d/sparse_pose_graph.h

@@ -104,6 +104,12 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   mapping::MapById<mapping::NodeId, mapping::TrajectoryNode>
   GetTrajectoryNodes() override EXCLUDES(mutex_);
   std::vector<Constraint> 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<mapping::SubmapId> GrowSubmapTransformsAsNeeded(
+  std::vector<mapping::SubmapId> InitializeGlobalSubmapPoses(
-      int trajectory_id,
+      int trajectory_id, const common::Time time,
       const std::vector<std::shared_ptr<const Submap>>& insertion_submaps)
       REQUIRES(mutex_);
 
@@ -230,6 +236,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // Set of all frozen trajectories not being optimized.
   std::set<int> frozen_trajectories_ GUARDED_BY(mutex_);
 
+  // Set of all initial trajectory poses.
+  std::map<int, InitialTrajectoryPose> 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 {