mockingbirdnest · Nov 16, 2021 · Nov 23, 2021 · Nov 23, 2021 · Nov 23, 2021 · Nov 24, 2021
Showing with 176 additions and 30 deletions.

+14 −1 physics/discrete_trajectory.hpp

+54 −2 physics/discrete_trajectory_body.hpp

+8 −3 physics/discrete_trajectory_segment.hpp

+51 −24 physics/discrete_trajectory_segment_body.hpp

+49 −0 physics/discrete_trajectory_test.cpp
diff --git a/physics/discrete_trajectory.hpp b/physics/discrete_trajectory.hpp
@@ -88,7 +88,7 @@ class DiscreteTrajectory : public Trajectory<Frame> {
   SegmentIterator NewSegment();
 
   DiscreteTrajectory DetachSegments(SegmentIterator begin);
-  SegmentIterator AttachSegments(DiscreteTrajectory&& trajectory);
+  SegmentIterator AttachSegments(DiscreteTrajectory trajectory);
   void DeleteSegments(SegmentIterator& begin);
 
   // Deletes the trajectory points with a time in [t, end[.  Drops the segments
@@ -105,6 +105,19 @@ class DiscreteTrajectory : public Trajectory<Frame> {
   absl::Status Append(Instant const& t,
                       DegreesOfFreedom<Frame> const& degrees_of_freedom);
 
+  // Merges |trajectory| (the source) into this object (the target).  The
+  // operation processes pairs of segments taken from each trajectory and
+  // proceeds as follows:
+  // 1. If the source segment is empty (or missing), leave the target segment
+  //    unchanged.
+  // 2. If the target segment is empty (or missing), move the source segment
+  //    into the target.  This includes its downsampling state.
+  // 3. If both segments are nonempty, they must be nonoverlapping.  The points
+  //    from the source segment are inserted in the target segment (possibly
+  //    before the beginning of that segment).  The downsampling state of the
+  //    result is that of the latest segment (the one with the largest times).
+  void Merge(DiscreteTrajectory<Frame> trajectory);
+
   Instant t_min() const override;
   Instant t_max() const override;
 

diff --git a/physics/discrete_trajectory_body.hpp b/physics/discrete_trajectory_body.hpp
@@ -204,8 +204,7 @@ DiscreteTrajectory<Frame>::DetachSegments(SegmentIterator const begin) {
 
 template<typename Frame>
 typename DiscreteTrajectory<Frame>::SegmentIterator
-DiscreteTrajectory<Frame>::AttachSegments(
-    DiscreteTrajectory&& trajectory) {
+DiscreteTrajectory<Frame>::AttachSegments(DiscreteTrajectory trajectory) {
   CHECK(!trajectory.empty());
 
   if (empty()) {
@@ -375,6 +374,59 @@ absl::Status DiscreteTrajectory<Frame>::Append(
   return absl::OkStatus();
 }
 
+template<typename Frame>
+void DiscreteTrajectory<Frame>::Merge(DiscreteTrajectory<Frame> trajectory) {
+  auto sit_s = trajectory.segments_->begin();  // Source iterator.
+  auto sit_t = segments_->begin();  // Target iterator.
+  for (;;) {
+    if (sit_s != trajectory.segments_->end() && sit_t != segments_->end()) {
+      // Record the existing left endpoint to update the time-to-segment map as
+      // needed.
+      const std::optional<Instant> left_endpoint =
+          sit_t->empty() ? std::nullopt
+                         : std::make_optional(sit_t->front().time);
+
+      // 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 (left_endpoint.has_value() &&
+          sit_t->front().time < left_endpoint.value()) {
+        segment_by_left_endpoint_.erase(left_endpoint.value());
+      }
+      if (!sit_t->empty()) {
+        segment_by_left_endpoint_.insert_or_assign(sit_t->front().time, sit_t);
+      }
+
+      ++sit_s;
+      ++sit_t;
+    } else if (sit_s != trajectory.segments_->end()) {
+      // No more segments in the target.  We splice the segments of the source.
+
+      // The |end| iterator keeps pointing at the end after the splice.
+      // Instead, we track the iterator to the last segment.
+      auto const last_before_splice = --segments_->end();
+
+      segments_->splice(segments_->end(),
+                        *trajectory.segments_,
+                        sit_s,
+                        trajectory.segments_->end());
+
+      auto const end_before_splice = std::next(last_before_splice);
+      AdjustAfterSplicing(/*from=*/trajectory,
+                          /*to=*/*this,
+                          /*to_segments_begin=*/end_before_splice);
+      break;
+    } else {
+      // No more segments in the source, or both lists done.
+      break;
+    }
+  }
+
+  DCHECK_OK(ConsistencyStatus());
+}
+
 template<typename Frame>
 Instant DiscreteTrajectory<Frame>::t_min() const {
   return segments_->front().t_min();

diff --git a/physics/discrete_trajectory_segment.hpp b/physics/discrete_trajectory_segment.hpp
@@ -143,9 +143,6 @@ class DiscreteTrajectorySegment : public Trajectory<Frame> {
   void Prepend(Instant const& t,
                DegreesOfFreedom<Frame> const& degrees_of_freedom);
 
-  absl::Status Append(Instant const& t,
-                      DegreesOfFreedom<Frame> const& degrees_of_freedom);
-
   // Removes all points with a time greater than or equal to |t| (1st overload)
   // or starting at |begin| (2nd overload).
   void ForgetAfter(Instant const& t);
@@ -156,6 +153,14 @@ class DiscreteTrajectorySegment : public Trajectory<Frame> {
   void ForgetBefore(Instant const& t);
   void ForgetBefore(typename Timeline::const_iterator end);
 
+  absl::Status Append(Instant const& t,
+                      DegreesOfFreedom<Frame> const& degrees_of_freedom);
+
+  // Merges the points from the given |segment| into this object.  The two
+  // segments must have nonoverlapping times.  The downsampling state of the
+  // result is that of the latest segment (with the largest times).
+  void Merge(DiscreteTrajectorySegment<Frame> segment);
+
   // Computes |number_of_dense_points_| based on the start of the dense
   // timeline.  Used for compatibility deserialization.
   void SetStartOfDenseTimeline(Instant const& t);

diff --git a/physics/discrete_trajectory_segment_body.hpp b/physics/discrete_trajectory_segment_body.hpp
@@ -328,30 +328,6 @@ void DiscreteTrajectorySegment<Frame>::Prepend(
   timeline_.emplace_hint(timeline_.cbegin(), t, degrees_of_freedom);
 }
 
-template<typename Frame>
-absl::Status DiscreteTrajectorySegment<Frame>::Append(
-    Instant const& t,
-    DegreesOfFreedom<Frame> const& degrees_of_freedom) {
-  if (!timeline_.empty() && timeline_.cbegin()->time == t) {
-    LOG(WARNING) << "Append at existing time " << t << ", time range = ["
-                 << timeline_.cbegin()->time << ", "
-                 << timeline_.crbegin()->time << "]";
-    return absl::OkStatus();
-  }
-  auto it = timeline_.emplace_hint(timeline_.cend(),
-                                   t,
-                                   degrees_of_freedom);
-  CHECK(++it == timeline_.end())
-      << "Append out of order at " << t << ", last time is "
-      << timeline_.crbegin()->time;
-
-  if (downsampling_parameters_.has_value()) {
-    return DownsampleIfNeeded();
-  } else {
-    return absl::OkStatus();
-  }
-}
-
 template<typename Frame>
 void DiscreteTrajectorySegment<Frame>::ForgetAfter(Instant const& t) {
   ForgetAfter(timeline_.lower_bound(t));
@@ -386,6 +362,57 @@ void DiscreteTrajectorySegment<Frame>::ForgetBefore(
   timeline_.erase(timeline_.cbegin(), end);
 }
 
+template<typename Frame>
+absl::Status DiscreteTrajectorySegment<Frame>::Append(
+    Instant const& t,
+    DegreesOfFreedom<Frame> const& degrees_of_freedom) {
+  if (!timeline_.empty() && timeline_.cbegin()->time == t) {
+    LOG(WARNING) << "Append at existing time " << t << ", time range = ["
+                 << timeline_.cbegin()->time << ", "
+                 << timeline_.crbegin()->time << "]";
+    return absl::OkStatus();
+  }
+  auto it = timeline_.emplace_hint(timeline_.cend(),
+                                   t,
+                                   degrees_of_freedom);
+  CHECK(++it == timeline_.end())
+      << "Append out of order at " << t << ", last time is "
+      << timeline_.crbegin()->time;
+
+  if (downsampling_parameters_.has_value()) {
+    return DownsampleIfNeeded();
+  } else {
+    return absl::OkStatus();
+  }
+}
+
+template<typename Frame>
+void DiscreteTrajectorySegment<Frame>::Merge(
+    DiscreteTrajectorySegment<Frame> segment) {
+  if (segment.timeline_.empty()) {
+    return;
+  } else if (timeline_.empty()) {
+    downsampling_parameters_ = segment.downsampling_parameters_;
+    timeline_ = std::move(segment.timeline_);
+    number_of_dense_points_ = segment.number_of_dense_points_;
+  } else if (std::prev(timeline_.cend())->time <
+             segment.timeline_.cbegin()->time) {
+    downsampling_parameters_ = segment.downsampling_parameters_;
+    timeline_.merge(segment.timeline_);
+    number_of_dense_points_ = segment.number_of_dense_points_;
+  } else if (std::prev(segment.timeline_.cend())->time <
+             timeline_.cbegin()->time) {
+    timeline_.merge(segment.timeline_);
+  } else {
+    // TODO(phl): We might need to have to check <= above and to verify that the
+    // points match.
+    LOG(FATAL) << "Overlapping merge: [" << segment.timeline_.cbegin()->time
+               << ", " << std::prev(segment.timeline_.cend())->time
+               << "] into [" << timeline_.cbegin()->time << ", "
+               << std::prev(timeline_.cend())->time << "]";
+  }
+}
+
 template<typename Frame>
 void DiscreteTrajectorySegment<Frame>::SetStartOfDenseTimeline(
     Instant const& t) {

diff --git a/physics/discrete_trajectory_test.cpp b/physics/discrete_trajectory_test.cpp
@@ -570,6 +570,55 @@ TEST_F(DiscreteTrajectoryTest, ForgetBefore) {
   EXPECT_TRUE(trajectory.empty());
 }
 
+TEST_F(DiscreteTrajectoryTest, Merge) {
+  {
+    auto trajectory1 = MakeTrajectory();
+    auto trajectory2 = MakeTrajectory();
+
+    trajectory1.ForgetAfter(t0_ + 6 * Second);
+    trajectory2.ForgetBefore(t0_ + 6 * Second);
+
+    trajectory1.Merge(std::move(trajectory2));
+
+    EXPECT_EQ(3, trajectory1.segments().size());
+    auto sit = trajectory1.segments().begin();
+    EXPECT_EQ(5, sit->size());
+    EXPECT_EQ(t0_, sit->front().time);
+    EXPECT_EQ(t0_ + 4 * Second, sit->back().time);
+    ++sit;
+    EXPECT_EQ(6, sit->size());
+    EXPECT_EQ(t0_ + 4 * Second, sit->front().time);
+    EXPECT_EQ(t0_ + 9 * Second, sit->back().time);
+    ++sit;
+    EXPECT_EQ(6, sit->size());
+    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_ + 6 * Second);
+    trajectory2.ForgetBefore(t0_ + 6 * Second);
+
+    trajectory2.Merge(std::move(trajectory1));
+
+    EXPECT_EQ(3, trajectory2.segments().size());
+    auto sit = trajectory2.segments().begin();
+    EXPECT_EQ(5, sit->size());
+    EXPECT_EQ(t0_, sit->front().time);
+    EXPECT_EQ(t0_ + 4 * Second, sit->back().time);
+    ++sit;
+    EXPECT_EQ(6, sit->size());
+    EXPECT_EQ(t0_ + 4 * Second, sit->front().time);
+    EXPECT_EQ(t0_ + 9 * Second, sit->back().time);
+    ++sit;
+    EXPECT_EQ(6, sit->size());
+    EXPECT_EQ(t0_ + 9 * Second, sit->front().time);
+    EXPECT_EQ(t0_ + 14 * Second, sit->back().time);
+  }
+}
+
 TEST_F(DiscreteTrajectoryTest, TMinTMaxEvaluate) {
   auto const trajectory = MakeTrajectory();
   EXPECT_EQ(t0_, trajectory.t_min());