Merge pull request #3150 from pleroy/Downsampling2

DiscreteTrajectorySegment, part 4: downsampling

Author: pleroy

physics/discrete_trajectory_segment.hpp

@@ -2,9 +2,9 @@
 
 #include <cstdint>
 #include <iterator>
+#include <optional>
 
 #include "absl/container/btree_map.h"
-#include "absl/container/btree_set.h"
 #include "absl/status/status.h"
 #include "geometry/named_quantities.hpp"
 #include "numerics/hermite3.hpp"
@@ -84,6 +84,9 @@ class DiscreteTrajectorySegment : public Trajectory<Frame> {
       Instant const& t) const override;
 
  private:
+  using DownsamplingParameters =
+      internal_discrete_trajectory_types::DownsamplingParameters;
+
   absl::Status Append(Instant const& t,
                       DegreesOfFreedom<Frame> const& degrees_of_freedom);
 
@@ -97,6 +100,20 @@ class DiscreteTrajectorySegment : public Trajectory<Frame> {
   void ForgetBefore(Instant const& t);
   void ForgetBefore(typename Timeline::const_iterator end);
 
+  // This segment must have 0 or 1 points.  Occasionally removes intermediate
+  // points from the segment when |Append|ing, ensuring that positions remain
+  // within the desired tolerance.
+  void SetDownsampling(DownsamplingParameters const& downsampling_parameters);
+
+  // Clear the downsampling parameters.  From now on, all points appended to the
+  // segment are going to be retained.
+  void ClearDownsampling();
+
+  // Called by |Append| after appending a point to this segment.  If
+  // appropriate, performs downsampling and deletes some of the points of the
+  // segment.
+  absl::Status DownsampleIfNeeded();
+
   // Returns the Hermite interpolation for the left-open, right-closed
   // trajectory segment bounded above by |upper|.
   Hermite3<Instant, Position<Frame>> GetInterpolation(
@@ -105,10 +122,14 @@ class DiscreteTrajectorySegment : public Trajectory<Frame> {
   virtual typename Timeline::const_iterator timeline_begin() const;
   virtual typename Timeline::const_iterator timeline_end() const;
 
-  DiscreteTrajectorySegmentIterator<Frame> self_;
+  std::optional<DownsamplingParameters> downsampling_parameters_;
 
+  DiscreteTrajectorySegmentIterator<Frame> self_;
   Timeline timeline_;
-  absl::btree_set<Instant> dense_points_;
+
+  // The number of points at the end of the segment that are part of a "dense"
+  // span, i.e., have not been subjected to downsampling yet.
+  std::int64_t number_of_dense_points_ = 0;
 
   template<typename F>
   friend class internal_discrete_trajectory_iterator::

physics/discrete_trajectory_segment_body.hpp

@@ -2,17 +2,24 @@
 
 #include "physics/discrete_trajectory_segment.hpp"
 
+#include <algorithm>
 #include <iterator>
+#include <list>
+#include <vector>
 
 #include "astronomy/epoch.hpp"
+#include "geometry/named_quantities.hpp"
 #include "glog/logging.h"
+#include "numerics/fit_hermite_spline.hpp"
 
 namespace principia {
 namespace physics {
 namespace internal_discrete_trajectory_segment {
 
 using astronomy::InfiniteFuture;
 using astronomy::InfinitePast;
+using geometry::Position;
+using numerics::FitHermiteSpline;
 
 template<typename Frame>
 DiscreteTrajectorySegment<Frame>::DiscreteTrajectorySegment(
@@ -160,34 +167,114 @@ absl::Status DiscreteTrajectorySegment<Frame>::Append(
       << "Append out of order at " << t << ", last time is "
       << timeline_.crbegin()->first;
 
-  // TODO(phl): Downsampling.
-  return absl::OkStatus();
+  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));
-  // TODO(phl): Downsampling.
 }
 
 template<typename Frame>
 void DiscreteTrajectorySegment<Frame>::ForgetAfter(
     typename Timeline::const_iterator const begin) {
-  timeline_.erase(begin, timeline_.end());
-  // TODO(phl): Downsampling.
+  std::int64_t number_of_points_to_remove =
+      std::distance(begin, timeline_.cend());
+  number_of_dense_points_ =
+      std::max(0LL, number_of_dense_points_ - number_of_points_to_remove);
+
+  timeline_.erase(begin, timeline_.cend());
 }
 
 template<typename Frame>
 void DiscreteTrajectorySegment<Frame>::ForgetBefore(Instant const& t) {
   ForgetBefore(timeline_.lower_bound(t));
-  // TODO(phl): Downsampling.
 }
 
 template<typename Frame>
 void DiscreteTrajectorySegment<Frame>::ForgetBefore(
     typename Timeline::const_iterator const end) {
-  timeline_.erase(timeline_.begin(), end);
-  // TODO(phl): Downsampling.
+  std::int64_t number_of_points_to_remove =
+      std::distance(timeline_.cbegin(), end);
+  number_of_dense_points_ =
+      std::max(0LL, number_of_dense_points_ - number_of_points_to_remove);
+
+  timeline_.erase(timeline_.cbegin(), end);
+}
+
+template<typename Frame>
+void DiscreteTrajectorySegment<Frame>::SetDownsampling(
+    DownsamplingParameters const& downsampling_parameters) {
+  // The semantics of changing downsampling on a segment that has 2 points or
+  // more are unclear.  Let's not do that.
+  CHECK_LE(timeline_.size(), 1);
+  downsampling_parameters_ = downsampling_parameters;
+  number_of_dense_points_ = timeline_.empty() ? 0 : 1;
+}
+
+template<typename Frame>
+void DiscreteTrajectorySegment<Frame>::ClearDownsampling() {
+  downsampling_parameters_ = std::nullopt;
+}
+
+template<typename Frame>
+absl::Status DiscreteTrajectorySegment<Frame>::DownsampleIfNeeded() {
+  ++number_of_dense_points_;
+  // Points, hence one more than intervals.
+  if (number_of_dense_points_ >
+      downsampling_parameters_->max_dense_intervals) {
+    // Obtain iterators for all the dense points of the segment.
+    using ConstIterators = std::vector<typename Timeline::const_iterator>;
+    ConstIterators dense_iterators(number_of_dense_points_);
+    CHECK_LE(dense_iterators.size(), timeline_.size());
+    auto it = timeline_.crbegin();
+    for (int i = dense_iterators.size() - 1; i >= 0; --i) {
+      dense_iterators[i] = std::prev(it.base());
+      ++it;
+    }
+
+    absl::StatusOr<std::list<ConstIterators::const_iterator>>
+        right_endpoints = FitHermiteSpline<Instant, Position<Frame>>(
+            dense_iterators,
+            [](auto&& it) -> auto&& { return it->first; },
+            [](auto&& it) -> auto&& { return it->second.position(); },
+            [](auto&& it) -> auto&& { return it->second.velocity(); },
+            downsampling_parameters_->tolerance);
+    if (!right_endpoints.ok()) {
+      // Note that the actual appending took place; the propagated status only
+      // reflects a lack of downsampling.
+      return right_endpoints.status();
+    }
+
+    if (right_endpoints->empty()) {
+      number_of_dense_points_ = timeline_.empty() ? 0 : 1;
+      return absl::OkStatus();
+    }
+
+    // Obtain the times for the right endpoints.  This is necessary because we
+    // cannot use iterators for erasing points, as they would get invalidated
+    // after the first erasure.
+    std::vector<Instant> right_endpoints_times;
+    right_endpoints_times.reserve(right_endpoints.value().size());
+    for (auto const& it_in_dense_iterators : right_endpoints.value()) {
+      right_endpoints_times.push_back((*it_in_dense_iterators)->first);
+    }
+
+    // Poke holes in the timeline at the places given by
+    // |right_endpoints_times|.  This requires one lookup per erasure.
+    auto left_it = dense_iterators.front();
+    for (Instant const& right : right_endpoints_times) {
+      ++left_it;
+      auto const right_it = timeline_.find(right);
+      left_it = timeline_.erase(left_it, right_it);
+    }
+    number_of_dense_points_ = std::distance(left_it, timeline_.cend());
+  }
+  return absl::OkStatus();
 }
 
 template<typename Frame>

physics/discrete_trajectory_segment_test.cpp

@@ -32,8 +32,10 @@ using quantities::si::Milli;
 using quantities::si::Nano;
 using quantities::si::Radian;
 using quantities::si::Second;
+using testing_utilities::AppendTrajectorySegment;
 using testing_utilities::IsNear;
 using testing_utilities::NewCircularTrajectorySegment;
+using testing_utilities::NewEmptyTrajectorySegment;
 using testing_utilities::operator""_⑴;
 using ::testing::Eq;
 
@@ -63,6 +65,13 @@ class DiscreteTrajectorySegmentTest : public ::testing::Test {
     segment_->ForgetBefore(t);
   }
 
+  void SetDownsampling(
+      internal_discrete_trajectory_types::DownsamplingParameters const&
+          downsampling_parameters,
+      DiscreteTrajectorySegment<World>& segment) {
+    segment.SetDownsampling(downsampling_parameters);
+  }
+
   DiscreteTrajectorySegment<World>* segment_;
   internal_discrete_trajectory_types::Segments<World> segments_;
   Instant const t0_;
@@ -189,5 +198,40 @@ TEST_F(DiscreteTrajectorySegmentTest, Evaluate) {
               IsNear(10.4_⑴ * Nano(Metre / Second)));
 }
 
+TEST_F(DiscreteTrajectorySegmentTest, Downsampling) {
+  auto const circle = NewEmptyTrajectorySegment<World>();
+  auto const downsampled_circle = NewEmptyTrajectorySegment<World>();
+  SetDownsampling({.max_dense_intervals = 50, .tolerance = 1 * Milli(Metre)},
+                  *downsampled_circle->front());
+  AngularFrequency const ω = 3 * Radian / Second;
+  Length const r = 2 * Metre;
+  Time const Δt = 10 * Milli(Second);
+  Instant const t1 = t0_;
+  Instant const t2 = t0_ + 10 * Second;
+  AppendTrajectorySegment(
+      *NewCircularTrajectorySegment<World>(ω, r, Δt, t1, t2)->front(),
+      /*to=*/*circle->front());
+  AppendTrajectorySegment(
+      *NewCircularTrajectorySegment<World>(ω, r, Δt, t1, t2)->front(),
+      /*to=*/*downsampled_circle->front());
+
+  EXPECT_THAT(circle->front()->size(), Eq(1001));
+  EXPECT_THAT(downsampled_circle->front()->size(), Eq(77));
+  std::vector<Length> position_errors;
+  std::vector<Speed> velocity_errors;
+  for (auto const& [time, degrees_of_freedom] : *circle->front()) {
+    position_errors.push_back(
+        (downsampled_circle->front()->EvaluatePosition(time) -
+         degrees_of_freedom.position()).Norm());
+    velocity_errors.push_back(
+        (downsampled_circle->front()->EvaluateVelocity(time) -
+         degrees_of_freedom.velocity()).Norm());
+  }
+  EXPECT_THAT(*std::max_element(position_errors.begin(), position_errors.end()),
+              IsNear(0.98_⑴ * Milli(Metre)));
+  EXPECT_THAT(*std::max_element(velocity_errors.begin(), velocity_errors.end()),
+              IsNear(14_⑴ * Milli(Metre / Second)));
+}
+
 }  // namespace physics
 }  // namespace principia

physics/discrete_trajectory_types.hpp

@@ -7,6 +7,7 @@
 #include "base/macros.hpp"
 #include "geometry/named_quantities.hpp"
 #include "physics/degrees_of_freedom.hpp"
+#include "quantities/quantities.hpp"
 
 // An internal header to avoid replicating data structures in multiple places.
 // Doesn't export anything outside of its internal namespace.
@@ -21,6 +22,15 @@ namespace internal_discrete_trajectory_types {
 
 using geometry::Instant;
 using physics::DegreesOfFreedom;
+using quantities::Length;
+
+// |max_dense_intervals| is the maximal number of dense intervals before
+// downsampling occurs.  |tolerance| is the tolerance for downsampling with
+// |FitHermiteSpline|.
+struct DownsamplingParameters {
+  std::int64_t max_dense_intervals;
+  Length tolerance;
+};
 
 // The use of an unique_ptr here makes it possible to only depend on a forward
 // declaration of DiscreteTrajectorySegment.