Merge pull request #3248 from pleroy/Merge1Point

Proper management of the time-to-segment map when merging trajectories with 1-point segments

Author: pleroy

physics/discrete_trajectory_body.hpp

@@ -187,7 +187,7 @@ DiscreteTrajectory<Frame>::NewSegment() {
         segment_by_left_endpoint_.end(), last_time, new_segment_sit);
   }
 
-  DCHECK_OK(ConsistencyStatus());
+  CHECK_OK(ConsistencyStatus());
   return new_self;
 }
 
@@ -205,7 +205,7 @@ DiscreteTrajectory<Frame>::DetachSegments(SegmentIterator const begin) {
                       /*to=*/detached,
                       /*to_segments_begin=*/detached.segments_->begin());
 
-  DCHECK_OK(ConsistencyStatus());
+  CHECK_OK(ConsistencyStatus());
   return detached;
 }
 
@@ -245,7 +245,7 @@ DiscreteTrajectory<Frame>::AttachSegments(DiscreteTrajectory trajectory) {
                       /*to=*/*this,
                       /*to_segments_begin=*/end_before_splice);
 
-  DCHECK_OK(ConsistencyStatus());
+  CHECK_OK(ConsistencyStatus());
   return SegmentIterator(segments_.get(), end_before_splice);
 }
 
@@ -270,7 +270,7 @@ void DiscreteTrajectory<Frame>::DeleteSegments(SegmentIterator& begin) {
   // Make sure that the client doesn't try to use the invalid iterator.
   begin = segments().end();
 
-  DCHECK_OK(ConsistencyStatus());
+  CHECK_OK(ConsistencyStatus());
 }
 
 template<typename Frame>
@@ -300,7 +300,7 @@ void DiscreteTrajectory<Frame>::ForgetAfter(Instant const& t) {
                                     segment_by_left_endpoint_.end());
   }
 
-  DCHECK_OK(ConsistencyStatus());
+  CHECK_OK(ConsistencyStatus());
 }
 
 template<typename Frame>
@@ -342,7 +342,7 @@ void DiscreteTrajectory<Frame>::ForgetBefore(Instant const& t) {
         sit);
   }
 
-  DCHECK_OK(ConsistencyStatus());
+  CHECK_OK(ConsistencyStatus());
 }
 
 template<typename Frame>
@@ -396,12 +396,18 @@ void DiscreteTrajectory<Frame>::Merge(DiscreteTrajectory<Frame> trajectory) {
       // Merge corresponding segments.
       sit_t->Merge(std::move(*sit_s));
 
-      // If the left endpoint has changed, remove its entry from the time-to-
-      // segment map.  Insert a new entry if the segment is not empty.
+      // If the left endpoint of |sit_t| has changed, remove its entry from the
+      // time-to-segment map, if any.
       if (left_endpoint.has_value() &&
           sit_t->front().time < left_endpoint.value()) {
-        segment_by_left_endpoint_.erase(left_endpoint.value());
+        auto const it = segment_by_left_endpoint_.find(left_endpoint.value());
+        if (it != segment_by_left_endpoint_.end() && it->second == sit_t) {
+          segment_by_left_endpoint_.erase(left_endpoint.value());
+        }
       }
+      // Insert a new entry in the time-to-segment map if the segment is not
+      // empty.  This entry will be overwritten by any future entry at the same
+      // time, thereby enforcing the invariants of the time-to-segment map.
       if (!sit_t->empty()) {
         segment_by_left_endpoint_.insert_or_assign(sit_t->front().time, sit_t);
       }
@@ -431,7 +437,7 @@ void DiscreteTrajectory<Frame>::Merge(DiscreteTrajectory<Frame> trajectory) {
     }
   }
 
-  DCHECK_OK(ConsistencyStatus());
+  CHECK_OK(ConsistencyStatus());
 }
 
 template<typename Frame>
@@ -726,6 +732,12 @@ absl::Status DiscreteTrajectory<Frame>::ConsistencyStatus() const {
                          " and ", DebugString(sit2->front().time)));
       }
     }
+    if (sit1 != segments_->cend() && !sit1->empty()) {
+        return absl::InternalError(
+            absl::StrCat("Segment at time ", DebugString(sit1->front().time),
+                         " missing in the time-to-segment map of size ",
+                         segment_by_left_endpoint_.size()));
+    }
   }
   if (!segments_->empty()) {
     int i = 0;

physics/discrete_trajectory_test.cpp

@@ -628,6 +628,17 @@ TEST_F(DiscreteTrajectoryTest, Merge) {
     EXPECT_EQ(t0_ + 9 * Second, sit->front().time);
     EXPECT_EQ(t0_ + 14 * Second, sit->back().time);
   }
+  {
+    auto trajectory1 = MakeTrajectory();
+    auto trajectory2 = MakeTrajectory();
+
+    trajectory1.ForgetAfter(t0_ + 9 * Second);
+    // This trajectory starts with a 1-point segment.  Merge used to fail the
+    // consistency check because the time-to-segment map was losing an entry.
+    trajectory2.ForgetBefore(t0_ + 9 * Second);
+
+    trajectory2.Merge(std::move(trajectory1));
+  }
 }
 
 TEST_F(DiscreteTrajectoryTest, TMinTMaxEvaluate) {