Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Guilhem Saurel
ndcurves
Commits
9829c732
Commit
9829c732
authored
Dec 20, 2019
by
Pierre Fernbach
Browse files
isApprox can now return true only with two curves of the same type
parent
aa7b386c
Changes
9
Hide whitespace changes
Inline
Side-by-side
include/curves/bezier_curve.h
View file @
9829c732
...
...
@@ -140,9 +140,15 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
}
}
virtual
bool
isApprox
(
const
bezier_curve_t
&
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
(),
const
size_t
order
=
5
)
const
{
//std::cout<<"is approx in bezier called."<<std::endl;
(
void
)
order
;
/**
* @brief isApprox check if other and *this are approximately equals.
* Only two curves of the same class can be approximately equals, for comparison between different type of curves see isEquivalent
* @param other the other curve to check
* @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals
*/
bool
isApprox
(
const
bezier_curve_t
&
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
())
const
{
std
::
cout
<<
"is approx in bezier called."
<<
std
::
endl
;
bool
equal
=
T_min_
==
other
.
min
()
&&
T_max_
==
other
.
max
()
&&
dim_
==
other
.
dim
()
...
...
@@ -160,6 +166,14 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
return
true
;
}
virtual
bool
isApprox
(
const
curve_abc_t
*
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
())
const
{
const
bezier_curve_t
*
other_cast
=
dynamic_cast
<
const
bezier_curve_t
*>
(
other
);
if
(
other_cast
)
return
isApprox
(
*
other_cast
,
prec
);
else
return
false
;
}
virtual
bool
operator
==
(
const
bezier_curve_t
&
other
)
const
{
return
isApprox
(
other
);
...
...
@@ -169,15 +183,6 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
return
!
(
*
this
==
other
);
}
virtual
bool
isApprox
(
const
curve_abc_t
&
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
(),
const
size_t
order
=
5
)
const
{
const
bezier_curve_t
*
other_cast
=
dynamic_cast
<
const
bezier_curve_t
*>
(
&
other
);
if
(
other_cast
)
return
isApprox
(
*
other_cast
);
else
return
curve_abc_t
::
isApprox
(
other
,
prec
,
order
);
}
/// \brief Compute the derived curve at order N.
/// Computes the derivative order N, \f$\frac{d^Nx(t)}{dt^N}\f$ of bezier curve of parametric equation x(t).
...
...
include/curves/cubic_hermite_spline.h
View file @
9829c732
...
...
@@ -104,17 +104,14 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
}
/**
* @brief isApprox check if other and *this are equals, given a precision treshold.
* This test is done by discretizing, it should be re-implemented in the child class to check exactly
* all the members.
* @brief isApprox check if other and *this are approximately equals.
* Only two curves of the same class can be approximately equals, for comparison between different type of curves see isEquivalent
* @param other the other curve to check
* @param order the order up to which the derivatives of the curves are checked for equality
* @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals
*/
virtual
bool
isApprox
(
const
cubic_hermite_spline_t
&
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
(),
const
size_t
order
=
5
)
const
{
//std::cout<<"is approx in hermite called."<<std::endl;
(
void
)
order
;
// silent warning, order is not used in this class.
bool
isApprox
(
const
cubic_hermite_spline_t
&
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
())
const
{
std
::
cout
<<
"is approx in hermite called."
<<
std
::
endl
;
bool
equal
=
T_min_
==
other
.
min
()
&&
T_max_
==
other
.
max
()
&&
dim_
==
other
.
dim
()
...
...
@@ -133,6 +130,14 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
return
true
;
}
virtual
bool
isApprox
(
const
curve_abc_t
*
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
())
const
{
const
cubic_hermite_spline_t
*
other_cast
=
dynamic_cast
<
const
cubic_hermite_spline_t
*>
(
other
);
if
(
other_cast
)
return
isApprox
(
*
other_cast
,
prec
);
else
return
false
;
}
virtual
bool
operator
==
(
const
cubic_hermite_spline_t
&
other
)
const
{
return
isApprox
(
other
);
}
...
...
@@ -141,14 +146,6 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
return
!
(
*
this
==
other
);
}
virtual
bool
isApprox
(
const
curve_abc_t
&
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
(),
const
size_t
order
=
5
)
const
{
const
cubic_hermite_spline_t
*
other_cast
=
dynamic_cast
<
const
cubic_hermite_spline_t
*>
(
&
other
);
if
(
other_cast
)
return
isApprox
(
*
other_cast
);
else
return
curve_abc_t
::
isApprox
(
other
,
prec
,
order
);
}
/// \brief Evaluate the derivative of order N of spline at time t.
...
...
include/curves/curve_abc.h
View file @
9829c732
...
...
@@ -59,42 +59,17 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$, point corresponding on derivative curve of order N at time t.
virtual
point_derivate_t
derivate
(
const
time_t
t
,
const
std
::
size_t
order
)
const
=
0
;
/**
* @brief isApprox check if other and *this are equals
,
given a precision treshold
.
*
This test is done by discretizing, it should be re-implemented in the child class to check exactly
*
all the members
.
* @brief isApprox check if other and *this are
approximately
equals given a precision treshold
*
Only two curves of the same class can be approximately equals,
*
for comparison between different type of curves see isEquivalent
.
* @param other the other curve to check
* @param order the order up to which the derivatives of the curves are checked for equality
* @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals
*/
virtual
bool
isApprox
(
const
curve_t
&
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
(),
const
size_t
order
=
5
)
const
{
bool
equal
=
(
min
()
==
other
.
min
())
&&
(
max
()
==
other
.
max
())
&&
(
dim
()
==
other
.
dim
());
if
(
!
equal
){
return
false
;
}
// check the value along the two curves
Numeric
t
=
min
();
while
(
t
<=
max
()){
if
(
!
(
*
this
)(
t
).
isApprox
(
other
(
t
),
prec
)){
return
false
;
}
t
+=
0.01
;
// FIXME : define this step somewhere ??
}
// check if the derivatives are equals
for
(
size_t
n
=
1
;
n
<=
order
;
++
n
){
t
=
min
();
while
(
t
<=
max
()){
if
(
!
derivate
(
t
,
n
).
isApprox
(
other
.
derivate
(
t
,
n
),
prec
)){
return
false
;
}
t
+=
0.01
;
// FIXME : define this step somewhere ??
}
}
return
true
;
}
virtual
bool
isApprox
(
const
curve_t
*
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
())
const
=
0
;
/*Operations*/
...
...
include/curves/exact_cubic.h
View file @
9829c732
...
...
@@ -109,6 +109,17 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> {
/// \brief Destructor.
virtual
~
exact_cubic
()
{}
/**
* @brief isApprox check if other and *this are approximately equals.
* Only two curves of the same class can be approximately equals, for comparison between different type of curves see isEquivalent
* @param other the other curve to check
* @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals
*/
bool
isApprox
(
const
exact_cubic_t
&
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
())
const
{
return
piecewise_curve_t
::
isApprox
(
other
,
prec
);
}
std
::
size_t
getNumberSplines
()
{
return
this
->
getNumberCurves
();
}
spline_t
getSplineAt
(
std
::
size_t
index
)
{
...
...
include/curves/helpers/effector_spline_rotation.h
View file @
9829c732
...
...
@@ -73,21 +73,27 @@ class rotation_spline : public curve_abc_quat_t {
}
/**
* @brief isApprox check if other and *this are equals, given a precision treshold.
* This test is done by discretizing, it should be re-implemented in the child class to check exactly
* all the members.
* @brief isApprox check if other and *this are approximately equals.
* Only two curves of the same class can be approximately equals, for comparison between different type of curves see isEquivalent
* @param other the other curve to check
* @param order the order up to which the derivatives of the curves are checked for equality
* @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals
*/
virtual
bool
isApprox
(
const
rotation_spline
&
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
()
,
const
size_t
order
=
5
)
const
{
bool
isApprox
(
const
rotation_spline
&
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
())
const
{
return
min_
==
other
.
min_
&&
max_
==
other
.
max_
&&
dim_
==
other
.
dim_
&&
quat_from_
.
isApprox
(
other
.
quat_from_
,
prec
)
&&
quat_to_
.
isApprox
(
other
.
quat_to_
,
prec
)
&&
time_reparam_
.
isApprox
(
other
.
time_reparam_
,
prec
,
order
);
&&
time_reparam_
.
isApprox
(
other
.
time_reparam_
,
prec
);
}
virtual
bool
isApprox
(
const
curve_abc_quat_t
*
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
())
const
{
const
rotation_spline
*
other_cast
=
dynamic_cast
<
const
rotation_spline
*>
(
other
);
if
(
other_cast
)
return
isApprox
(
*
other_cast
,
prec
);
else
return
false
;
}
virtual
bool
operator
==
(
const
rotation_spline
&
other
)
const
{
...
...
include/curves/piecewise_curve.h
View file @
9829c732
...
...
@@ -81,25 +81,30 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
}
/**
* @brief isApprox check if other and *this are equals, given a precision treshold.
* This test is done by discretizing, it should be re-implemented in the child class to check exactly
* all the members.
* @brief isApprox check if other and *this are approximately equals.
* Only two curves of the same class can be approximately equals, for comparison between different type of curves see isEquivalent
* @param other the other curve to check
* @param order the order up to which the derivatives of the curves are checked for equality
* @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals
*/
virtual
bool
isApprox
(
const
piecewise_curve_t
&
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
()
,
const
size_t
order
=
5
)
const
{
bool
isApprox
(
const
piecewise_curve_t
&
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
())
const
{
std
::
cout
<<
"is approx in piecewise called."
<<
std
::
endl
;
if
(
num_curves
()
!=
other
.
num_curves
())
return
false
;
for
(
size_t
i
=
0
;
i
<
num_curves
()
;
++
i
)
{
if
(
!
curve_at_index
(
i
)
->
isApprox
(
*
other
.
curve_at_index
(
i
)
,
prec
,
order
))
if
(
!
curve_at_index
(
i
)
->
isApprox
(
other
.
curve_at_index
(
i
)
.
get
(),
prec
))
return
false
;
}
return
true
;
}
virtual
bool
isApprox
(
const
curve_t
*
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
())
const
{
const
piecewise_curve_t
*
other_cast
=
dynamic_cast
<
const
piecewise_curve_t
*>
(
other
);
if
(
other_cast
)
return
isApprox
(
*
other_cast
,
prec
);
else
return
false
;
}
virtual
bool
operator
==
(
const
piecewise_curve_t
&
other
)
const
{
return
isApprox
(
other
);
...
...
include/curves/polynomial.h
View file @
9829c732
...
...
@@ -255,17 +255,14 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
}
/**
* @brief isApprox check if other and *this are equals, given a precision treshold.
* This test is done by discretizing, it should be re-implemented in the child class to check exactly
* all the members.
* @brief isApprox check if other and *this are approximately equals.
* Only two curves of the same class can be approximately equals, for comparison between different type of curves see isEquivalent
* @param other the other curve to check
* @param order the order up to which the derivatives of the curves are checked for equality
* @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals
*/
virtual
bool
isApprox
(
const
polynomial_t
&
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
(),
const
size_t
order
=
5
)
const
{
//std::cout<<"is approx in polynomial called."<<std::endl;
(
void
)
order
;
// silent warning, order is not used in this class.
bool
isApprox
(
const
polynomial_t
&
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
())
const
{
std
::
cout
<<
"is approx in polynomial called."
<<
std
::
endl
;
return
T_min_
==
other
.
min
()
&&
T_max_
==
other
.
max
()
&&
dim_
==
other
.
dim
()
...
...
@@ -273,6 +270,15 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
&&
coefficients_
.
isApprox
(
other
.
coefficients_
,
prec
);
}
virtual
bool
isApprox
(
const
curve_abc_t
*
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
())
const
{
const
polynomial_t
*
other_cast
=
dynamic_cast
<
const
polynomial_t
*>
(
other
);
if
(
other_cast
)
return
isApprox
(
*
other_cast
,
prec
);
else
return
false
;
}
virtual
bool
operator
==
(
const
polynomial_t
&
other
)
const
{
return
isApprox
(
other
);
}
...
...
@@ -281,14 +287,6 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
return
!
(
*
this
==
other
);
}
virtual
bool
isApprox
(
const
curve_abc_t
&
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
(),
const
size_t
order
=
5
)
const
{
const
polynomial_t
*
other_cast
=
dynamic_cast
<
const
polynomial_t
*>
(
&
other
);
if
(
other_cast
)
return
isApprox
(
*
other_cast
);
else
return
curve_abc_t
::
isApprox
(
other
,
prec
,
order
);
}
/// \brief Evaluation of the derivative of order N of spline at time t.
...
...
include/curves/se3_curve.h
View file @
9829c732
...
...
@@ -151,23 +151,29 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric
}
/**
* @brief isApprox check if other and *this are equals, given a precision treshold.
* This test is done by discretizing, it should be re-implemented in the child class to check exactly
* all the members.
* @brief isApprox check if other and *this are approximately equals.
* Only two curves of the same class can be approximately equals, for comparison between different type of curves see isEquivalent
* @param other the other curve to check
* @param order the order up to which the derivatives of the curves are checked for equality
* @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals
*/
virtual
bool
isApprox
(
const
SE3Curve_t
&
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
(),
const
size_t
order
=
5
)
const
{
//std::cout<<"is approx in SE3 called."<<std::endl;
(
void
)
order
;
// silent warning, order is not used in this class.
bool
isApprox
(
const
SE3Curve_t
&
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
())
const
{
std
::
cout
<<
"is approx in SE3 called."
<<
std
::
endl
;
return
T_min_
==
other
.
min
()
&&
T_max_
==
other
.
max
()
&&
(
translation_curve_
==
other
.
translation_curve_
||
translation_curve_
->
isApprox
(
*
other
.
translation_curve_
,
prec
))
&&
(
rotation_curve_
==
other
.
rotation_curve_
||
rotation_curve_
->
isApprox
(
*
other
.
rotation_curve_
,
prec
));
&&
(
translation_curve_
==
other
.
translation_curve_
||
translation_curve_
->
isApprox
(
other
.
translation_curve_
.
get
()
,
prec
))
&&
(
rotation_curve_
==
other
.
rotation_curve_
||
rotation_curve_
->
isApprox
(
other
.
rotation_curve_
.
get
()
,
prec
));
}
virtual
bool
isApprox
(
const
curve_abc_t
*
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
())
const
{
const
SE3Curve_t
*
other_cast
=
dynamic_cast
<
const
SE3Curve_t
*>
(
other
);
if
(
other_cast
)
return
isApprox
(
*
other_cast
,
prec
);
else
return
false
;
}
virtual
bool
operator
==
(
const
SE3Curve_t
&
other
)
const
{
return
isApprox
(
other
);
}
...
...
@@ -176,13 +182,6 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric
return
!
(
*
this
==
other
);
}
virtual
bool
isApprox
(
const
curve_abc_t
&
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
(),
const
size_t
order
=
5
)
const
{
const
SE3Curve_t
*
other_cast
=
dynamic_cast
<
const
SE3Curve_t
*>
(
&
other
);
if
(
other_cast
)
return
isApprox
(
*
other_cast
);
else
return
curve_abc_t
::
isApprox
(
other
,
prec
,
order
);
}
/// \brief Evaluation of the derivative of order N of spline at time t.
/// \param t : the time when to evaluate the spline.
...
...
include/curves/so3_linear.h
View file @
9829c732
...
...
@@ -108,17 +108,14 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
virtual
matrix3_t
operator
()(
const
time_t
t
)
const
{
return
computeAsQuaternion
(
t
).
toRotationMatrix
();
}
/**
* @brief isApprox check if other and *this are equals, given a precision treshold.
* This test is done by discretizing, it should be re-implemented in the child class to check exactly
* all the members.
* @brief isApprox check if other and *this are approximately equals.
* Only two curves of the same class can be approximately equals, for comparison between different type of curves see isEquivalent
* @param other the other curve to check
* @param order the order up to which the derivatives of the curves are checked for equality
* @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals
*/
virtual
bool
isApprox
(
const
SO3Linear_t
&
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
(),
const
size_t
order
=
5
)
const
{
//std::cout<<"is approx in SO3 called."<<std::endl;
(
void
)
order
;
// silent warning, order is not used in this class.
bool
isApprox
(
const
SO3Linear_t
&
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
())
const
{
std
::
cout
<<
"is approx in SO3 called."
<<
std
::
endl
;
return
T_min_
==
other
.
min
()
&&
T_max_
==
other
.
max
()
&&
dim_
==
other
.
dim
()
...
...
@@ -126,6 +123,15 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
&&
end_rot_
.
toRotationMatrix
().
isApprox
(
other
.
end_rot_
.
toRotationMatrix
(),
prec
);
}
virtual
bool
isApprox
(
const
curve_abc_t
*
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
())
const
{
const
SO3Linear_t
*
other_cast
=
dynamic_cast
<
const
SO3Linear_t
*>
(
other
);
if
(
other_cast
)
return
isApprox
(
*
other_cast
,
prec
);
else
return
false
;
}
virtual
bool
operator
==
(
const
SO3Linear_t
&
other
)
const
{
return
isApprox
(
other
);
}
...
...
@@ -134,14 +140,6 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
return
!
(
*
this
==
other
);
}
virtual
bool
isApprox
(
const
curve_abc_t
&
other
,
const
Numeric
prec
=
Eigen
::
NumTraits
<
Numeric
>::
dummy_precision
(),
const
size_t
order
=
5
)
const
{
const
SO3Linear_t
*
other_cast
=
dynamic_cast
<
const
SO3Linear_t
*>
(
&
other
);
if
(
other_cast
)
return
isApprox
(
*
other_cast
);
else
return
curve_abc_t
::
isApprox
(
other
,
prec
,
order
);
}
/// \brief Evaluation of the derivative of order N of spline at time t.
/// \param t : the time when to evaluate the spline.
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment