Skip to content

Commit c67c201

Browse files
jorisvjorisv
committed
spline: Add deBoorFullBasis function
1 parent dd672f7 commit c67c201

2 files changed

Lines changed: 251 additions & 7 deletions

File tree

‎include/pinocchio/src/multibody/joint/spline-utils.hxx‎

Lines changed: 125 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -114,8 +114,8 @@ namespace pinocchio
114114
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> & basis)
115115
{
116116
assert(degree >= 0);
117-
assert(basis.rows() >= static_cast<int>(degree) + 1);
118-
assert(basis.cols() >= static_cast<int>(degree) + 1);
117+
assert(basis.rows() >= degree + 1);
118+
assert(basis.cols() >= degree + 1);
119119
assert(knots.size() > degree + 1);
120120
assert(degree <= root_basis);
121121
assert(root_basis < knots.size() - 1 - degree);
@@ -191,6 +191,129 @@ namespace pinocchio
191191
}
192192
}
193193

194+
/** Return num / den if den != 0 and 0 in the other case.
195+
* This function help support Casadi scalar type that doesn't support
196+
* comparison.
197+
*/
198+
template<typename Scalar>
199+
Scalar safeAlpha(Scalar num, Scalar den)
200+
{
201+
// clang-format off
202+
// if(den > dummy_precision)
203+
// return (num / den)
204+
// else
205+
// return 0
206+
// clang-format on
207+
return if_then_else(
208+
GT, den, Eigen::NumTraits<Scalar>::dummy_precision(), (num / den), Scalar(0));
209+
}
210+
211+
/** De Boor algorithm modification to compute all basis in the spline valid span.
212+
* This function will compute a lot of zeros and it's implemented for Casadi scalar support.
213+
* Use deBoorBasis instead.
214+
* \param degree Curve degree.
215+
* \param knots Knot vector of size m (at least of size \p degree + 1)
216+
* \param q Value to evaluate.
217+
* \param basis of size (degree + 1, m - degree - 1).
218+
* Each element i, j of this array will hold a basis function N_{i,j} where i and j
219+
* are respectively the basis function index and degree.
220+
*/
221+
template<typename Scalar>
222+
void deBoorFullBasis(
223+
int degree,
224+
const Eigen::Matrix<Scalar, Eigen::Dynamic, 1> & knots,
225+
Scalar q,
226+
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> & basis)
227+
{
228+
assert(degree >= 0);
229+
assert(basis.rows() >= degree + 1);
230+
// Number of max degree basis functions
231+
assert(basis.cols() >= knots.size() - degree - 1);
232+
assert(knots.size() > degree + 1);
233+
basis.setZero();
234+
const int first_degree0_basis = degree;
235+
const int last_degree0_basis = knots.size() - degree - 2;
236+
const int nb_degree0_basis = last_degree0_basis + 1 - first_degree0_basis;
237+
238+
for (int i = 0; i < nb_degree0_basis; ++i)
239+
{
240+
int current_basis = first_degree0_basis + i;
241+
// clang-format off
242+
// if(knots[i] <= x && x < knots[i + 1])
243+
// return 1;
244+
// else
245+
// return 0;
246+
// clang-format on
247+
Scalar is_in_standard_range = if_then_else(
248+
LE, knots[current_basis], q,
249+
if_then_else(LT, q, knots[current_basis + 1], Scalar(1), Scalar(0)), Scalar(0));
250+
251+
// clang-format off
252+
// if(x == knots.back() && x == knots[i + 1])
253+
// return 1;
254+
// else
255+
// return 0;
256+
// clang-format on
257+
Scalar is_at_final_range = if_then_else(
258+
EQ, q, knots[knots.size() - degree - 1],
259+
if_then_else(EQ, q, knots[current_basis + 1], Scalar(1), Scalar(0)), Scalar(0));
260+
261+
basis(0, i) = is_in_standard_range + is_at_final_range;
262+
}
263+
264+
// Compute left most and right most basis functions (first pass).
265+
for (int previous_degree = 0; previous_degree < degree; ++previous_degree)
266+
{
267+
const int current_degree = previous_degree + 1;
268+
const int basis_numbers = nb_degree0_basis + current_degree;
269+
const int left_most_basis = first_degree0_basis - current_degree;
270+
const int left_most_basis_start_knot = left_most_basis + 1;
271+
const int left_most_basis_end_knot = left_most_basis_start_knot + current_degree;
272+
const Scalar left_most_basis_alpha_num = knots[left_most_basis_end_knot] - q;
273+
const Scalar left_most_basis_alpha_den =
274+
knots[left_most_basis_end_knot] - knots[left_most_basis_start_knot];
275+
basis(current_degree, 0) = safeAlpha(left_most_basis_alpha_num, left_most_basis_alpha_den)
276+
* basis(previous_degree, 0);
277+
278+
const int right_most_basis = last_degree0_basis;
279+
const int right_most_basis_start_knot = right_most_basis;
280+
const int right_most_basis_end_knot = right_most_basis_start_knot + current_degree;
281+
const Scalar right_most_basis_alpha_num = q - knots[right_most_basis_start_knot];
282+
const Scalar right_most_basis_alpha_den =
283+
knots[right_most_basis_end_knot] - knots[right_most_basis_start_knot];
284+
basis(current_degree, basis_numbers - 1) =
285+
safeAlpha(right_most_basis_alpha_num, right_most_basis_alpha_den)
286+
* basis(previous_degree, basis_numbers - 2);
287+
}
288+
289+
// Compute central basis functions (second pass).
290+
for (int previous_degree = 0; previous_degree < degree; ++previous_degree)
291+
{
292+
const int current_degree = previous_degree + 1;
293+
const int left_most_basis = first_degree0_basis - current_degree;
294+
const int basis_numbers = nb_degree0_basis + current_degree;
295+
for (int i = 1; i < basis_numbers - 1; ++i)
296+
{
297+
const int current_basis = left_most_basis + i;
298+
const int left_side_start_knot = current_basis;
299+
const int left_side_end_knot = current_basis + current_degree;
300+
const Scalar left_side_alpha_num = (q - knots[left_side_start_knot]);
301+
const Scalar left_side_alpha_den =
302+
(knots[left_side_end_knot] - knots[left_side_start_knot]);
303+
304+
const int right_side_start_knot = left_side_start_knot + 1;
305+
const int right_side_end_knot = left_side_end_knot + 1;
306+
const Scalar right_side_alpha_num = (knots[right_side_end_knot] - q);
307+
const Scalar right_side_alpha_den =
308+
(knots[right_side_end_knot] - knots[right_side_start_knot]);
309+
310+
basis(current_degree, i) =
311+
safeAlpha(left_side_alpha_num, left_side_alpha_den) * basis(previous_degree, i - 1)
312+
+ safeAlpha(right_side_alpha_num, right_side_alpha_den) * basis(previous_degree, i);
313+
}
314+
}
315+
}
316+
194317
/** Return basis function value N_{index,degree} from basis matrix computed by \p deBoorBasis.
195318
* \param root_basis Argument provided to \p deBoorBasis function.
196319
* \param basis Basis matrix computed by \p deBoorBasis.

‎unittest/joint-spline.cpp‎

Lines changed: 126 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -452,7 +452,7 @@ BOOST_AUTO_TEST_CASE(deBoorBasis_degree0)
452452
const int degree = 0;
453453
Eigen::VectorXd knots(3);
454454
knots << 0., 2., 3.;
455-
Eigen::MatrixXd basis(1, 1);
455+
Eigen::MatrixXd basis(Eigen::MatrixXd::Zero(1, 1));
456456

457457
deBoorBasis(degree, knots, 0, 0., basis);
458458
BOOST_CHECK_SMALL(basis(0, 0) - 1., 1e-8);
@@ -476,7 +476,7 @@ BOOST_AUTO_TEST_CASE(deBoorBasis_degree1)
476476
const int degree = 1;
477477
Eigen::VectorXd knots(5);
478478
knots << 0., 2., 3., 5., 5.5;
479-
Eigen::MatrixXd basis(2, 2);
479+
Eigen::MatrixXd basis(Eigen::MatrixXd::Zero(2, 2));
480480

481481
// Evaluate between 2 and 3
482482
deBoorBasis(degree, knots, 1, 2., basis);
@@ -510,7 +510,7 @@ BOOST_AUTO_TEST_CASE(deBoorBasis_degree3)
510510
const int degree = 3;
511511
Eigen::VectorXd knots(9);
512512
knots << 0., 2., 3., 5., 5.5, 8., 8.5, 10., 11.5;
513-
Eigen::MatrixXd basis(4, 4);
513+
Eigen::MatrixXd basis(Eigen::MatrixXd::Zero(4, 4));
514514

515515
deBoorBasis(degree, knots, 3, 5., basis);
516516
BOOST_CHECK_SMALL(
@@ -583,6 +583,127 @@ BOOST_AUTO_TEST_CASE(deBoorBasis_degree3)
583583
getAbsoluteBasis(4, basis, 4, degree) - bsplineBasis(4, degree, 8., knots), 1e-8);
584584
}
585585

586+
// Test deBoorBasisFull with a knot vector only defined
587+
// on one span.
588+
// This should give the same result than deBoorBasis function.
589+
BOOST_AUTO_TEST_CASE(deBoorBasisFull_degree2_minimal_knot)
590+
{
591+
using pinocchio::internal::deBoorBasis;
592+
using pinocchio::internal::deBoorFullBasis;
593+
594+
const int degree = 2;
595+
Eigen::VectorXd knots(6);
596+
// Defined between 3 and 5
597+
knots << 0., 2., 3., 5., 5.5, 8.3;
598+
Eigen::MatrixXd basis_ref(Eigen::MatrixXd::Zero(3, 3));
599+
Eigen::MatrixXd basis(3, 3);
600+
601+
deBoorBasis(degree, knots, 2, 3., basis_ref);
602+
deBoorFullBasis(degree, knots, 3., basis);
603+
BOOST_CHECK(basis_ref.isApprox(basis));
604+
605+
deBoorBasis(degree, knots, 2, 4., basis_ref);
606+
deBoorFullBasis(degree, knots, 4., basis);
607+
BOOST_CHECK(basis_ref.isApprox(basis));
608+
609+
deBoorBasis(degree, knots, 2, 5., basis_ref);
610+
deBoorFullBasis(degree, knots, 5., basis);
611+
BOOST_CHECK(basis_ref.isApprox(basis));
612+
}
613+
614+
// Test deBoorBasisFull with a knot vector allowing to compute
615+
// more basis function than deBoorBasis.
616+
// We compare it to deBoorBasis called on the right span.
617+
BOOST_AUTO_TEST_CASE(deBoorBasisFull_degree3_nominal_knot)
618+
{
619+
using pinocchio::internal::deBoorBasis;
620+
using pinocchio::internal::deBoorFullBasis;
621+
622+
const int degree = 2;
623+
Eigen::VectorXd knots(8);
624+
// Defined between 3 and 8.3
625+
knots << 0., 2., 3., 5., 5.5, 8.3, 9., 11.3;
626+
Eigen::MatrixXd basis_ref(Eigen::MatrixXd::Zero(3, 3));
627+
Eigen::MatrixXd basis(3, 5);
628+
629+
deBoorFullBasis(degree, knots, 3., basis);
630+
deBoorBasis(degree, knots, 2, 3., basis_ref);
631+
BOOST_CHECK(basis_ref.isApprox(basis.block<3, 3>(0, 0)));
632+
BOOST_CHECK((basis.block<3, 2>(0, 3).isZero()));
633+
634+
deBoorFullBasis(degree, knots, 4., basis);
635+
deBoorBasis(degree, knots, 2, 4., basis_ref);
636+
BOOST_CHECK(basis_ref.isApprox(basis.block<3, 3>(0, 0)));
637+
BOOST_CHECK((basis.block<3, 2>(0, 3).isZero()));
638+
639+
deBoorFullBasis(degree, knots, 5., basis);
640+
deBoorBasis(degree, knots, 3, 5., basis_ref);
641+
BOOST_CHECK(basis_ref.isApprox(basis.block<3, 3>(0, 1)));
642+
BOOST_CHECK((basis.col(0).isZero()));
643+
BOOST_CHECK((basis.col(4).isZero()));
644+
645+
deBoorFullBasis(degree, knots, 5.3, basis);
646+
deBoorBasis(degree, knots, 3, 5.3, basis_ref);
647+
BOOST_CHECK(basis_ref.isApprox(basis.block<3, 3>(0, 1)));
648+
BOOST_CHECK((basis.col(0).isZero()));
649+
BOOST_CHECK((basis.col(4).isZero()));
650+
651+
deBoorFullBasis(degree, knots, 5.5, basis);
652+
deBoorBasis(degree, knots, 4, 5.5, basis_ref);
653+
BOOST_CHECK(basis_ref.isApprox(basis.block<3, 3>(0, 2)));
654+
BOOST_CHECK((basis.block<3, 2>(0, 0).isZero()));
655+
656+
deBoorFullBasis(degree, knots, 8., basis);
657+
deBoorBasis(degree, knots, 4, 8., basis_ref);
658+
BOOST_CHECK(basis_ref.isApprox(basis.block<3, 3>(0, 2)));
659+
BOOST_CHECK((basis.block<3, 2>(0, 0).isZero()));
660+
661+
deBoorFullBasis(degree, knots, 8.3, basis);
662+
deBoorBasis(degree, knots, 4, 8.3, basis_ref);
663+
BOOST_CHECK(basis_ref.isApprox(basis.block<3, 3>(0, 2)));
664+
BOOST_CHECK((basis.block<3, 2>(0, 0).isZero()));
665+
}
666+
667+
// Test deBoorBasisFull with a knot vector with element of multiplicity 3.
668+
// This should create division by 0 issue managed by the algorithm.
669+
BOOST_AUTO_TEST_CASE(deBoorBasisFull_degree3_multiplicity_knot)
670+
{
671+
using pinocchio::internal::deBoorBasis;
672+
using pinocchio::internal::deBoorFullBasis;
673+
674+
const int degree = 2;
675+
Eigen::VectorXd knots(9);
676+
// Defined between 0 and 2
677+
knots << 0., 0., 0., 1., 1., 1., 2., 2., 2.;
678+
Eigen::MatrixXd basis_ref(Eigen::MatrixXd::Zero(3, 3));
679+
Eigen::MatrixXd basis(3, 6);
680+
681+
deBoorFullBasis(degree, knots, 0., basis);
682+
deBoorBasis(degree, knots, 2, 0., basis_ref);
683+
BOOST_CHECK(basis_ref.isApprox(basis.block<3, 3>(0, 0)));
684+
BOOST_CHECK((basis.block<3, 3>(0, 3).isZero()));
685+
686+
deBoorFullBasis(degree, knots, 0.5, basis);
687+
deBoorBasis(degree, knots, 2, 0.5, basis_ref);
688+
BOOST_CHECK(basis_ref.isApprox(basis.block<3, 3>(0, 0)));
689+
BOOST_CHECK((basis.block<3, 3>(0, 3).isZero()));
690+
691+
deBoorFullBasis(degree, knots, 1., basis);
692+
deBoorBasis(degree, knots, 5, 1., basis_ref);
693+
BOOST_CHECK(basis_ref.isApprox(basis.block<3, 3>(0, 3)));
694+
BOOST_CHECK((basis.block<3, 3>(0, 0).isZero()));
695+
696+
deBoorFullBasis(degree, knots, 1.5, basis);
697+
deBoorBasis(degree, knots, 5, 1.5, basis_ref);
698+
BOOST_CHECK(basis_ref.isApprox(basis.block<3, 3>(0, 3)));
699+
BOOST_CHECK((basis.block<3, 3>(0, 0).isZero()));
700+
701+
deBoorFullBasis(degree, knots, 2., basis);
702+
deBoorBasis(degree, knots, 5, 2., basis_ref);
703+
BOOST_CHECK(basis_ref.isApprox(basis.block<3, 3>(0, 3)));
704+
BOOST_CHECK((basis.block<3, 3>(0, 0).isZero()));
705+
}
706+
586707
// Test cumulativeBasisDerivative against bsplineBasisDerivative
587708
BOOST_AUTO_TEST_CASE(cumulativeBasisDerivative)
588709
{
@@ -592,7 +713,7 @@ BOOST_AUTO_TEST_CASE(cumulativeBasisDerivative)
592713
const int degree = 3;
593714
Eigen::VectorXd knots(9);
594715
knots << 0., 2., 3., 5., 5.5, 8., 8.5, 10., 11.5;
595-
Eigen::MatrixXd basis(4, 4);
716+
Eigen::MatrixXd basis(Eigen::MatrixXd::Zero(4, 4));
596717

597718
auto computeDerivative = [knots](int start, double q) {
598719
Eigen::VectorXd res(4);
@@ -673,7 +794,7 @@ BOOST_AUTO_TEST_CASE(cumulativeBasisDerivative2)
673794
const int degree = 3;
674795
Eigen::VectorXd knots(9);
675796
knots << 0., 2., 3., 5., 5.5, 8., 8.5, 10., 11.5;
676-
Eigen::MatrixXd basis(4, 4);
797+
Eigen::MatrixXd basis(Eigen::MatrixXd::Zero(4, 4));
677798

678799
auto computeDerivative2 = [knots](int start, double q) {
679800
Eigen::VectorXd res(4);

0 commit comments

Comments
 (0)