Skip to content

Commit

Permalink
Merge pull request #25 from bchretien/master
Browse files Browse the repository at this point in the history 
Improvements for (cubic) B-splines
  • Loading branch information
@bchretien
bchretien committed Apr 7, 2015
2 parents 19b221b + d7cdebc commit fc8c0e0
Show file tree
Hide file tree
Showing 11 changed files with 5,026 additions and 1,669 deletions.
7 changes: 5 additions & 2 deletions include/roboptim/trajectory/b-spline.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,12 +71,14 @@ namespace trajectory
/// \param dimension spline dimension: \f$n\f$
/// \param parameters vector of parameters defining control points
/// \param name function title
/// \param clamped whether the spline should be clamped
///
/// Number of control points is inferred from dimension of dimenion of
/// parameter vector.
BSpline (interval_t timeRange, size_type dimension,
const vector_t& parameters,
const std::string name = "B-Spline");
const std::string name = "B-Spline",
bool clamped = false);

/// \brief Instantiate a B-Spline with parameters and knot points.
///
Expand Down Expand Up @@ -186,7 +188,8 @@ namespace trajectory
/// \brief basisPolynomials_[i][j] = B_{i,i+j}
basisPolynomialsVector_t basisPolynomials_;

/// \brief For backward compatibility only.
/// \brief Whether the B-spline is uniform.
/// Note: used for faster interval lookup.
bool uniform_;

protected:
Expand Down
111 changes: 72 additions & 39 deletions include/roboptim/trajectory/b-spline.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ namespace trajectory
template <int N>
BSpline<N>::BSpline (interval_t tr, size_type outputSize,
const vector_t& p,
std::string name)
std::string name, bool clamped)
: Trajectory<N> (tr, outputSize, p, name),
nbp_ (p.size () / outputSize), uniform_ (true)
{
Expand All @@ -42,18 +42,40 @@ namespace trajectory
// Fill vector of regularly spaced knots
size_type m = nbp_ + order_ + 1;

//FIXME: check this
value_type delta_t =
(tr.second - tr.first)
/ static_cast<value_type> (m - order_ - 1 - order_);
/ static_cast<value_type> (nbp_ - order_);

value_type ti = tr.first - order_ * delta_t;
knots_.resize (m);
for (size_type i = 0; i < m; i++)

// Clamped B-spline.
if (clamped)
{
knots_ (i) = ti;
ti += delta_t;
// The first order_+1 knots should be equal to tr.first.
// The last one will be added in the main loop.
for (size_type i = 0; i < order_; i++) {
knots_ (i) = tr.first;
}

// Note: we do not use an accumulator to get improved numerical precision
for (size_type i = 0; i < nbp_ - order_ + 1; i++) {
knots_ (order_ + i) = tr.first + static_cast<double> (i) * delta_t;
}

// The last order_+1 knots should be equal to tr.second.
// The 1st one was added in the main loop.
for (size_type i = 0; i < order_; i++) {
knots_ (m-1-i) = tr.second;
}
}
else
{
for (size_type i = 0; i < m; i++)
{
knots_ (i) = tr.first + static_cast<value_type> (i-order_) * delta_t;
}
}

setParameters (p);
computeBasisPolynomials ();
}
Expand Down Expand Up @@ -288,53 +310,61 @@ namespace trajectory
t = detail::fixTime (t, *this);
typedef boost::numeric::converter<size_type, value_type> Double2SizeType;

// t_{order]
value_type tmin = this->timeRange ().first;
size_type i = 0;
size_type imin = order_;

// t_{m-?}
value_type tmax = this->timeRange ().second;
size_type imax = nbp_;

unsigned int count = 0;
bool found = false;
size_type i = 1;
size_type iPrev = 0;
// In the uniform case, we can access the interval directly
if (uniform_)
{
double delta_t = (this->timeRange ().second
- this->timeRange ().first)
/ (static_cast<double> (nbp_ - order_));

while (!found && iPrev != i)
i = imin + Double2SizeType::convert
(std::floor ((t - this->timeRange ().first)/delta_t));
}
else
{
value_type imin_ = static_cast<value_type> (imin);
value_type imax_ = static_cast<value_type> (imax);
value_type d =
std::floor (imin_ + (t - tmin) / (tmax - tmin) * (imax_ - imin_));
i = Double2SizeType::convert (d);
if (t < knots_ [i])
{
tmax = knots_ [i - 1];
imax = i - 1;
}
else if (t >= knots_ [i + 1])
bool found = false;

while (!found)
{
if (t < knots_ [i + 2])
i = Double2SizeType::convert
(std::floor (.5 * static_cast<double> (imin + imax) + .5));

if (t < knots_ [i])
{
imax = i - 1;
}
else if (t >= knots_ [i + 1])
{
if (t < knots_ [i + 2])
{
i = i + 1;
found = true;
}
else
{
imin = i + 1;
}
}
else
{
i = i + 1;
found = true;
}
imin = i + 1;
tmin = knots_ [i + 1];
assert (imin <= imax);
}
else
{
found = true;
}
count++;
assert (count < 10000);
iPrev = i;
}

if (i > nbp_ - 1)
i = nbp_ - 1;
if (i < order_)
i = order_;

assert (knots_ [i] <= t);
assert (t <= knots_ [i+1]);

return i;
}

Expand Down Expand Up @@ -477,7 +507,10 @@ namespace trajectory
template <int N> std::ostream&
BSpline<N>::print (std::ostream& o) const
{
using roboptim::operator <<;

o << "Order " << order_ << " B-spline:" << incindent
<< iendl << "Name: " << this->getName ()
<< iendl << "Number of parameters per spline function: " << nbp_
<< iendl << "Length: " << this->length ()
<< iendl << "Knot vector: " << knots_
Expand Down
19 changes: 10 additions & 9 deletions include/roboptim/trajectory/cubic-b-spline.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,18 +46,19 @@ namespace trajectory

typedef std::vector <value_type> knots_t;

/// \brief Instantiate a cubic B-Spline from its definition.
/// \brief Instantiate a uniform cubic B-Spline from its definition.
///
/// \param timeRange spline time range: $\f$[t_3,t_n]\f$
/// \param dimension spline dimension: \f$n\f$
/// \param parameters vector of parameters defining control points
/// \param name function title
/// \param clamped whether the spline should be clamped
///
/// The number of control points is inferred from the dimension of
/// the parameter vector.
CubicBSpline (interval_t timeRange, size_type dimension,
const vector_t& parameters,
const std::string name = "cubic B-Spline");
const std::string name = "cubic B-Spline", bool clamped = false);

/// \brief Instantiate a cubic B-Spline from its definition.
///
Expand Down Expand Up @@ -151,6 +152,11 @@ namespace trajectory
return basisPolynomials_;
}

/// \brief Find the index of the interval in which t is.
/// \param t instant considered.
/// \return index of the interval in which t is.
size_type interval (value_type t) const;

/// \brief Get the number of control points of the spline.
/// \return Number of control points of the spline.
size_type getNumberControlPoints() const
Expand All @@ -160,7 +166,7 @@ namespace trajectory

/// \brief Return the knot vector of the spline.
/// \return knot vector of the spline (const).
const std::vector <value_type>& knotVector () const
const knots_t& knotVector () const
{
return knots_;
}
Expand All @@ -187,11 +193,6 @@ namespace trajectory
void impl_derivative (gradient_ref g, StableTimePoint, size_type order)
const;

/// \brief Find the index of the interval in which t is.
/// \param t instant considered.
/// \return index of the interval in which t is.
size_type interval (value_type t) const;

/// \brief Compute the basis polynomials for the cubic B-spline.
void computeBasisPolynomials ();

Expand All @@ -214,7 +215,7 @@ namespace trajectory
polynomials3vectors_t basisPolynomials_;

/// \brief Whether the B-spline is uniform.
/// Note: for backward compatibility only.
/// Note: used for faster interval lookup.
bool uniform_;

public:
Expand Down
98 changes: 69 additions & 29 deletions src/cubic-b-spline.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ namespace trajectory
//FIXME: defined_lc_in has to be true (false untested).
CubicBSpline::CubicBSpline (interval_t tr, size_type outputSize,
const vector_t& p,
std::string name)
std::string name, bool clamped)
: Trajectory<3> (tr, outputSize, p, name),
nbp_ (p.size () / outputSize), uniform_ (true)
{
Expand All @@ -45,13 +45,39 @@ namespace trajectory

double delta_t = (tr.second - tr.first) / (static_cast<double> (m) - 7.);

double ti = tr.first - 3*delta_t;
for (size_type i = 0; i < m; i++) {
knots_.push_back (ti);
ti += delta_t;
}
// Clamped B-spline.
if (clamped)
{
// The first 4 knots should be equal to tr.first.
// The 4th one will be added in the main loop.
for (size_type i = 0; i < 3; i++) {
knots_.push_back (tr.first);
}

// Note: we do not use an accumulator to get improved numerical precision
for (size_type i = 0; i < nbp_ - 2; i++) {
knots_.push_back (tr.first + static_cast<double> (i) * delta_t);
}

// The last 4 knots should be equal to tr.second.
// The 1st one was added in the main loop.
for (size_type i = 0; i < 3; i++) {
knots_.push_back (tr.second);
}
}
else // Default case.
{
// Note: we do not use an accumulator to get improved numerical precision
for (size_type i = 0; i < m; i++) {
knots_.push_back (tr.first + static_cast<double> (i-3) * delta_t);
}
}

assert (knots_.size () == static_cast<size_t> (m));

// interval lower bound should be rigorously equal to knot 3.
knots_ [3] = tr.first;
assert (knots_ [3] == tr.first);

setParameters (p);
computeBasisPolynomials ();
}
Expand Down Expand Up @@ -182,43 +208,57 @@ namespace trajectory
CubicBSpline::interval (value_type t) const
{
t = detail::fixTime (t, *this);
typedef boost::numeric::converter<size_type, double> Double2SizeType;

size_type i = 0;
size_type imin = 3;
size_type imax = static_cast<size_type> (knots_.size () - 5);

unsigned int count = 0;
bool found = false;
size_type i = 1;
std::size_t i_ = static_cast<std::size_t> (i);
typedef boost::numeric::converter<size_type, double> Double2SizeType;

while (!found)
// In the uniform case, we can access the interval directly
if (uniform_)
{
i = Double2SizeType::convert
(std::floor (.5 * static_cast<double> (imin + imax) + .5));
i_ = static_cast<std::size_t> (i);
if (t < knots_ [i_])
{
imax = i - 1;
}
else if (t >= knots_ [i_ + 1])
{
imin = i + 1;
}
else
size_type m = nbp_ + 4;
double delta_t = (timeRange ().second - timeRange ().first) / (static_cast<double> (m) - 7.);

i = imin + Double2SizeType::convert (std::floor ((t - timeRange ().first)/delta_t));
}
else // Default case: use dichotomy
{
bool found = false;

std::size_t i_ = static_cast<std::size_t> (i);

while (!found)
{
found = true;
i = Double2SizeType::convert
(std::floor (.5 * static_cast<double> (imin + imax) + .5));
i_ = static_cast<std::size_t> (i);
if (t < knots_ [i_])
{
imax = i - 1;
}
else if (t >= knots_ [i_ + 1])
{
imin = i + 1;
}
else
{
found = true;
}
assert (imin <= imax);
}
++count;
assert (count < 10000);
}

if (i > nbp_ - 1)
i = nbp_-1;
if (i < 3)
i = 3;
ROBOPTIM_DEBUG_ONLY(i_ = static_cast<std::size_t> (i);)

ROBOPTIM_DEBUG_ONLY(std::size_t i_ = static_cast<std::size_t> (i);)
assert (knots_ [i_] <= t);
assert (t <= knots_ [i_+1]);

return i;
}

Expand Down
Loading

0 comments on commit fc8c0e0

Please sign in to comment.