Merge pull request #117 from ethz-asl/fix/extrema_sampling

Fix/extrema sampling

mav_trajectory_generation/include/mav_trajectory_generation/impl/polynomial_optimization_linear_impl.h

@@ -414,32 +414,44 @@ void PolynomialOptimization<_N>::
     computeSegmentMaximumMagnitudeCandidatesBySampling(
         const Segment& segment, double t_start, double t_stop, double dt,
         std::vector<double>* candidates) {
-  Eigen::VectorXd value_old, value_start;
-  value_start = segment.evaluate(t_start - dt, Derivative);
-
-  value_old = segment.evaluate(t_start, Derivative);
-
-  // Determine initial direction from t_start -dt to t_start.
-  // t_start may be an extremum, especially for start and end vertices!
-  double direction = value_old.norm() - value_start.norm();
-
-  // Continue with direction from t_start to t_start + dt until t_stop + dt.
-  // Again, there may be an extremum at t_stop (e.g. end vertex).
-  for (double t = t_start + dt; t < t_stop + dt; t += dt) {
-    Eigen::VectorXd value_new;
+  CHECK_NOTNULL(candidates);
+  // Start is candidate.
+  candidates->push_back(t_start);
+
+  // Determine initial direction from t_start to t_start + dt.
+  auto t_old = t_start + dt;
+  auto value_new = segment.evaluate(t_old, Derivative);
+  auto value_old = segment.evaluate(t_start, Derivative);
+  auto direction = value_new.norm() - value_old.norm();
+
+  // Continue with direction from t_start + dt to t_start + 2 dt until t_stop.
+  bool last_sample = false;
+  for (double t = t_start + dt + dt; t <= t_stop; t += dt) {
+    // Update direction.
+    value_old = value_new;
     value_new = segment.evaluate(t, Derivative);
-
-    double direction_new = value_new.norm() - value_old.norm();
+    auto direction_new = value_new.norm() - value_old.norm();
 
     if (std::signbit(direction) != std::signbit(direction_new)) {
-      Eigen::VectorXd value_deriv = segment.evaluate(t - dt, Derivative + 1);
+      auto value_deriv = segment.evaluate(t_old, Derivative + 1);
       if (value_deriv.norm() < 1e-2) {
-        candidates->push_back(t - dt);  // extremum was at last dt
+        candidates->push_back(t_old);  // extremum was at last dt
       }
     }
 
-    value_old = value_new;
     direction = direction_new;
+    t_old = t;
+
+    // Check last sample before t_stop.
+    if ((t + dt) > t_stop && !last_sample) {
+      t = t_stop - dt;
+      last_sample = true;
+    }
+  }
+
+  // End is candidates.
+  if (candidates->back() != t_stop) {
+    candidates->push_back(t_stop);
   }
 }
 

mav_trajectory_generation/src/segment.cpp

@@ -63,6 +63,7 @@ void printSegment(std::ostream& stream, const Segment& s, int derivative) {
   stream << " coefficients for " << positionDerivativeToString(derivative)
          << ": " << std::endl;
   for (int i = 0; i < s.D(); ++i) {
+    stream << "dim " << i << ": " << std::endl;
     stream << s[i].getCoefficients(derivative) << std::endl;
   }
 }
@@ -211,11 +212,11 @@ bool Segment::getSegmentWithAppendedDimension(const Segment& segment_to_append,
   // Get common polynomial order.
   const int new_N = std::max(segment_to_append.N(), N_);
   const int new_D = D_ + segment_to_append.D();
-  
+
   // Create temporary segments to scale polynomials if necessary.
   Segment current_segment = *this;
   Segment segment_to_append_temp = segment_to_append;
-  
+
   // Scale segment polynomials to the longer segment time.
   const double new_time = std::max(time_, segment_to_append.getTime());
   if (time_ < new_time && new_time > 0.0){
@@ -227,7 +228,7 @@ bool Segment::getSegmentWithAppendedDimension(const Segment& segment_to_append,
       segment_to_append_temp[d].scalePolynomialInTime(segment_to_append.getTime() / new_time);
     }
   }
-  
+
   *new_segment = Segment(new_N, new_D);
 
   if (N_ == segment_to_append.N()) {

mav_trajectory_generation/test/test_polynomial_optimization.cpp

@@ -335,24 +335,18 @@ TEST_P(PolynomialOptimizationTests, ExtremaOfMagnitude) {
 
     std::vector<double> res_sampling;
     time_sampling.Start();
+    const double dt = 0.01;
     opt.computeSegmentMaximumMagnitudeCandidatesBySampling<kDerivative>(
-        s, 0, s.getTime(), 0.01, &res_sampling);
+        s, 0, s.getTime(), dt, &res_sampling);
     time_sampling.Stop();
 
-    // First check that the candidates are ACTUAL extrema.
-    // Do this by evaluating the derivative+1 of the segment.
-    for (double candidate : res_sampling) {
-      Eigen::VectorXd sampled = s.evaluate(candidate, kDerivative + 1);
-      EXPECT_NEAR(0.0, sampled.norm(), 0.01) << "Sampled Result: Time: "
-                                             << candidate;
-    }
-
-    constexpr double check_tolerance = 0.01;
+    const double check_tolerance = 2 * dt; // Nyquist resolution
     bool success = checkExtrema(res_sampling, res, check_tolerance);
     if (!success) {
       std::cout << "############CHECK XTREMA FAILED: \n";
       std::cout << "segment idx: " << segment_idx << "/" << segments.size()
-                << " time: " << s.getTime() << std::endl;
+                << " time: " << s.getTime() << std::endl
+                << "segment: " << s << std::endl;
 
       std::cout << "analytically found: ";
       for (const double& t : res) {