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 
16 /// A linear interpolator for arbitrary inverse functions.
17 /// Helpful for path-length parameterization with ignition::math::Splines.
18 /// Given a function F, its domain D, and codomain CD, this class gives a linear
19 /// interpolant of F's inverse that maps CD to D.
21  public:
23 
24  /// Constructor that takes the function, its domain interval and an error
25  /// boundary for the interpolation.
26  /// The valid range of @p function's codomain is determined as follows. The
27  /// minimum value is @p xmin applied to @p function. The maximum value is
28  /// @p xmax applied to @p function. This is valid because @p function is
29  /// assumed to be monotonically increasing with x.
30  ///
31  /// @param[in] function an arbitrary continuous function for which to
32  /// interpolate an inverse. It should be monotonically increasing with x.
33  /// @param[in] xmin @p function 's domain interval low bound.
34  /// @param[in] xmax @p function 's domain interval upper bound.
35  /// @param[in] error_boundary a positive constraint on the maximum error
36  /// allowed when approximating the inverse function.
37  /// @throws std::runtime_error When @p error_boundary is not positive.
38  /// @throws std::runtime_error When @p xmin is equal or greater than @p xmax.
39  /// @throws std::runtime_error When evaluating @p function throws.
40  explicit InverseFunctionInterpolator(std::function<double(double)> function,
41  double xmin, double xmax,
42  double error_boundary);
43 
44  /// Interpolates @f$ x^{(derivative_order)}(y) @f$, that is, the inverse of
45  /// the given function.
46  /// @param[in] derivative_order a non-negative integer describing
47  /// the order of the inverse function derivative to interpolate. Any value
48  /// bigger than 1 will make the function return 0.0 as this is a linear
49  /// interpolant.
50  /// @param[in] y the range value to interpolate at, constrained by the direct
51  /// function image, which is inside the codomain of the direct function.
52  /// @return interpolated @p derivative_order derivative
53  /// @f$ x^{(derivative_order)}(y) @f$ .
54  /// @throws when @p derivative_order is a negative integer.
55  /// @throws when @p y is larger than the maximum range of the function's
56  /// codomain as described in the constructor's documentation.
57  /// @throws std::runtime_error When @p y is smaller than the minimum range of
58  /// the function's codomain as described in the constructor's documentation.
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 
87 /// An extension for ignition::math::Splines that reparameterizes
88 /// them by path length.
90  public:
92 
93  /// Constructor that takes a spline and an error bound
94  /// for path length parameterization approximations.
95  /// @param[in] spline the spline to be parameterized by path length.
96  /// @param[in] error_boundary a positive constraint on the
97  /// maximum error allowed when approximating the path length
98  /// parameterization.
99  /// @throws std::runtime_error When @p spline is nullptr.
100  /// @throws std::runtime_error When @p error_boundary is not a positive
101  /// number.
103  std::unique_ptr<ignition::math::Spline> spline, double error_boundary);
104 
105  /// Interpolates @f$ Q(s) @f$, that is, the spline parameterized
106  /// by path length.
107  /// @param[in] derivative_order a non-negative integer describing
108  /// the order of the function derivative to interpolate. Since cubic
109  /// interpolation is done, any derivative order greater than 3 will be zero.
110  /// @param[in] s path length to interpolate at, constrained
111  /// by the curve dimensions [0, path_length].
112  /// @return the @p derivative_order derivative
113  /// @f$ Q^{(derivative_order)}(s) @f$.
114  /// @throws std::runtime_error If @p derivative_order is a negative integer.
115  ignition::math::Vector3d InterpolateMthDerivative(int derivative_order,
116  double s);
117 
118  /// @return a mutable pointer to the underlying spline.
119  inline ignition::math::Spline* BaseSpline() { return this->q_t_.get(); }
120 
121  /// Finds the closest point on the spline to a point in space.
122  ///
123  /// It will iterate over the range of s of the spline which is 0 to 1 in terms
124  /// of ignition::math::Spline's parameter at constant path length @p step
125  /// increments. On each iteration we get the distance and save the path
126  /// length coordinate that gives the minimum distance.
127  ///
128  /// @param point the point in space from which we want to get the s path
129  /// length value whose image of the direct function (the spline) within 0 and
130  /// the total length of the direct function.
131  /// @param step the path length increment to iterate over the complete spline
132  /// path length.
133  /// @return the path length value that applied to the
134  /// ignition::math::Spline, which was provided at construction time, has a
135  /// minimum distance to @p point.
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 
145 /// Provides the equivalent set of points in cubic Bezier base from two pairs
146 /// of points and tangents at the extents of a spline.
147 /// @param p0 A vector that describes the starting position of the curve.
148 /// @param t0 A vector that describes the tangent at @p p0.
149 /// @param p1 A vector that describes the ending position of the curve.
150 /// @param t1 A vector that describes the tangent at @p p1.
151 /// @return A vector containing four Bezier control points. The first and last
152 /// points are the extent points of the Bezier curve and the other two are the
153 /// tangent controlling waypoints.
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 
158 /// Provides the equivalent set of points in cubic spline base from four cubic
159 /// Bezier control points.
160 /// @param p0 A vector that describes the starting position of the curve.
161 /// @param p1 A vector that describes the first control point of the curve.
162 /// @param p2 A vector that describes the second control point of the curve.
163 /// @param p3 A vector that describes the last control point of the curve.
164 /// @return A vector containing four spline control points. The points are
165 /// returned in the following order:
166 /// 1. index 0 --> curve position at the beginning.
167 /// 2. index 1 --> curve tangent at the beginning.
168 /// 3. index 2 --> curve position at the ending.
169 /// 4. index 3 --> curve tangent at the ending.
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 
174 /// Provides a conditionally convex and monotonic Bezier curve given a vector of
175 /// control points @p control_points.
176 /// First it computes the intersection of the lines represented by point and
177 /// tangent at the beginning and at the end of the curve. From here we have a
178 /// a first branch in the behavior, if there is no intersection, we assume that
179 /// all these curves are 2-D curves over the z = 0.0 api::GeoPosition frame.
180 /// Then, we create two intermediate control points for them that will provide a
181 /// S shape to match the curve. The change in convexity is set to be in the mid
182 /// point of the extents of the curve. In case there is an intersection between
183 /// the two lines, we find it and this will be the critical point. From this
184 /// step, there are two types of geometries we can generate but for both, the
185 /// control points at the extents remain the same. For those control points in
186 /// between we use the following equations to compute them:
187 ///
188 /// @p control_points[1] = @p control_points[0] +
189 /// (±1.0) * @p scale * (critical_point -
190 /// control_points[0])
191 /// @p control_points[2] = @p control_points[3] +
192 /// (∓1.0) * @p scale * (critical_point -
193 /// control_points[3])
194 ///
195 /// When the curve preserves convexity, @p scale is multiplied by (+1.0) in both
196 /// cases. However, when it is not, opposite signs are used.
197 /// @param control_points A vector containing four Bezier control points. The
198 /// first and last points are the extent points of the Bezier curve and the
199 /// other two are the tangent controlling waypoints.
200 /// @param scale A scale factor with a range value between 0.0 and 1.0.
201 /// @return A vector containing four Bezier control points. The first and last
202 /// points are the extent points of the Bezier curve and the other two are the
203 /// tangent controlling waypoints.
204 /// @throws std::runtime_error When the size of @p control_points is different
205 /// from 4.
206 /// @throws std::runtime_error When @p scale is bigger than 1.0.
207 /// @throws std::runtime_error When @p scale is smaller than 0.0.
208 std::vector<ignition::math::Vector3d> MakeBezierCurveMonotonic(
209  const std::vector<ignition::math::Vector3d>& control_points, double scale);
210 
211 /// Creates a ignition::math::Spline from a set of @p positions. These positions
212 /// are the control points where the curve must go through. The final curve is
213 /// based from a PChip algorithm, which makes the interpolation safe in terms of
214 /// piecewise convexity and monotonicity.
215 /// @param positions A vector of position where the spline should go through. It
216 /// should have more than two points. In addition, two consecutive points that
217 /// have a length of zero will throw an exception since it's not yet supported.
218 /// @return A ignition's Spline containing as knots the positions vector and as
219 /// tangent's the PChip's interpolated value.
220 /// @throws std::runtime_error When positions' size is less than three.
221 /// @throws std::runtime_error When two consecutive @p positions' items have a
222 /// distance of zero.
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
int y
Definition: rgbd_renderer_test.cc:33
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:393
Definition: branch_point.cc:7
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
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:33
Provides careful macros to selectively enable or disable the special member functions for copy-constr...