Drake
spline_helpers.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <functional>
4 #include <memory>
5 #include <vector>
6 
7 #include "ignition/math/Spline.hh"
8 #include "ignition/math/Vector3.hh"
9 
11 
12 namespace drake {
13 namespace maliput {
14 namespace rndf {
15 
21  public:
23 
24 
25  explicit InverseFunctionInterpolator(std::function<double(double)> function,
41  double xmin, double xmax,
42  double error_boundary);
43 
59  double InterpolateMthDerivative(int derivative_order, double y);
60 
61  private:
62  // A segment of a single-variable function graph.
63  struct FunctionGraphSegment {
64  double xmin; //< Lower x bound.
65  double xmax; //< Upper x bound.
66  double ymin; //< Lower y bound.
67  double ymax; //< Upper y bound.
68  };
69 
70  // A partition of a single-variable function graph, as a segment plus a set
71  // of sub partitions.
72  struct FunctionGraphPartition {
73  // Segment covered.
74  FunctionGraphSegment segment;
75  // Sub-partitions covered.
76  std::vector<std::unique_ptr<FunctionGraphPartition>> partitions;
77  };
78 
79  const std::function<double(double)> function_; //< Base function.
80  const double error_boundary_{}; //< Error boundary for interpolation.
81  const int partition_tree_degree_{}; //< Function partition tree degree.
82  const int partition_tree_max_depth_{}; //< Function partition tree max depth.
83  std::unique_ptr<FunctionGraphPartition>
84  partition_tree_; //< Function partition tree.
85 };
86 
90  public:
92 
93 
103  std::unique_ptr<ignition::math::Spline> spline, double error_boundary);
104 
115  ignition::math::Vector3d InterpolateMthDerivative(int derivative_order,
116  double s);
117 
119  inline ignition::math::Spline* BaseSpline() { return this->q_t_.get(); }
120 
136  double FindClosestPointTo(const ignition::math::Vector3d& point,
137  double step) const;
138 
139  private:
140  std::unique_ptr<ignition::math::Spline> q_t_; //< Parameterized curve Q(t).
141  std::unique_ptr<InverseFunctionInterpolator>
142  F_ts_; //< Inverse path length function t(s).
143 };
144 
154 std::vector<ignition::math::Vector3d> SplineToBezier(
155  const ignition::math::Vector3d& p0, const ignition::math::Vector3d& t0,
156  const ignition::math::Vector3d& p1, const ignition::math::Vector3d& t1);
157 
170 std::vector<ignition::math::Vector3d> BezierToSpline(
171  const ignition::math::Vector3d& p0, const ignition::math::Vector3d& p1,
172  const ignition::math::Vector3d& p2, const ignition::math::Vector3d& p3);
173 
208 std::vector<ignition::math::Vector3d> MakeBezierCurveMonotonic(
209  const std::vector<ignition::math::Vector3d>& control_points, double scale);
210 
223 std::unique_ptr<ignition::math::Spline> CreatePChipBasedSpline(
224  const std::vector<ignition::math::Vector3d>& positions);
225 
226 } // namespace rndf
227 } // namespace maliput
228 } // namespace drake
std::vector< ignition::math::Vector3d > MakeBezierCurveMonotonic(const std::vector< ignition::math::Vector3d > &control_points, double scale=1.0)
Provides a conditionally convex and monotonic Bezier curve given a vector of control points control_p...
Definition: spline_helpers.cc:309
Definition: automotive_demo.cc:88
std::unique_ptr< ignition::math::Spline > CreatePChipBasedSpline(const std::vector< ignition::math::Vector3d > &positions)
Creates a ignition::math::Spline from a set of positions.
Definition: spline_helpers.cc:383
InverseFunctionInterpolator(const InverseFunctionInterpolator &)=delete
double InterpolateMthDerivative(int derivative_order, double y)
Interpolates , that is, the inverse of the given function.
Definition: spline_helpers.cc:63
double y
Definition: vtk_util_test.cc:26
std::vector< ignition::math::Vector3d > SplineToBezier(const ignition::math::Vector3d &p0, const ignition::math::Vector3d &t0, const ignition::math::Vector3d &p1, const ignition::math::Vector3d &t1)
Provides the equivalent set of points in cubic Bezier base from two pairs of points and tangents at t...
Definition: spline_helpers.cc:247
ignition::math::Spline * BaseSpline()
Definition: spline_helpers.h:119
A linear interpolator for arbitrary inverse functions.
Definition: spline_helpers.h:20
std::vector< ignition::math::Vector3d > BezierToSpline(const ignition::math::Vector3d &p0, const ignition::math::Vector3d &p1, const ignition::math::Vector3d &p2, const ignition::math::Vector3d &p3)
Provides the equivalent set of points in cubic spline base from four cubic Bezier control points...
Definition: spline_helpers.cc:278
An extension for ignition::math::Splines that reparameterizes them by path length.
Definition: spline_helpers.h:89
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:35
Provides careful macros to selectively enable or disable the special member functions for copy-constr...