Commit 9829c732 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

isApprox can now return true only with two curves of the same type

parent aa7b386c
...@@ -140,9 +140,15 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> { ...@@ -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; * @brief isApprox check if other and *this are approximately equals.
(void)order; * 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() bool equal = T_min_ == other.min()
&& T_max_ == other.max() && T_max_ == other.max()
&& dim_ == other.dim() && dim_ == other.dim()
...@@ -160,6 +166,14 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> { ...@@ -160,6 +166,14 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
return true; 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 { virtual bool operator==(const bezier_curve_t& other) const {
return isApprox(other); return isApprox(other);
...@@ -169,15 +183,6 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> { ...@@ -169,15 +183,6 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
return !(*this == other); 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. /// \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). /// Computes the derivative order N, \f$\frac{d^Nx(t)}{dt^N}\f$ of bezier curve of parametric equation x(t).
......
...@@ -104,17 +104,14 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> { ...@@ -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. * @brief isApprox check if other and *this are approximately equals.
* This test is done by discretizing, it should be re-implemented in the child class to check exactly * Only two curves of the same class can be approximately equals, for comparison between different type of curves see isEquivalent
* all the members.
* @param other the other curve to check * @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() * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals * @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{ 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; std::cout<<"is approx in hermite called."<<std::endl;
(void)order; // silent warning, order is not used in this class.
bool equal = T_min_ == other.min() bool equal = T_min_ == other.min()
&& T_max_ == other.max() && T_max_ == other.max()
&& dim_ == other.dim() && dim_ == other.dim()
...@@ -133,6 +130,14 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> { ...@@ -133,6 +130,14 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
return true; 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 { virtual bool operator==(const cubic_hermite_spline_t& other) const {
return isApprox(other); return isApprox(other);
} }
...@@ -141,14 +146,6 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> { ...@@ -141,14 +146,6 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
return !(*this == other); 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. /// \brief Evaluate the derivative of order N of spline at time t.
......
...@@ -59,42 +59,17 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria ...@@ -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. /// \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; 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. * @brief isApprox check if other and *this are approximately equals given a precision treshold
* This test is done by discretizing, it should be re-implemented in the child class to check exactly * Only two curves of the same class can be approximately equals,
* all the members. * for comparison between different type of curves see isEquivalent.
* @param other the other curve to check * @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() * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals * @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{ virtual bool isApprox(const curve_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const = 0;
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;
}
/*Operations*/ /*Operations*/
......
...@@ -109,6 +109,17 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> { ...@@ -109,6 +109,17 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> {
/// \brief Destructor. /// \brief Destructor.
virtual ~exact_cubic() {} 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(); } std::size_t getNumberSplines() { return this->getNumberCurves(); }
spline_t getSplineAt(std::size_t index) { spline_t getSplineAt(std::size_t index) {
......
...@@ -73,21 +73,27 @@ class rotation_spline : public curve_abc_quat_t { ...@@ -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. * @brief isApprox check if other and *this are approximately equals.
* This test is done by discretizing, it should be re-implemented in the child class to check exactly * Only two curves of the same class can be approximately equals, for comparison between different type of curves see isEquivalent
* all the members.
* @param other the other curve to check * @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() * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals * @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_ return min_ == other.min_
&& max_ == other.max_ && max_ == other.max_
&& dim_ == other.dim_ && dim_ == other.dim_
&& quat_from_.isApprox(other.quat_from_,prec) && quat_from_.isApprox(other.quat_from_,prec)
&& quat_to_.isApprox(other.quat_to_,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 { virtual bool operator==(const rotation_spline& other) const {
......
...@@ -81,25 +81,30 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv ...@@ -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. * @brief isApprox check if other and *this are approximately equals.
* This test is done by discretizing, it should be re-implemented in the child class to check exactly * Only two curves of the same class can be approximately equals, for comparison between different type of curves see isEquivalent
* all the members.
* @param other the other curve to check * @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() * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals * @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; std::cout<<"is approx in piecewise called."<<std::endl;
if(num_curves() != other.num_curves()) if(num_curves() != other.num_curves())
return false; return false;
for (size_t i = 0 ; i < num_curves() ; ++i) { 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 false;
} }
return true; 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 { virtual bool operator==(const piecewise_curve_t& other) const {
return isApprox(other); return isApprox(other);
......
...@@ -255,17 +255,14 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> { ...@@ -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. * @brief isApprox check if other and *this are approximately equals.
* This test is done by discretizing, it should be re-implemented in the child class to check exactly * Only two curves of the same class can be approximately equals, for comparison between different type of curves see isEquivalent
* all the members.
* @param other the other curve to check * @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() * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals * @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{ bool isApprox(const polynomial_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
//std::cout<<"is approx in polynomial called."<<std::endl; std::cout<<"is approx in polynomial called."<<std::endl;
(void)order; // silent warning, order is not used in this class.
return T_min_ == other.min() return T_min_ == other.min()
&& T_max_ == other.max() && T_max_ == other.max()
&& dim_ == other.dim() && dim_ == other.dim()
...@@ -273,6 +270,15 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> { ...@@ -273,6 +270,15 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
&& coefficients_.isApprox(other.coefficients_,prec); && 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 { virtual bool operator==(const polynomial_t& other) const {
return isApprox(other); return isApprox(other);
} }
...@@ -281,14 +287,6 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> { ...@@ -281,14 +287,6 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
return !(*this == other); 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. /// \brief Evaluation of the derivative of order N of spline at time t.
......
...@@ -151,23 +151,29 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric ...@@ -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. * @brief isApprox check if other and *this are approximately equals.
* This test is done by discretizing, it should be re-implemented in the child class to check exactly * Only two curves of the same class can be approximately equals, for comparison between different type of curves see isEquivalent
* all the members.
* @param other the other curve to check * @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() * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals * @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{ bool isApprox(const SE3Curve_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
//std::cout<<"is approx in SE3 called."<<std::endl; std::cout<<"is approx in SE3 called."<<std::endl;
(void)order; // silent warning, order is not used in this class.
return T_min_ == other.min() return T_min_ == other.min()
&& T_max_ == other.max() && T_max_ == other.max()
&& (translation_curve_ == other.translation_curve_ || translation_curve_->isApprox(*other.translation_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_,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 { virtual bool operator==(const SE3Curve_t& other) const {
return isApprox(other); return isApprox(other);
} }
...@@ -176,13 +182,6 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric ...@@ -176,13 +182,6 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric
return !(*this == other); 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. /// \brief Evaluation of the derivative of order N of spline at time t.
/// \param t : the time when to evaluate the spline. /// \param t : the time when to evaluate the spline.
......
...@@ -108,17 +108,14 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric, ...@@ -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(); } 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. * @brief isApprox check if other and *this are approximately equals.
* This test is done by discretizing, it should be re-implemented in the child class to check exactly * Only two curves of the same class can be approximately equals, for comparison between different type of curves see isEquivalent
* all the members.
* @param other the other curve to check * @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() * @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals * @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{ bool isApprox(const SO3Linear_t& other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
//std::cout<<"is approx in SO3 called."<<std::endl; std::cout<<"is approx in SO3 called."<<std::endl;
(void)order; // silent warning, order is not used in this class.
return T_min_ == other.min() return T_min_ == other.min()
&& T_max_ == other.max() && T_max_ == other.max()
&& dim_ == other.dim() && dim_ == other.dim()
...@@ -126,6 +123,15 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric, ...@@ -126,6 +123,15 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
&& end_rot_.toRotationMatrix().isApprox(other.end_rot_.toRotationMatrix(),prec); && 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 { virtual bool operator==(const SO3Linear_t& other) const {
return isApprox(other); return isApprox(other);
} }
...@@ -134,14 +140,6 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric, ...@@ -134,14 +140,6 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
return !(*this == other); 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. /// \brief Evaluation of the derivative of order N of spline at time t.
/// \param t : the time when to evaluate the spline. /// \param t : the time when to evaluate the spline.
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment