Store landmark observations as LandmarkNodes in PoseGraph. (#850)

cartographer/mapping/pose_graph_interface.h

@@ -49,6 +49,18 @@ class PoseGraphInterface {
     enum Tag { INTRA_SUBMAP, INTER_SUBMAP } tag;
   };
 
+  struct LandmarkNode {
+    struct LandmarkObservation {
+      int trajectory_id;
+      common::Time time;
+      transform::Rigid3d landmark_to_tracking_transform;
+      double translation_weight;
+      double rotation_weight;
+    };
+    std::vector<LandmarkObservation> landmark_observations;
+    transform::Rigid3d global_landmark_pose;
+  };
+
   struct SubmapPose {
     int version;
     transform::Rigid3d pose;

cartographer/mapping_2d/pose_graph.cc

@@ -175,7 +175,17 @@ void PoseGraph::AddFixedFramePoseData(
 void PoseGraph::AddLandmarkData(int trajectory_id,
                                 const sensor::LandmarkData& landmark_data)
     EXCLUDES(mutex_) {
-  LOG(FATAL) << "Not yet implemented.";
+  common::MutexLocker locker(&mutex_);
+  for (const auto& observation : landmark_data.landmark_observations) {
+    landmark_nodes_[observation.id].landmark_observations.emplace_back(
+        PoseGraph::LandmarkNode::LandmarkObservation{
+            trajectory_id,
+            landmark_data.time,
+            observation.landmark_to_tracking_transform,
+            observation.translation_weight,
+            observation.rotation_weight,
+        });
+  }
 }
 
 void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
@@ -191,10 +201,10 @@ void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
           last_connection_time +
               common::FromSeconds(
                   options_.global_constraint_search_after_n_seconds())) {
-    // If the node and the submap belong to the same trajectory or if there has
+    // If the node and the submap belong to the same trajectory or if there
-    // been a recent global constraint that ties that node's trajectory to the
+    // has been a recent global constraint that ties that node's trajectory to
-    // submap's trajectory, it suffices to do a match constrained to a local
+    // the submap's trajectory, it suffices to do a match constrained to a
-    // search window.
+    // local search window.
     const transform::Rigid2d initial_relative_pose =
         optimization_problem_.submap_data()
             .at(submap_id)
@@ -243,8 +253,8 @@ void PoseGraph::ComputeConstraintsForNode(
       constant_data->gravity_alignment);
   for (size_t i = 0; i < insertion_submaps.size(); ++i) {
     const mapping::SubmapId submap_id = submap_ids[i];
-    // Even if this was the last node added to 'submap_id', the submap will only
+    // Even if this was the last node added to 'submap_id', the submap will
-    // 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);
     submap_data_.at(submap_id).node_ids.emplace(node_id);
     const transform::Rigid2d constraint_transform =
@@ -537,8 +547,9 @@ void PoseGraph::RunOptimization() {
   }
 
   // No other thread is accessing the optimization_problem_, constraints_ and
-  // frozen_trajectories_ when executing the Solve. Solve is time consuming, so
+  // frozen_trajectories_ when executing the Solve. Solve is time consuming,
-  // not taking the mutex before Solve to avoid blocking foreground processing.
+  // so not taking the mutex before Solve to avoid blocking foreground
+  // processing.
   optimization_problem_.Solve(constraints_, frozen_trajectories_);
   common::MutexLocker locker(&mutex_);
 

cartographer/mapping_2d/pose_graph.h

@@ -39,6 +39,7 @@
 #include "cartographer/mapping_2d/pose_graph/optimization_problem.h"
 #include "cartographer/mapping_2d/submaps.h"
 #include "cartographer/sensor/fixed_frame_pose_data.h"
+#include "cartographer/sensor/landmark_data.h"
 #include "cartographer/sensor/odometry_data.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/transform/rigid_transform.h"
@@ -244,6 +245,10 @@ class PoseGraph : public mapping::PoseGraph {
   mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>
       global_submap_poses_ GUARDED_BY(mutex_);
 
+  // Global landmark poses with all observations.
+  std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
+      landmark_nodes_ GUARDED_BY(mutex_);
+
   // List of all trimmers to consult when optimizations finish.
   std::vector<std::unique_ptr<mapping::PoseGraphTrimmer>> trimmers_
       GUARDED_BY(mutex_);

cartographer/mapping_3d/pose_graph.cc

@@ -177,7 +177,17 @@ void PoseGraph::AddFixedFramePoseData(
 void PoseGraph::AddLandmarkData(int trajectory_id,
                                 const sensor::LandmarkData& landmark_data)
     EXCLUDES(mutex_) {
-  LOG(FATAL) << "Not yet implemented.";
+  common::MutexLocker locker(&mutex_);
+  for (const auto& observation : landmark_data.landmark_observations) {
+    landmark_nodes_[observation.id].landmark_observations.emplace_back(
+        PoseGraph::LandmarkNode::LandmarkObservation{
+            trajectory_id,
+            landmark_data.time,
+            observation.landmark_to_tracking_transform,
+            observation.translation_weight,
+            observation.rotation_weight,
+        });
+  }
 }
 
 void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id,

cartographer/mapping_3d/pose_graph.h

@@ -39,6 +39,7 @@
 #include "cartographer/mapping_3d/pose_graph/optimization_problem.h"
 #include "cartographer/mapping_3d/submaps.h"
 #include "cartographer/sensor/fixed_frame_pose_data.h"
+#include "cartographer/sensor/landmark_data.h"
 #include "cartographer/sensor/odometry_data.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/transform/rigid_transform.h"
@@ -248,6 +249,10 @@ class PoseGraph : public mapping::PoseGraph {
   mapping::MapById<mapping::SubmapId, pose_graph::SubmapData>
       global_submap_poses_ GUARDED_BY(mutex_);
 
+  // Global landmark poses with all observations.
+  std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
+      landmark_nodes_ GUARDED_BY(mutex_);
+
   // List of all trimmers to consult when optimizations finish.
   std::vector<std::unique_ptr<mapping::PoseGraphTrimmer>> trimmers_
       GUARDED_BY(mutex_);