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> {
}
}
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).
......
......@@ -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.
......
......@@ -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*/
......
......@@ -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) {
......
......@@ -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 {
......
......@@ -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);
......
......@@ -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.
......
......@@ -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.
......
......@@ -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.
......
Markdown is supported
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