Landmark improvements (#901)

cartographer/mapping_2d/pose_graph.cc

@@ -176,16 +176,15 @@ void PoseGraph::AddLandmarkData(int trajectory_id,
                                 const sensor::LandmarkData& landmark_data)
     EXCLUDES(mutex_) {
   common::MutexLocker locker(&mutex_);
-  for (const auto& observation : landmark_data.landmark_observations) {
+  AddWorkItem([=]() REQUIRES(mutex_) {
-    landmark_nodes_[observation.id].landmark_observations.emplace_back(
+    for (const auto& observation : landmark_data.landmark_observations) {
-        PoseGraph::LandmarkNode::LandmarkObservation{
+      landmark_nodes_[observation.id].landmark_observations.emplace_back(
-            trajectory_id,
+          PoseGraph::LandmarkNode::LandmarkObservation{
-            landmark_data.time,
+              trajectory_id, landmark_data.time,
-            observation.landmark_to_tracking_transform,
+              observation.landmark_to_tracking_transform,
-            observation.translation_weight,
+              observation.translation_weight, observation.rotation_weight});
-            observation.rotation_weight,
+    }
-        });
+  });
-  }
 }
 
 void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
@@ -546,10 +545,10 @@ void PoseGraph::RunOptimization() {
     return;
   }
 
-  // No other thread is accessing the optimization_problem_, constraints_ and
+  // No other thread is accessing the optimization_problem_, constraints_,
-  // frozen_trajectories_ when executing the Solve. Solve is time consuming,
+  // frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve
-  // so not taking the mutex before Solve to avoid blocking foreground
+  // is time consuming, so not taking the mutex before Solve to avoid blocking
-  // processing.
+  // foreground processing.
   optimization_problem_.Solve(constraints_, frozen_trajectories_,
                               landmark_nodes_);
   common::MutexLocker locker(&mutex_);

cartographer/mapping_2d/pose_graph/optimization_problem.cc

@@ -79,18 +79,23 @@ void AddLandmarkCostFunctions(
   for (const auto& landmark_node : landmark_nodes) {
     for (const auto& observation : landmark_node.second.landmark_observations) {
       const std::string& landmark_id = landmark_node.first;
+      const auto& begin_of_trajectory =
+          node_data.BeginOfTrajectory(observation.trajectory_id);
+      // The landmark observation was made before the trajectory was created.
+      if (observation.time < begin_of_trajectory->data.time) {
+        continue;
+      }
       // Find the trajectory nodes before and after the landmark observation.
       auto next =
           node_data.lower_bound(observation.trajectory_id, observation.time);
-      // The landmark observation was made before the trajectory was created.
-      if (next == node_data.BeginOfTrajectory(observation.trajectory_id)) {
-        continue;
-      }
       // The landmark observation was made, but the next trajectory node has
       // not been added yet.
       if (next == node_data.EndOfTrajectory(observation.trajectory_id)) {
         continue;
       }
+      if (next == begin_of_trajectory) {
+        next = std::next(next);
+      }
       auto prev = std::prev(next);
       // Add parameter blocks for the landmark ID if they were not added before.
       if (!C_landmarks->count(landmark_id)) {

cartographer/mapping_3d/pose_graph.cc

@@ -178,16 +178,15 @@ void PoseGraph::AddLandmarkData(int trajectory_id,
                                 const sensor::LandmarkData& landmark_data)
     EXCLUDES(mutex_) {
   common::MutexLocker locker(&mutex_);
-  for (const auto& observation : landmark_data.landmark_observations) {
+  AddWorkItem([=]() REQUIRES(mutex_) {
-    landmark_nodes_[observation.id].landmark_observations.emplace_back(
+    for (const auto& observation : landmark_data.landmark_observations) {
-        PoseGraph::LandmarkNode::LandmarkObservation{
+      landmark_nodes_[observation.id].landmark_observations.emplace_back(
-            trajectory_id,
+          PoseGraph::LandmarkNode::LandmarkObservation{
-            landmark_data.time,
+              trajectory_id, landmark_data.time,
-            observation.landmark_to_tracking_transform,
+              observation.landmark_to_tracking_transform,
-            observation.translation_weight,
+              observation.translation_weight, observation.rotation_weight});
-            observation.rotation_weight,
+    }
-        });
+  });
-  }
 }
 
 void PoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
@@ -575,9 +574,10 @@ void PoseGraph::RunOptimization() {
     return;
   }
 
-  // No other thread is accessing the optimization_problem_, constraints_ and
+  // No other thread is accessing the optimization_problem_, constraints_,
-  // frozen_trajectories_ when executing the Solve. Solve is time consuming, so
+  // frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve
-  // not taking the mutex before Solve to avoid blocking foreground processing.
+  // is time consuming, so not taking the mutex before Solve to avoid blocking
+  // foreground processing.
   optimization_problem_.Solve(constraints_, frozen_trajectories_,
                               landmark_nodes_);
   common::MutexLocker locker(&mutex_);

cartographer/mapping_3d/pose_graph/optimization_problem.cc

@@ -120,18 +120,23 @@ void AddLandmarkCostFunctions(
   for (const auto& landmark_node : landmark_nodes) {
     for (const auto& observation : landmark_node.second.landmark_observations) {
       const std::string& landmark_id = landmark_node.first;
+      const auto& begin_of_trajectory =
+          node_data.BeginOfTrajectory(observation.trajectory_id);
+      // The landmark observation was made before the trajectory was created.
+      if (observation.time < begin_of_trajectory->data.time) {
+        continue;
+      }
       // Find the trajectory nodes before and after the landmark observation.
       auto next =
           node_data.lower_bound(observation.trajectory_id, observation.time);
-      // The landmark observation was made before the trajectory was created.
-      if (next == node_data.BeginOfTrajectory(observation.trajectory_id)) {
-        continue;
-      }
       // The landmark observation was made, but the next trajectory node has
       // not been added yet.
       if (next == node_data.EndOfTrajectory(observation.trajectory_id)) {
         continue;
       }
+      if (next == begin_of_trajectory) {
+        next = std::next(next);
+      }
       auto prev = std::prev(next);
       // Add parameter blocks for the landmark ID if they were not added before.
       if (!C_landmarks->count(landmark_id)) {