GCC Code Coverage Report

Directory: ./
File: include/parametric-curves/spline.hpp
Date: 2025-05-07 13:05:43
Exec Total Coverage
Lines: 180 225 80.0%
Branches: 328 678 48.4%
Line Branch Exec Source
1 /**
2 * \file exact_cubic.h
3 * \brief class allowing to create an Exact cubic spline.
4 * \author Steve T.
5 * \version 0.1
6 * \date 06/17/2013
7 *
8 * This file contains definitions for the Spline class.
9 */
10
11 #ifndef _parameteric_curves_spline_hpp
12 #define _parameteric_curves_spline_hpp
13
14 #include <parametric-curves/MathDefs.h>
15
16 #include <boost/archive/text_iarchive.hpp>
17 #include <boost/archive/text_oarchive.hpp>
18 #include <boost/serialization/split_member.hpp>
19 #include <boost/serialization/vector.hpp>
20 #include <fstream>
21 #include <parametric-curves/abstract-curve.hpp>
22 #include <parametric-curves/curve-constraint.hpp>
23 #include <parametric-curves/polynomial.hpp>
24 #include <vector>
25 namespace parametriccurves {
26
27 /// \brief Creates coefficient vector of a cubic spline defined on the interval
28 /// [tBegin, tEnd]. It follows the equation
29 /// x(t) = a + b(t - t_min_) + c(t - t_min_)^2 + d(t - t_min_)^3
30 ///
31 template <typename Point, typename T_Point>
32 50 T_Point make_cubic_vector(Point const& a, Point const& b, Point const& c,
33 Point const& d) {
34 50 T_Point res;
35
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 50 res.push_back(a);
36
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 50 res.push_back(b);
37
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 50 res.push_back(c);
38
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 50 res.push_back(d);
39 50 return res;
40 }
41
42 template <typename Numeric, Eigen::Index Dim, typename Point, typename T_Point>
43 50 Polynomial<Numeric, Dim, Point> create_cubic(Point const& a, Point const& b,
44 Point const& c, Point const& d,
45 const Numeric min,
46 const Numeric max) {
47
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 50 T_Point coeffs = make_cubic_vector<Point, T_Point>(a, b, c, d);
48 return Polynomial<Numeric, Dim, Point>(coeffs.begin(), coeffs.end(), min,
49
1/2
✓ Branch 3 taken 25 times.
✗ Branch 4 not taken.
 100 max);
50 50 }
51 /// \brief Creates coefficient vector of a quintic spline defined on the
52 /// interval [tBegin, tEnd]. It follows the equation x(t) = a + b(t - t_min_) +
53 /// c(t - t_min_)^2 + d(t - t_min_)^3 + e(t - t_min_)^4 + f(t - t_min_)^5
54 ///
55 template <typename Point, typename T_Point>
56 2 T_Point make_quintic_vector(Point const& a, Point const& b, Point const& c,
57 Point const& d, Point const& e, Point const& f) {
58 2 T_Point res;
59
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 res.push_back(a);
60
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 res.push_back(b);
61
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 res.push_back(c);
62
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 res.push_back(d);
63
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 res.push_back(e);
64
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 res.push_back(f);
65 2 return res;
66 }
67
68 template <typename Numeric, Eigen::Index Dim, typename Point, typename T_Point>
69 2 Polynomial<Numeric, Dim, Point> create_quintic(Point const& a, Point const& b,
70 Point const& c, Point const& d,
71 Point const& e, Point const& f,
72 const Numeric min,
73 const Numeric max) {
74
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 T_Point coeffs = make_quintic_vector<Point, T_Point>(a, b, c, d, e, f);
75 return Polynomial<Numeric, Dim, Point>(coeffs.begin(), coeffs.end(), min,
76
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 4 max);
77 2 }
78
79 /// \class Spline
80 /// \brief Represents a set of cubic splines defining a continuous function
81 /// crossing each of the waypoint given in its initialization
82 ///
83 template <typename Numeric = double, Eigen::Index Dim = Eigen::Dynamic,
84 typename Point = Eigen::Matrix<Numeric, Dim, 1>,
85 typename SplineBase = Polynomial<Numeric, Dim, Point> >
86 struct Spline : public AbstractCurve<Numeric, Point> {
87 typedef Point point_t;
88 typedef std::vector<Point, Eigen::aligned_allocator<Point> > t_point_t;
89 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
90 typedef Numeric time_t;
91 typedef Numeric num_t;
92 typedef SplineBase spline_t;
93 typedef typename std::vector<spline_t, Eigen::aligned_allocator<spline_t> >
94 t_spline_t;
95 typedef typename t_spline_t::iterator it_spline_t;
96 typedef typename t_spline_t::const_iterator cit_spline_t;
97 typedef curve_constraints<point_t> spline_constraints;
98 typedef AbstractCurve<Numeric, Point> curve_abc_t;
99
100 public:
101 ///\brief Constructor
102
103
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 17 Spline() : curve_abc_t() {}
104
105 ///\brief Constructor
106 ///\param subSplines: vector of subsplines
107 ✗ Spline(const t_spline_t& subSplines)
108 ✗ : curve_abc_t(subSplines.front().tmin(), subSplines.back().tmax()),
109 ✗ subSplines_(subSplines) {}
110
111 ///\brief Copy Constructor
112 ✗ Spline(const Spline& other)
113 ✗ : curve_abc_t(other.subSplines_.front().tmin(),
114 ✗ other.subSplines_.front().tmax()),
115 ✗ subSplines_(other.subSplines_) {}
116
117 ///\brief Destructor
118 22 ~Spline() {}
119
120 public:
121 /* Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique
122 * set of cubic splines fulfulling those 4 restrictions :
123 * - x_i(t_i) = x_i* ; this means that the curve passes through each waypoint
124 * - x_i(t_i+1) = x_i+1* ;
125 * - its derivative is continous at t_i+1
126 * - its 2nd derivative is continous at t_i+1
127 * more details in paper "Task-Space Trajectories via Cubic Spline
128 * Optimization" By J. Zico Kolter and Andrew Y.ng (ICRA 2009) */
129 template <typename In>
130 8 void createSplineFromWayPoints(In wayPointsBegin, In wayPointsEnd) {
131
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
132
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 8 if (size < 1) {
133 ✗ throw; // TODO
134 }
135 8 subSplines_.clear();
136
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 subSplines_.reserve(size);
137
138 // refer to the paper to understand all this.
139
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 MatrixX h1 = MatrixX::Zero(size, size);
140
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 MatrixX h2 = MatrixX::Zero(size, size);
141
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 MatrixX h3 = MatrixX::Zero(size, size);
142
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 MatrixX h4 = MatrixX::Zero(size, size);
143
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 MatrixX h5 = MatrixX::Zero(size, size);
144
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 MatrixX h6 = MatrixX::Zero(size, size);
145
146
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 MatrixX a = MatrixX::Zero(size, Dim);
147
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 MatrixX b = MatrixX::Zero(size, Dim);
148
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 MatrixX c = MatrixX::Zero(size, Dim);
149
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 MatrixX d = MatrixX::Zero(size, Dim);
150
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 MatrixX x = MatrixX::Zero(size, Dim);
151 8 In it(wayPointsBegin), next(wayPointsBegin);
152 8 ++next;
153
154
2/2
✓ Branch 2 taken 13 times.
✓ Branch 3 taken 4 times.
 34 for (std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i) {
155 26 num_t const dTi((*next).first - (*it).first);
156 26 num_t const dTi_sqr(dTi * dTi);
157 26 num_t const dTi_cube(dTi_sqr * dTi);
158 // filling matrices values
159
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 h3(i, i) = -3 / dTi_sqr;
160
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 h3(i, i + 1) = 3 / dTi_sqr;
161
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 h4(i, i) = -2 / dTi;
162
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 h4(i, i + 1) = -1 / dTi;
163
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 h5(i, i) = 2 / dTi_cube;
164
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 h5(i, i + 1) = -2 / dTi_cube;
165
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 h6(i, i) = 1 / dTi_sqr;
166
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 h6(i, i + 1) = 1 / dTi_sqr;
167
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 4 times.
 26 if (i + 2 < size) {
168 18 In it2(next);
169 18 ++it2;
170 18 num_t const dTi_1((*it2).first - (*next).first);
171 18 num_t const dTi_1sqr(dTi_1 * dTi_1);
172 // this can be optimized but let's focus on clarity as long as not
173 // needed
174
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 18 h1(i + 1, i) = 2 / dTi;
175
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 18 h1(i + 1, i + 1) = 4 / dTi + 4 / dTi_1;
176
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 18 h1(i + 1, i + 2) = 2 / dTi_1;
177
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 18 h2(i + 1, i) = -6 / dTi_sqr;
178
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 18 h2(i + 1, i + 1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
179
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 18 h2(i + 1, i + 2) = 6 / dTi_1sqr;
180 }
181
3/6
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 13 times.
✗ Branch 9 not taken.
 26 x.row(i) = (*it).second.transpose();
182 }
183 // adding last x
184
3/6
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
 8 x.row(size - 1) = (*it).second.transpose();
185
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 a = x;
186
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 parametriccurves::PseudoInverse(h1);
187
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 8 b = h1 * h2 * x; // h1 * b = h2 * x => b = (h1)^-1 * h2 * x
188
4/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
 8 c = h3 * x + h4 * b;
189
4/8
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
 8 d = h5 * x + h6 * b;
190 8 it = wayPointsBegin, next = wayPointsBegin;
191 8 ++next;
192
193
2/2
✓ Branch 2 taken 13 times.
✓ Branch 3 taken 4 times.
 34 for (int i = 0; next != wayPointsEnd; ++i, ++it, ++next) {
194 26 Numeric min = (*it).first;
195 26 Numeric max = (*next).first;
196
13/26
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 13 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 13 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 13 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 13 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 13 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 13 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 13 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 13 times.
✗ Branch 38 not taken.
 26 Point a_ = a.row(i) - b.row(i) * min + c.row(i) * min * min -
197
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 26 d.row(i) * min * min * min;
198
11/22
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 13 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 13 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 13 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 13 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 13 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 13 times.
✗ Branch 32 not taken.
 26 Point b_ = b.row(i) - 2 * c.row(i) * min + 3 * d.row(i) * min * min;
199
6/12
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 13 times.
✗ Branch 17 not taken.
 26 Point c_ = c.row(i) - 3 * d.row(i) * min;
200
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 26 Point d_ = d.row(i);
201
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 26 subSplines_.push_back(create_cubic<Numeric, Dim, Point, t_point_t>(
202 a_, b_, c_, d_, min, max));
203 }
204 8 Numeric min = (*it).first;
205
12/24
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 4 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 4 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 4 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 4 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 4 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 4 times.
✗ Branch 35 not taken.
 16 Point a_ = a.row(size - 1) - b.row(size - 1) * min +
206
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 16 c.row(size - 1) * min * min - d.row(size - 1) * min * min * min;
207
9/18
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 4 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 4 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 4 times.
✗ Branch 26 not taken.
 8 Point b_ = b.row(size - 1) - 2 * c.row(size - 1) * min +
208
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 3 * d.row(size - 1) * min * min;
209
6/12
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 4 times.
✗ Branch 17 not taken.
 8 Point c_ = c.row(size - 1) - 3 * d.row(size - 1) * min;
210
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 Point d_ = d.row(size - 1);
211
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 subSplines_.push_back(
212 create_cubic<Numeric, Dim, Point, t_point_t>(a_, b_, c_, d_, min, min));
213
214
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 8 this->t_min = subSplines_.front().tmin();
215
1/2
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
 8 this->t_max = subSplines_.back().tmax();
216 16 return;
217 8 }
218
219 template <typename In>
220 2 void createSplineFromWayPointsConstr(In wayPointsBegin, In wayPointsEnd,
221 const spline_constraints& constraints) {
222
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
223
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 2 if (size < 1) throw; // TODO
224 2 subSplines_.clear();
225
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 subSplines_.reserve(size);
226
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 spline_constraints cons = constraints;
227 2 In it(wayPointsBegin), next(wayPointsBegin), end(wayPointsEnd - 1);
228 2 ++next;
229
2/2
✓ Branch 3 taken 8 times.
✓ Branch 4 taken 2 times.
 10 for (std::size_t i(0); next != end; ++next, ++it, ++i)
230
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 compute_one_spline<In>(it, next, cons, subSplines_);
231
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 compute_end_spline<In>(it, next, cons, subSplines_);
232 4 return;
233 2 }
234
235 public:
236 50 virtual const point_t operator()(const time_t& t) const {
237
7/10
✓ Branch 2 taken 25 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 25 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 25 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
✓ Branch 11 taken 24 times.
✓ Branch 12 taken 1 times.
✓ Branch 13 taken 24 times.
 50 if ((t < subSplines_.front().tmin() || t > subSplines_.back().tmax())) {
238
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 throw std::out_of_range("t is out of range");
239 }
240
1/2
✓ Branch 4 taken 58 times.
✗ Branch 5 not taken.
 116 for (cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++it) {
241
7/10
✓ Branch 2 taken 58 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 58 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 58 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 24 times.
✓ Branch 11 taken 34 times.
✓ Branch 12 taken 24 times.
✓ Branch 13 taken 34 times.
 116 if (t >= (it->tmin()) && t <= (it->tmax())) {
242
1/2
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
 48 return it->operator()(t);
243 }
244 }
245 ✗ const point_t dummy;
246 ✗ return dummy;
247 }
248
249 16 virtual const point_t derivate(const time_t& t,
250 const std::size_t& order) const {
251
5/10
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 8 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 8 times.
 16 if ((t < subSplines_.front().tmin() || t > subSplines_.back().tmax())) {
252 ✗ throw std::out_of_range("derivative call out of range");
253 }
254
1/2
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
 48 for (cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++it) {
255
7/10
✓ Branch 2 taken 24 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 24 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 8 times.
✓ Branch 11 taken 16 times.
✓ Branch 12 taken 8 times.
✓ Branch 13 taken 16 times.
 48 if (t >= (it->tmin()) && t <= (it->tmax())) {
256
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 16 return it->derivate(t, order);
257 }
258 }
259
260 ✗ const point_t dummy;
261 ✗ return dummy;
262 }
263
264 ✗ virtual const std::size_t& size() const { return subSplines_[0].size(); }
265 ✗ const t_spline_t& getSubsplines() const { return subSplines_; }
266
267 ✗ virtual bool setInitialPoint(const point_t& /*x_init*/) { return false; }
268 ✗ virtual bool setInitialPoint(const num_t& /*x_init*/) { return false; }
269
270 protected:
271 /*Attributes*/
272 t_spline_t subSplines_; // const
273
274 private:
275 template <typename In>
276 8 void compute_one_spline(In wayPointsBegin, In wayPointsNext,
277 spline_constraints& constraints,
278 t_spline_t& subSplines) const {
279
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
 8 const point_t &a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
280
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 const point_t &b0 = constraints.init_vel, c0 = constraints.init_acc / 2.;
281 8 const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
282 8 const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt;
283
7/14
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 8 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 8 times.
✗ Branch 20 not taken.
 8 const point_t d0 = (a1 - a0 - b0 * dt - c0 * dt_2) / dt_3;
284
285
10/20
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 8 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 8 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 8 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 8 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 8 times.
✗ Branch 29 not taken.
 8 Point a_ =
286 a0 - b0 * init_t + c0 * init_t * init_t - d0 * init_t * init_t * init_t;
287
8/16
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 8 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 8 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 8 times.
✗ Branch 23 not taken.
 8 Point b_ = b0 - 2 * c0 * init_t + 3 * d0 * init_t * init_t;
288
4/8
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
 8 Point c_ = c0 - 3 * d0 * init_t;
289
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 Point d_ = d0;
290
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 subSplines.push_back(create_cubic<Numeric, Dim, Point, t_point_t>(
291 a_, b_, c_, d_, init_t, end_t));
292
293
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 8 constraints.init_vel = subSplines.back().derivate(end_t, 1);
294
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
 8 constraints.init_acc = subSplines.back().derivate(end_t, 2);
295 8 }
296
297 template <typename In>
298 2 void compute_end_spline(In wayPointsBegin, In wayPointsNext,
299 spline_constraints& constraints,
300 t_spline_t& subSplines) const {
301
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 const point_t &a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
302
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const point_t &b0 = constraints.init_vel, b1 = constraints.end_vel,
303
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 c0 = constraints.init_acc / 2., c1 = constraints.end_acc;
304 2 const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
305 2 const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt,
306 2 dt_4 = dt_3 * dt, dt_5 = dt_4 * dt;
307 // solving a system of four linear eq with 4 unknows: d0, e0
308
6/12
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
 2 const point_t alpha_0 = a1 - a0 - b0 * dt - c0 * dt_2;
309
5/10
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
 2 const point_t alpha_1 = b1 - b0 - 2 * c0 * dt;
310
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 const point_t alpha_2 = c1 - 2 * c0;
311 2 const num_t x_d_0 = dt_3, x_d_1 = 3 * dt_2, x_d_2 = 6 * dt;
312 2 const num_t x_e_0 = dt_4, x_e_1 = 4 * dt_3, x_e_2 = 12 * dt_2;
313 2 const num_t x_f_0 = dt_5, x_f_1 = 5 * dt_4, x_f_2 = 20 * dt_3;
314
315
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 point_t d, e, f;
316
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 Eigen::MatrixXd rhs = Eigen::MatrixXd::Zero(3, Dim);
317
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 rhs.row(0) = alpha_0;
318
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 rhs.row(1) = alpha_1;
319
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 rhs.row(2) = alpha_2;
320
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 Eigen::Matrix3d eq = Eigen::Matrix3d::Zero();
321
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 eq(0, 0) = x_d_0;
322
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 eq(0, 1) = x_e_0;
323
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 eq(0, 2) = x_f_0;
324
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 eq(1, 0) = x_d_1;
325
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 eq(1, 1) = x_e_1;
326
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 eq(1, 2) = x_f_1;
327
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 eq(2, 0) = x_d_2;
328
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 eq(2, 1) = x_e_2;
329
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 eq(2, 2) = x_f_2;
330
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
 2 rhs = eq.inverse().eval() * rhs;
331
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 d = rhs.row(0);
332
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 e = rhs.row(1);
333
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 f = rhs.row(2);
334 2 num_t min = init_t;
335 2 Numeric min2 = min * min;
336 2 Numeric min3 = min2 * min;
337 2 Numeric min4 = min3 * min;
338 2 Numeric min5 = min4 * min;
339
11/22
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 2 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 2 times.
✗ Branch 32 not taken.
 2 Point a_ = a0 - b0 * min + c0 * min2 - d * min3 + e * min4 - f * min5;
340
13/26
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 2 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 2 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 2 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 2 times.
✗ Branch 38 not taken.
 2 Point b_ = b0 - 2 * c0 * min + 3 * d * min2 - 4 * e * min3 + 5 * f * min4;
341
10/20
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 2 times.
✗ Branch 29 not taken.
 2 Point c_ = c0 - 3 * d * min + 6 * e * min2 - 10 * f * min3;
342
7/14
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
 2 Point d_ = d - 4 * e * min + 10 * f * min2;
343
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
 2 Point e_ = e - 5 * f * min;
344
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Point f_ = f;
345
346
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 subSplines.push_back(create_quintic<Numeric, Dim, Point, t_point_t>(
347 a_, b_, c_, d_, e_, f_, init_t, end_t));
348 2 }
349
350 // Serialization of the class
351 friend class boost::serialization::access;
352 template <class Archive>
353 ✗ void save(Archive& ar, const unsigned int /*version*/) const {
354 ✗ ar& subSplines_;
355
356 ✗ return;
357 }
358
359 template <class Archive>
360 ✗ void load(Archive& ar, const unsigned int /*version*/) {
361 ✗ ar& subSplines_;
362
363 ✗ this->t_min = subSplines_.front().tmin();
364 ✗ this->t_max = subSplines_.back().tmax();
365 ✗ return;
366 }
367
368 ✗ BOOST_SERIALIZATION_SPLIT_MEMBER()
369
370 public:
371 ✗ bool loadFromFile(const std::string& filename) {
372 ✗ std::ifstream ifs(filename.c_str());
373 ✗ if (ifs) {
374 ✗ boost::archive::text_iarchive ia(ifs);
375 ✗ Spline& cubic_spline = *static_cast<Spline*>(this);
376 ✗ ia >> cubic_spline;
377 ✗ } else {
378 ✗ const std::string exception_message(filename +
379 " does not seem to be a valid file.");
380 ✗ throw std::invalid_argument(exception_message);
381 return false;
382 }
383 ✗ return true;
384 }
385
386 /// \brief Saved a Derived object as a text file.
387 ✗ bool saveToFile(const std::string& filename) const {
388 ✗ std::ofstream ofs(filename.c_str());
389 ✗ if (ofs) {
390 ✗ boost::archive::text_oarchive oa(ofs);
391 ✗ oa << *static_cast<const Spline*>(this);
392 ✗ } else {
393 ✗ const std::string exception_message(filename +
394 " does not seem to be a valid file.");
395 ✗ throw std::invalid_argument(exception_message);
396 return false;
397 }
398 ✗ return true;
399 }
400
401 // BOOST_SERIALIZATION_SPLIT_MEMBER()
402 };
403 } // namespace parametriccurves
404 #endif //_CLASS_EXACTCUBIC
405