GCC Code Coverage Report

Directory: ./
File: include/ndcurves/se3_curve.h
Date: 2025-03-05 17:18:30
Exec Total Coverage
Lines: 113 124 91.1%
Branches: 86 172 50.0%
Line Branch Exec Source
1 #ifndef _STRUCT_SE3_CURVE_H
2 #define _STRUCT_SE3_CURVE_H
3
4 #include <Eigen/Dense>
5 #include <boost/math/constants/constants.hpp>
6
7 #include "MathDefs.h"
8 #include "curve_abc.h"
9 #include "polynomial.h"
10 #include "so3_linear.h"
11
12 namespace ndcurves {
13
14 /// \class SE3Curve.
15 /// \brief Composition of a curve of any type of dimension 3 and a curve
16 /// representing an rotation (in current implementation, only SO3Linear can be
17 /// used for the rotation part) The output is a vector of size 7
18 /// (pos_x,pos_y,pos_z,quat_x,quat_y,quat_z,quat_w) The output of the derivative
19 /// of any order is a vector of size 6
20 /// (linear_x,linear_y,linear_z,angular_x,angular_y,angular_z)
21 ///
22 ///
23 template <typename Time = double, typename Numeric = Time, bool Safe = false>
24 struct SE3Curve : public curve_abc<Time, Numeric, Safe,
25 Eigen::Transform<Numeric, 3, Eigen::Affine>,
26 Eigen::Matrix<Numeric, 6, 1> > {
27 typedef Numeric Scalar;
28 typedef Eigen::Transform<Numeric, 3, Eigen::Affine> transform_t;
29 typedef transform_t point_t;
30 typedef Eigen::Matrix<Scalar, 6, 1> point_derivate_t;
31 typedef Eigen::Quaternion<Scalar> Quaternion;
32 typedef Time time_t;
33 typedef curve_abc<Time, Numeric, Safe, point_t, point_derivate_t>
34 curve_abc_t; // parent class
35 typedef polynomial<Time, Numeric, Safe, point_derivate_t> curve_derivate_t;
36 typedef curve_abc<Time, Numeric, Safe, pointX_t>
37 curve_X_t; // generic class of curve
38 typedef curve_abc<Time, Numeric, Safe, point3_t, point3_t>
39 curve_translation_t; // templated class used for the translation (return
40 // dimension are fixed)
41 typedef curve_abc<Time, Numeric, Safe, matrix3_t, point3_t>
42 curve_rotation_t; // templated class used for the rotation (return
43 // dimension are fixed)
44 typedef std::shared_ptr<curve_X_t> curve_ptr_t;
45 typedef std::shared_ptr<curve_rotation_t> curve_rotation_ptr_t;
46 typedef std::shared_ptr<curve_translation_t> curve_translation_ptr_t;
47
48 typedef SO3Linear<Time, Numeric, Safe> SO3Linear_t;
49 typedef polynomial<Time, Numeric, Safe, pointX_t> polynomial_t;
50 typedef SE3Curve<Time, Numeric, Safe> SE3Curve_t;
51
52 public:
53 /* Constructors - destructors */
54 /// \brief Empty constructor. Curve obtained this way can not perform other
55 /// class functions.
56 ///
57 24 SE3Curve()
58 : curve_abc_t(),
59 24 dim_(3),
60 24 translation_curve_(),
61 24 rotation_curve_(),
62 24 T_min_(0),
63 24 T_max_(0) {}
64
65 /// \brief Destructor
66 198 virtual ~SE3Curve() {
67 // should we delete translation_curve and rotation_curve here ?
68 // better switch to shared ptr
69 198 }
70
71 /* Constructor without curve object for the translation : */
72 /// \brief Constructor from init/end transform use polynomial of degree 1 for
73 /// position and SO3Linear for rotation
74 11 SE3Curve(const transform_t& init_transform, const transform_t& end_transform,
75 const time_t& t_min, const time_t& t_max)
76 : curve_abc_t(),
77 11 dim_(6),
78
3/4
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 1 times.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 11 translation_curve_(new polynomial3_t(
79
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 12 point3_t(init_transform.translation()),
80
3/6
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11 times.
✗ Branch 8 not taken.
 12 point3_t(end_transform.translation()), t_min, t_max)),
81
5/10
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
 10 rotation_curve_(new SO3Linear_t(
82 init_transform.rotation(), end_transform.rotation(), t_min, t_max)),
83 10 T_min_(t_min),
84
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 11 T_max_(t_max) {
85
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 safe_check();
86 11 }
87
88 /// \brief Constructor from init/end pose, with quaternion. use polynomial of
89 /// degree 1 for position and SO3Linear for rotation
90 2 SE3Curve(const point3_t& init_pos, const point3_t& end_pos,
91 const Quaternion& init_rot, const Quaternion& end_rot,
92 const time_t& t_min, const time_t& t_max)
93 : curve_abc_t(),
94 2 dim_(6),
95
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 translation_curve_(new polynomial3_t(init_pos, end_pos, t_min, t_max)),
96
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 rotation_curve_(new SO3Linear_t(init_rot, end_rot, t_min, t_max)),
97 2 T_min_(t_min),
98
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 T_max_(t_max) {
99
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 safe_check();
100 2 }
101
102 /// \brief Constructor from init/end pose, with rotation matrix. use
103 /// polynomial of degree 1 for position and SO3Linear for rotation
104 1 SE3Curve(const point3_t& init_pos, const point3_t& end_pos,
105 const matrix3_t& init_rot, const matrix3_t& end_rot,
106 const time_t& t_min, const time_t& t_max)
107 : curve_abc_t(),
108 1 dim_(6),
109
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 translation_curve_(new polynomial3_t(init_pos, end_pos, t_min, t_max)),
110
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 rotation_curve_(new SO3Linear_t(init_rot, end_rot, t_min, t_max)),
111 1 T_min_(t_min),
112
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 T_max_(t_max) {
113
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 safe_check();
114 1 }
115
116 /* Constructor with curve object for the translation : */
117 /// \brief Constructor from curve for the translation and init/end rotation,
118 /// with quaternion. Use SO3Linear for rotation with the same time bounds as
119 /// the
120 1 SE3Curve(curve_translation_ptr_t translation_curve,
121 const Quaternion& init_rot, const Quaternion& end_rot)
122 : curve_abc_t(),
123 1 dim_(6),
124 1 translation_curve_(translation_curve),
125
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 rotation_curve_(new SO3Linear_t(init_rot, end_rot,
126
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 translation_curve->min(),
127
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 translation_curve->max())),
128
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 T_min_(translation_curve->min()),
129
1/2
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 T_max_(translation_curve->max()) {
130
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 safe_check();
131 1 }
132 /// \brief Constructor from curve for the translation and init/end rotation,
133 /// with rotation matrix. Use SO3Linear for rotation with the same time bounds
134 /// as the
135 16 SE3Curve(curve_translation_ptr_t translation_curve, const matrix3_t& init_rot,
136 const matrix3_t& end_rot)
137 : curve_abc_t(),
138 16 dim_(6),
139 16 translation_curve_(translation_curve),
140
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 16 rotation_curve_(new SO3Linear_t(init_rot, end_rot,
141
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 16 translation_curve->min(),
142
2/4
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 16 times.
✗ Branch 6 not taken.
 16 translation_curve->max())),
143
1/2
✓ Branch 2 taken 16 times.
✗ Branch 3 not taken.
 16 T_min_(translation_curve->min()),
144
1/2
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 32 T_max_(translation_curve->max()) {
145
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 safe_check();
146 16 }
147
148 /* Constructor from translation and rotation curves object : */
149 /// \brief Constructor from from translation and rotation curves object
150 6 SE3Curve(curve_translation_ptr_t translation_curve,
151 curve_rotation_ptr_t rotation_curve)
152 : curve_abc_t(),
153 6 dim_(6),
154 6 translation_curve_(translation_curve),
155 6 rotation_curve_(rotation_curve),
156
1/2
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
 6 T_min_(translation_curve->min()),
157
1/2
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 12 T_max_(translation_curve->max()) {
158
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 6 times.
 6 if (translation_curve->dim() != 3) {
159 ✗ throw std::invalid_argument(
160 "The translation curve should be of dimension 3.");
161 }
162
3/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 4 times.
 6 if (rotation_curve->min() != T_min_) {
163
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 throw std::invalid_argument(
164 "Min bounds of translation and rotation curve are not the same.");
165 }
166
3/4
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 2 times.
 4 if (rotation_curve->max() != T_max_) {
167
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 throw std::invalid_argument(
168 "Max bounds of translation and rotation curve are not the same.");
169 }
170
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 safe_check();
171 14 }
172
173 /// \brief Evaluation of the SE3Curve at time t
174 /// \param t : time when to evaluate the spline.
175 /// \return \f$x(t)\f$ point corresponding on spline at time t.
176 /// (pos_x,pos_y,pos_z,quat_x,quat_y,quat_z,quat_w)
177 8648 virtual point_t operator()(const time_t t) const {
178
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 8648 times.
 8648 if (translation_curve_->dim() != 3) {
179 ✗ throw std::invalid_argument(
180 "Translation curve should always be of dimension 3");
181 }
182 8648 point_t res = point_t::Identity();
183
1/2
✓ Branch 3 taken 8640 times.
✗ Branch 4 not taken.
 8648 res.translate(point3_t((*translation_curve_)(t)));
184
1/2
✓ Branch 3 taken 8640 times.
✗ Branch 4 not taken.
 8640 res.rotate((*rotation_curve_)(t));
185 8640 return res;
186 }
187
188 /**
189 * @brief isApprox check if other and *this are approximately equals.
190 * Only two curves of the same class can be approximately equals, for
191 * comparison between different type of curves see isEquivalent
192 * @param other the other curve to check
193 * @param prec the precision threshold, default
194 * Eigen::NumTraits<Numeric>::dummy_precision()
195 * @return true is the two curves are approximately equals
196 */
197 18 bool isApprox(
198 const SE3Curve_t& other,
199 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
200
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 36 return ndcurves::isApprox<Numeric>(T_min_, other.min()) &&
201
2/2
✓ Branch 2 taken 12 times.
✓ Branch 3 taken 6 times.
 36 ndcurves::isApprox<Numeric>(T_max_, other.max()) &&
202 18 (translation_curve_ == other.translation_curve_ ||
203
2/2
✓ Branch 3 taken 7 times.
✓ Branch 4 taken 5 times.
 12 translation_curve_->isApprox(other.translation_curve_.get(),
204
3/4
✓ Branch 0 taken 18 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 8 times.
✓ Branch 3 taken 5 times.
 44 prec)) &&
205 13 (rotation_curve_ == other.rotation_curve_ ||
206
2/2
✓ Branch 3 taken 6 times.
✓ Branch 4 taken 2 times.
 26 rotation_curve_->isApprox(other.rotation_curve_.get(), prec));
207 }
208
209 7 virtual bool isApprox(
210 const curve_abc_t* other,
211 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
212
1/2
✓ Branch 0 taken 7 times.
✗ Branch 1 not taken.
 7 const SE3Curve_t* other_cast = dynamic_cast<const SE3Curve_t*>(other);
213
1/2
✓ Branch 0 taken 7 times.
✗ Branch 1 not taken.
 7 if (other_cast)
214 7 return isApprox(*other_cast, prec);
215 else
216 ✗ return false;
217 }
218
219 10 virtual bool operator==(const SE3Curve_t& other) const {
220 10 return isApprox(other);
221 }
222
223 4 virtual bool operator!=(const SE3Curve_t& other) const {
224 4 return !(*this == other);
225 }
226
227 /// \brief Evaluation of the derivative of order N of spline at time t.
228 /// \param t : the time when to evaluate the spline.
229 /// \param order : order of derivative.
230 /// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derivative
231 /// spline at time t.
232 829 virtual point_derivate_t derivate(const time_t t,
233 const std::size_t order) const {
234
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 829 times.
 829 if (translation_curve_->dim() != 3) {
235 ✗ throw std::invalid_argument(
236 "Translation curve should always be of dimension 3");
237 }
238
1/2
✓ Branch 2 taken 829 times.
✗ Branch 3 not taken.
 829 point_derivate_t res = point_derivate_t::Zero();
239
2/4
✓ Branch 3 taken 825 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 825 times.
✗ Branch 7 not taken.
 829 res.segment(0, 3) = point3_t(translation_curve_->derivate(t, order));
240
2/4
✓ Branch 3 taken 822 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 822 times.
✗ Branch 7 not taken.
 825 res.segment(3, 3) = rotation_curve_->derivate(t, order);
241 822 return res;
242 }
243
244 ✗ curve_derivate_t compute_derivate(const std::size_t /*order*/) const {
245 ✗ throw std::logic_error("Compute derivate for SE3 is not implemented yet.");
246 }
247
248 /// \brief Compute the derived curve at order N.
249 /// \param order : order of derivative.
250 /// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the
251 /// curve.
252 ✗ curve_derivate_t* compute_derivate_ptr(const std::size_t order) const {
253 ✗ return new curve_derivate_t(compute_derivate(order));
254 }
255
256 /*Helpers*/
257 /// \brief Get dimension of curve.
258 /// \return dimension of curve.
259 22 std::size_t virtual dim() const { return dim_; };
260 /// \brief Get the minimum time for which the curve is defined
261 /// \return \f$t_{min}\f$ lower bound of time range.
262 96 time_t min() const { return T_min_; }
263 /// \brief Get the maximum time for which the curve is defined.
264 /// \return \f$t_{max}\f$ upper bound of time range.
265 146 time_t max() const { return T_max_; }
266 /// \brief Get the degree of the curve.
267 /// \return \f$degree\f$, the degree of the curve.
268 ✗ virtual std::size_t degree() const { return translation_curve_->degree(); }
269 /// \brief const accessor to the translation curve
270 4 const curve_translation_ptr_t translation_curve() const {
271 4 return translation_curve_;
272 }
273 /// \brief const accessor to the rotation curve
274 4 const curve_rotation_ptr_t rotation_curve() const { return rotation_curve_; }
275 /*Helpers*/
276
277 /*Attributes*/
278 std::size_t dim_; // dim doesn't mean anything in this class ...
279 curve_translation_ptr_t translation_curve_;
280 curve_rotation_ptr_t rotation_curve_;
281 time_t T_min_, T_max_;
282 /*Attributes*/
283
284 // Serialization of the class
285 friend class boost::serialization::access;
286
287 template <class Archive>
288 88 void serialize(Archive& ar, const unsigned int version) {
289 if (version) {
290 // Do something depending on version ?
291 }
292
1/2
✓ Branch 3 taken 44 times.
✗ Branch 4 not taken.
 88 ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
293
1/2
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
 88 ar& boost::serialization::make_nvp("dim", dim_);
294
1/2
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
 88 ar& boost::serialization::make_nvp("translation_curve", translation_curve_);
295
1/2
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
 88 ar& boost::serialization::make_nvp("rotation_curve", rotation_curve_);
296
1/2
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
 88 ar& boost::serialization::make_nvp("T_min", T_min_);
297
1/2
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
 88 ar& boost::serialization::make_nvp("T_max", T_max_);
298 88 }
299
300 private:
301 32 void safe_check() {
302 if (Safe) {
303
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 32 times.
 32 if (T_min_ > T_max_) {
304 ✗ throw std::invalid_argument("Tmin should be inferior to Tmax");
305 }
306
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 32 times.
 32 if (translation_curve_->dim() != 3) {
307 ✗ throw std::invalid_argument(
308 "Translation curve should always be of dimension 3");
309 }
310 }
311 32 }
312
313 }; // SE3Curve
314
315 } // namespace ndcurves
316
317 DEFINE_CLASS_TEMPLATE_VERSION(
318 SINGLE_ARG(typename Time, typename Numeric, bool Safe),
319 SINGLE_ARG(ndcurves::SE3Curve<Time, Numeric, Safe>))
320
321 #endif // SE3_CURVE_H
322