Commit a9efbf46 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

Merge remote-tracking branch 'jchemin/topic/add_serialization_boost' into devel

parents 7ea3d69b b6633721
......@@ -19,7 +19,7 @@ SETUP_HPP_PROJECT()
ADD_REQUIRED_DEPENDENCY(eigen3)
SET(BOOST_COMPONENTS unit_test_framework)
SET(BOOST_COMPONENTS unit_test_framework serialization)
OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
IF(BUILD_PYTHON_INTERFACE)
......@@ -30,13 +30,18 @@ IF(BUILD_PYTHON_INTERFACE)
ADD_REQUIRED_DEPENDENCY("eigenpy")
SET(BOOST_COMPONENTS ${BOOST_COMPONENTS} python)
ADD_SUBDIRECTORY(python)
ENDIF(BUILD_PYTHON_INTERFACE)
#find_package(Boost 1.58 REQUIRED unit_test_framework system serialization)
#SET(BOOST_COMPONENTS unit_test_framework serialization)
SEARCH_FOR_BOOST()
INCLUDE_DIRECTORIES(SYSTEM ${Boost_INCLUDE_DIRS})
IF(BUILD_PYTHON_INTERFACE)
ADD_SUBDIRECTORY(python)
ENDIF(BUILD_PYTHON_INTERFACE)
ADD_SUBDIRECTORY(include/curves)
ADD_SUBDIRECTORY(tests)
......
......@@ -20,3 +20,4 @@ INSTALL(FILES
)
ADD_SUBDIRECTORY(helpers)
ADD_SUBDIRECTORY(serialization)
......@@ -21,26 +21,22 @@
#include <vector>
#include <utility>
namespace curves{
//REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels
template<typename _Matrix_Type_>
void PseudoInverse(_Matrix_Type_& pinvmat)
{
//REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels
template<typename _Matrix_Type_>
void PseudoInverse(_Matrix_Type_& pinvmat)
{
Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
_Matrix_Type_ m_sigma = svd.singularValues();
double pinvtoler= 1.e-6; // choose your tolerance widely!
_Matrix_Type_ m_sigma_inv = _Matrix_Type_::Zero(pinvmat.cols(),pinvmat.rows());
for (long i=0; i<m_sigma.rows(); ++i)
{
if (m_sigma(i) > pinvtoler)
{
m_sigma_inv(i,i)=1.0/m_sigma(i);
}
if (m_sigma(i) > pinvtoler)
{
m_sigma_inv(i,i)=1.0/m_sigma(i);
}
}
pinvmat = (svd.matrixV()*m_sigma_inv*svd.matrixU().transpose());
}
}
} // namespace curves
#endif //_SPLINEMATH
......@@ -20,55 +20,70 @@
namespace curves
{
/// \brief Computes a binomial coefficient.
/// \param n : an unsigned integer.
/// \param k : an unsigned integer.
/// \return \f$\binom{n}{k}f$
///
inline unsigned int bin(const unsigned int n, const unsigned int k)
{
if(k > n)
throw std::runtime_error("binomial coefficient higher than degree");
if(k == 0)
return 1;
if(k > n/2)
return bin(n,n-k);
/// \brief Computes a binomial coefficient .
/// \param n : an unsigned integer.
/// \param k : an unsigned integer.
/// \return \f$\binom{n}{k}f$
///
inline unsigned int bin(const unsigned int n, const unsigned int k)
{
if(k > n) throw std::runtime_error("binomial coefficient higher than degree");
if(k == 0) return 1;
if(k > n/2) return bin(n,n-k);
return n * bin(n-1,k-1) / k;
}
}
/// \class Bernstein.
/// \brief Computes a Bernstein polynome.
///
template <typename Numeric = double>
struct Bern{
Bern(const unsigned int m, const unsigned int i)
:m_minus_i(m - i)
,i_(i)
,bin_m_i_(bin(m,i)) {}
/// \class Bernstein.
/// \brief Computes a Bernstein polynome.
///
template <typename Numeric = double>
struct Bern {
Bern(){}
Bern(const unsigned int m, const unsigned int i)
:m_minus_i(m - i),
i_(i),
bin_m_i_(bin(m,i))
{}
~Bern(){}
~Bern(){}
Numeric operator()(const Numeric u) const
{
assert(u >= 0. && u <= 1.);
return bin_m_i_*(pow(u, i_)) *pow((1-u),m_minus_i);
}
Numeric operator()(const Numeric u) const
{
assert(u >= 0. && u <= 1.);
return bin_m_i_*(pow(u, i_)) *pow((1-u),m_minus_i);
}
Numeric m_minus_i;
Numeric i_;
Numeric bin_m_i_;
};
/* Attributes */
Numeric m_minus_i;
Numeric i_;
Numeric bin_m_i_;
/* Attributes */
/// \brief Computes all Bernstein polynomes for a certain degree.
///
template <typename Numeric>
std::vector<Bern<Numeric> > makeBernstein(const unsigned int n)
{
// Serialization of the class
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive& ar, const unsigned int version){
if (version) {
// Do something depending on version ?
}
ar & boost::serialization::make_nvp("m_minus_i", m_minus_i);
ar & boost::serialization::make_nvp("i", i_);
ar & boost::serialization::make_nvp("bin_m_i", bin_m_i_);
}
}; // End struct Bern
/// \brief Computes all Bernstein polynomes for a certain degree.
///
template <typename Numeric>
std::vector<Bern<Numeric> > makeBernstein(const unsigned int n)
{
std::vector<Bern<Numeric> > res;
for(unsigned int i = 0; i<= n; ++i)
res.push_back(Bern<Numeric>(n, i));
{
res.push_back(Bern<Numeric>(n, i));
}
return res;
}
}
} // namespace curves
#endif //_CLASS_BERNSTEIN
......@@ -23,15 +23,15 @@
namespace curves
{
/// \class BezierCurve.
/// \brief Represents a Bezier curve of arbitrary dimension and order.
/// For degree lesser than 4, the evaluation is analitycal. Otherwise
/// the bernstein polynoms are used to evaluate the spline at a given location.
///
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
, typename Point= Eigen::Matrix<Numeric, Dim, 1> >
struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
/// \class BezierCurve.
/// \brief Represents a Bezier curve of arbitrary dimension and order.
/// For degree lesser than 4, the evaluation is analitycal. Otherwise
/// the bernstein polynoms are used to evaluate the spline at a given location.
///
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point>
{
typedef Point point_t;
typedef Time time_t;
typedef Numeric num_t;
......@@ -40,125 +40,139 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
typedef typename t_point_t::const_iterator cit_point_t;
typedef bezier_curve<Time, Numeric, Dim, Safe, Point > bezier_curve_t;
/* Constructors - destructors */
/* Constructors - destructors */
public:
/// \brief Constructor.
/// Given the first and last point of a control points set, create the bezier curve.
/// \param PointsBegin : an iterator pointing to the first element of a control point container.
/// \param PointsEnd : an iterator pointing to the last element of a control point container.
/// \param T : upper bound of time which is between \f$[0;T]\f$ (default \f$[0;1]\f$).
/// \param mult_T : ... (default value is 1.0).
///
template<typename In>
bezier_curve(In PointsBegin, In PointsEnd, const time_t T_min=0., const time_t T_max=1., const time_t mult_T=1.)
: T_min_(T_min)
, T_max_(T_max)
, mult_T_(mult_T)
, size_(std::distance(PointsBegin, PointsEnd))
, degree_(size_-1)
, bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_))
{
/// \brief Empty constructor. Curve obtained this way can not perform other class functions.
///
bezier_curve()
: T_min_(0), T_max_(0)
{}
/// \brief Constructor.
/// Given the first and last point of a control points set, create the bezier curve.
/// \param PointsBegin : an iterator pointing to the first element of a control point container.
/// \param PointsEnd : an iterator pointing to the last element of a control point container.
/// \param T : upper bound of time which is between \f$[0;T]\f$ (default \f$[0;1]\f$).
/// \param mult_T : ... (default value is 1.0).
///
template<typename In>
bezier_curve(In PointsBegin, In PointsEnd, const time_t T_min=0., const time_t T_max=1., const time_t mult_T=1.)
: T_min_(T_min),
T_max_(T_max),
mult_T_(mult_T),
size_(std::distance(PointsBegin, PointsEnd)),
degree_(size_-1),
bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_))
{
assert(bernstein_.size() == size_);
In it(PointsBegin);
if(Safe && (size_<1 || T_max_ <= T_min_))
{
throw std::invalid_argument("can't create bezier min bound is higher than max bound"); // TODO
throw std::invalid_argument("can't create bezier min bound is higher than max bound"); // TODO
}
for(; it != PointsEnd; ++it)
{
control_points_.push_back(*it);
control_points_.push_back(*it);
}
}
/// \brief Constructor
/// This constructor will add 4 points (2 after the first one, 2 before the last one)
/// to ensure that velocity and acceleration constraints are respected.
/// \param PointsBegin : an iterator pointing to the first element of a control point container.
/// \param PointsEnd : an iterator pointing to the last element of a control point container.
/// \param constraints : constraints applying on start / end velocities and acceleration.
///
template<typename In>
bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints,
const time_t T_min=0., const time_t T_max=1., const time_t mult_T=1.)
: T_min_(T_min)
, T_max_(T_max)
, mult_T_(mult_T)
, size_(std::distance(PointsBegin, PointsEnd)+4)
, degree_(size_-1)
, bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_))
{
}
/// \brief Constructor
/// This constructor will add 4 points (2 after the first one, 2 before the last one)
/// to ensure that velocity and acceleration constraints are respected.
/// \param PointsBegin : an iterator pointing to the first element of a control point container.
/// \param PointsEnd : an iterator pointing to the last element of a control point container.
/// \param constraints : constraints applying on start / end velocities and acceleration.
///
template<typename In>
bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints,
const time_t T_min=0., const time_t T_max=1., const time_t mult_T=1.)
: T_min_(T_min),
T_max_(T_max),
mult_T_(mult_T),
size_(std::distance(PointsBegin, PointsEnd)+4),
degree_(size_-1),
bernstein_(curves::makeBernstein<num_t>((unsigned int)degree_))
{
if(Safe && (size_<1 || T_max_ <= T_min_))
{
throw std::invalid_argument("can't create bezier min bound is higher than max bound");
throw std::invalid_argument("can't create bezier min bound is higher than max bound");
}
t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints);
for(cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit)
{
control_points_.push_back(*cit);
control_points_.push_back(*cit);
}
}
///\brief Destructor
~bezier_curve()
{
}
bezier_curve(const bezier_curve& other)
: T_min_(other.T_min_), T_max_(other.T_max_),
mult_T_(other.mult_T_), size_(other.size_),
degree_(other.degree_), bernstein_(other.bernstein_),
control_points_(other.control_points_)
{}
///\brief Destructor
~bezier_curve()
{
// NOTHING
}
private:
// bezier_curve(const bezier_curve&);
// bezier_curve& operator=(const bezier_curve&);
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the bezier curve at time t.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
virtual point_t operator()(const time_t t) const
{
}
/*Operations*/
/// \brief Evaluation of the bezier curve at time t.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
virtual point_t operator()(const time_t t) const
{
check_if_not_empty();
if(Safe &! (T_min_ <= t && t <= T_max_))
{
throw std::invalid_argument("can't evaluate bezier curve, time t is out of range"); // TODO
throw std::invalid_argument("can't evaluate bezier curve, time t is out of range"); // TODO
}
if (size_ == 1)
{
return mult_T_*control_points_[0];
}else
}
else
{
return evalHorner(t);
}
}
/// \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).
/// \param order : order of derivative.
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
bezier_curve_t compute_derivate(const std::size_t order) const
{
}
/// \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).
/// \param order : order of derivative.
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
bezier_curve_t compute_derivate(const std::size_t order) const
{
check_if_not_empty();
if(order == 0)
{
return *this;
return *this;
}
t_point_t derived_wp;
for(typename t_point_t::const_iterator pit = control_points_.begin(); pit != control_points_.end()-1; ++pit)
derived_wp.push_back((num_t)degree_ * (*(pit+1) - (*pit)));
{
derived_wp.push_back((num_t)degree_ * (*(pit+1) - (*pit)));
}
if(derived_wp.empty())
derived_wp.push_back(point_t::Zero(Dim));
{
derived_wp.push_back(point_t::Zero(Dim));
}
bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),T_min_, T_max_, mult_T_ * (1./(T_max_-T_min_)) );
return deriv.compute_derivate(order-1);
}
/// \brief Compute the primitive of the curve at order N.
/// Computes the primitive at order N of bezier curve of parametric equation \f$x(t)\f$. <br>
/// At order \f$N=1\f$, the primitve \f$X(t)\f$ of \f$x(t)\f$ is such as \f$\frac{dX(t)}{dt} = x(t)\f$.
/// \param order : order of the primitive.
/// \return primitive at order N of x(t).
bezier_curve_t compute_primitive(const std::size_t order) const
{
}
/// \brief Compute the primitive of the curve at order N.
/// Computes the primitive at order N of bezier curve of parametric equation \f$x(t)\f$. <br>
/// At order \f$N=1\f$, the primitve \f$X(t)\f$ of \f$x(t)\f$ is such as \f$\frac{dX(t)}{dt} = x(t)\f$.
/// \param order : order of the primitive.
/// \return primitive at order N of x(t).
bezier_curve_t compute_primitive(const std::size_t order) const
{
check_if_not_empty();
if(order == 0)
{
return *this;
return *this;
}
num_t new_degree = (num_t)(degree_+1);
t_point_t n_wp;
......@@ -168,61 +182,61 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
n_wp.push_back(current_sum);
for(typename t_point_t::const_iterator pit = control_points_.begin(); pit != control_points_.end(); ++pit)
{
current_sum += *pit;
n_wp.push_back(current_sum / new_degree);
current_sum += *pit;
n_wp.push_back(current_sum / new_degree);
}
bezier_curve_t integ(n_wp.begin(), n_wp.end(),T_min_, T_max_, mult_T_*(T_max_-T_min_));
return integ.compute_primitive(order-1);
}
/// \brief Evaluate the derivative order N of curve at time t.
/// If derivative is to be evaluated several times, it is
/// rather recommended to compute derived curve using compute_derivate.
/// \param order : order of derivative.
/// \param t : time when to evaluate the curve.
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derived curve of order N at time t.
///
virtual point_t derivate(const time_t t, const std::size_t order) const
{
}
/// \brief Evaluate the derivative order N of curve at time t.
/// If derivative is to be evaluated several times, it is
/// rather recommended to compute derived curve using compute_derivate.
/// \param order : order of derivative.
/// \param t : time when to evaluate the curve.
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derived curve of order N at time t.
///
virtual point_t derivate(const time_t t, const std::size_t order) const
{
bezier_curve_t deriv = compute_derivate(order);
return deriv(t);
}
/// \brief Evaluate all Bernstein polynomes for a certain degree.
/// A bezier curve with N control points is represented by : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$
/// with \f$ B_i^N(t) = \binom{N}{i}t^i (1-t)^{N-i} \f$.<br/>
/// Warning: the horner scheme is about 100 times faster than this method.<br>
/// This method will probably be removed in the future as the computation of bernstein polynomial is very costly.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
///
point_t evalBernstein(const Numeric t) const
{
}
/// \brief Evaluate all Bernstein polynomes for a certain degree.
/// A bezier curve with N control points is represented by : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$
/// with \f$ B_i^N(t) = \binom{N}{i}t^i (1-t)^{N-i} \f$.<br/>
/// Warning: the horner scheme is about 100 times faster than this method.<br>
/// This method will probably be removed in the future as the computation of bernstein polynomial is very costly.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
///
point_t evalBernstein(const Numeric t) const
{
const Numeric u = (t-T_min_)/(T_max_-T_min_);
point_t res = point_t::Zero(Dim);
typename t_point_t::const_iterator control_points_it = control_points_.begin();
for(typename std::vector<Bern<Numeric> >::const_iterator cit = bernstein_.begin();
cit !=bernstein_.end(); ++cit, ++control_points_it)
cit !=bernstein_.end(); ++cit, ++control_points_it)
{
res += cit->operator()(u) * (*control_points_it);
res += cit->operator()(u) * (*control_points_it);
}
return res*mult_T_;
}
/// \brief Evaluate all Bernstein polynomes for a certain degree using Horner's scheme.
/// A bezier curve with N control points is expressed as : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$.<br>
/// To evaluate the position on curve at time t,we can apply the Horner's scheme : <br>
/// \f$ x(t) = (1-t)^N(\sum_{i=0}^{N} \binom{N}{i} \frac{1-t}{t}^i P_i) \f$.<br>
/// Horner's scheme : for a polynom of degree N expressed by : <br>
/// \f$x(t) = a_0 + a_1t + a_2t^2 + ... + a_nt^n\f$
/// where \f$number of additions = N\f$ / f$number of multiplication = N!\f$<br>
/// Using Horner's method, the polynom is transformed into : <br>
/// \f$x(t) = a_0 + t(a_1 + t(a_2+t(...))\f$ with N additions and multiplications.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
///
point_t evalHorner(const Numeric t) const
{
}
/// \brief Evaluate all Bernstein polynomes for a certain degree using Horner's scheme.
/// A bezier curve with N control points is expressed as : \f$x(t) = \sum_{i=0}^{N} B_i^N(t) P_i\f$.<br>
/// To evaluate the position on curve at time t,we can apply the Horner's scheme : <br>
/// \f$ x(t) = (1-t)^N(\sum_{i=0}^{N} \binom{N}{i} \frac{1-t}{t}^i P_i) \f$.<br>
/// Horner's scheme : for a polynom of degree N expressed by : <br>
/// \f$x(t) = a_0 + a_1t + a_2t^2 + ... + a_nt^n\f$
/// where \f$number of additions = N\f$ / f$number of multiplication = N!\f$<br>
/// Using Horner's method, the polynom is transformed into : <br>
/// \f$x(t) = a_0 + t(a_1 + t(a_2+t(...))\f$ with N additions and multiplications.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
///
point_t evalHorner(const Numeric t) const
{
const Numeric u = (t-T_min_)/(T_max_-T_min_);
typename t_point_t::const_iterator control_points_it = control_points_.begin();
Numeric u_op, bc, tn;
......@@ -232,75 +246,80 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
point_t tmp =(*control_points_it)*u_op; ++control_points_it;
for(unsigned int i=1; i<degree_; i++, ++control_points_it)
{
tn = tn*u;
bc = bc*((num_t)(degree_-i+1))/i;
tmp = (tmp + tn*bc*(*control_points_it))*u_op;
tn = tn*u;
bc = bc*((num_t)(degree_-i+1))/i;
tmp = (tmp + tn*bc*(*control_points_it))*u_op;
}
return (tmp + tn*u*(*control_points_it))*mult_T_;
}
const t_point_t& waypoints() const {return control_points_;}
/// \brief Evaluate the curve value at time t using deCasteljau algorithm.
/// The algorithm will compute the \f$N-1\f$ centroids of parameters \f${t,1-t}\f$ of consecutive \f$N\f$ control points
/// of bezier curve, and perform it iteratively until getting one point in the list which will be the evaluation of bezier
/// curve at time \f$t\f$.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
///
point_t evalDeCasteljau(const Numeric t) const {
}
const t_point_t& waypoints() const {return control_points_;}
/// \brief Evaluate the curve value at time t using deCasteljau algorithm.
/// The algorithm will compute the \f$N-1\f$ centroids of parameters \f${t,1-t}\f$ of consecutive \f$N\f$ control points
/// of bezier curve, and perform it iteratively until getting one point in the list which will be the evaluation of bezier
/// curve at time \f$t\f$.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
///
point_t evalDeCasteljau(const Numeric t) const
{
// normalize time :
const Numeric u = (t-T_min_)/(T_max_-T_min_);
t_point_t pts = deCasteljauReduction(waypoints(),u);
while(pts.size() > 1)
{
pts = deCasteljauReduction(pts,u);
pts = deCasteljauReduction(pts,u);
}
return pts[0]*mult_T_;
}
}
t_point_t deCasteljauReduction(const Numeric t) const{
t_point_t deCasteljauReduction(const Numeric t) const
{
const Numeric u = (t-T_min_)/(T_max_-T_min_);
return deCasteljauReduction(waypoints(),u);
}
/// \brief Compute de Casteljau's reduction of the given list of points at time t.
/// For the list \f$pts\f$ of N points, compute a new list of points of size N-1 :<br>
/// \f$<br>( pts[0]*(1-t)+pts[1], pts[1]*(1-t)+pts[2], ..., pts[0]*(N-2)+pts[N-1] )\f$<br>
/// with t the time when to evaluate bezier curve.<br>\ The new list contains centroid of
/// parameters \f${t,1-t}\f$ of consecutive points in the list.
/// \param pts : list of points.
/// \param u : NORMALIZED time when to evaluate the curve.
/// \return reduced list of point (size of pts - 1).
///
t_point_t deCasteljauReduction(const t_point_t& pts, const Numeric u) const{
}
/// \brief Compute de Casteljau's reduction of the given list of points at time t.