12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

make Chebyshev nodes over N+1 points instead of N

release/4.3a0
Varun Agrawal 2023-10-23 18:14:18 -04:00
parent 9ee652c04a
commit fe7d877352
4 changed files with 111 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
gtsam/basis/Chebyshev2.cpp
View File

@ -22,13 +22,13 @@ namespace gtsam {
Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) {
// Allocate space for weights // Allocate space for weights
Weights weights(N); Weights weights(N + 1);
// We start by getting distances from x to all Chebyshev points // We start by getting distances from x to all Chebyshev points
// as well as getting smallest distance // as well as getting the smallest distance
Weights distances(N); Weights distances(N + 1);
for (size_t j = 0; j < N; j++) { for (size_t j = 0; j <= N; j++) {
const double dj = const double dj =
x - Point(N, j, a, b); // only thing that depends on [a,b] x - Point(N, j, a, b); // only thing that depends on [a,b]
@ -44,16 +44,16 @@ Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) {
// Beginning of interval, j = 0, x(0) = a // Beginning of interval, j = 0, x(0) = a
weights(0) = 0.5 / distances(0); weights(0) = 0.5 / distances(0);
// All intermediate points j=1:N-2 // All intermediate points j=1:N-1
double d = weights(0), s = -1; // changes sign s at every iteration double d = weights(0), s = -1; // changes sign s at every iteration
for (size_t j = 1; j < N - 1; j++, s = -s) { for (size_t j = 1; j < N; j++, s = -s) {
weights(j) = s / distances(j); weights(j) = s / distances(j);
d += weights(j); d += weights(j);
} }
// End of interval, j = N-1, x(N-1) = b // End of interval, j = N, x(N) = b
weights(N - 1) = 0.5 * s / distances(N - 1); weights(N) = 0.5 * s / distances(N);
d += weights(N - 1); d += weights(N);
// normalize // normalize
return weights / d; return weights / d;
@ -61,30 +61,30 @@ Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) {
Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) { Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) {
// Allocate space for weights // Allocate space for weights
Weights weightDerivatives(N); Weights weightDerivatives(N + 1);
// toggle variable so we don't need to use `pow` for -1 // toggle variable so we don't need to use `pow` for -1
double t = -1; double t = -1;
// We start by getting distances from x to all Chebyshev points // We start by getting distances from x to all Chebyshev points
// as well as getting smallest distance // as well as getting smallest distance
Weights distances(N); Weights distances(N + 1);
for (size_t j = 0; j < N; j++) { for (size_t j = 0; j <= N; j++) {
const double dj = const double dj =
x - Point(N, j, a, b); // only thing that depends on [a,b] x - Point(N, j, a, b); // only thing that depends on [a,b]
if (std::abs(dj) < 1e-12) { if (std::abs(dj) < 1e-12) {
// exceptional case: x coincides with a Chebyshev point // exceptional case: x coincides with a Chebyshev point
weightDerivatives.setZero(); weightDerivatives.setZero();
// compute the jth row of the differentiation matrix for this point // compute the jth row of the differentiation matrix for this point
double cj = (j == 0 || j == N - 1) ? 2. : 1.; double cj = (j == 0 || j == N) ? 2. : 1.;
for (size_t k = 0; k < N; k++) { for (size_t k = 0; k <= N; k++) {
if (j == 0 && k == 0) { if (j == 0 && k == 0) {
// we reverse the sign since we order the cheb points from -1 to 1 // we reverse the sign since we order the cheb points from -1 to 1
weightDerivatives(k) = -(cj * (N - 1) * (N - 1) + 1) / 6.0; weightDerivatives(k) = -(cj * N * N + 1) / 6.0;
} else if (j == N - 1 && k == N - 1) { } else if (j == N && k == N) {
// we reverse the sign since we order the cheb points from -1 to 1 // we reverse the sign since we order the cheb points from -1 to 1
weightDerivatives(k) = (cj * (N - 1) * (N - 1) + 1) / 6.0; weightDerivatives(k) = (cj * N * N + 1) / 6.0;
} else if (k == j) { } else if (k == j) {
double xj = Point(N, j); double xj = Point(N, j);
double xj2 = xj * xj; double xj2 = xj * xj;
@ -92,7 +92,7 @@ Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) {
} else { } else {
double xj = Point(N, j); double xj = Point(N, j);
double xk = Point(N, k); double xk = Point(N, k);
double ck = (k == 0 || k == N - 1) ? 2. : 1.; double ck = (k == 0 || k == N) ? 2. : 1.;
t = ((j + k) % 2) == 0 ? 1 : -1; t = ((j + k) % 2) == 0 ? 1 : -1;
weightDerivatives(k) = (cj / ck) * t / (xj - xk); weightDerivatives(k) = (cj / ck) * t / (xj - xk);
} }
@ -111,8 +111,8 @@ Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) {
double g = 0, k = 0; double g = 0, k = 0;
double w = 1; double w = 1;
for (size_t j = 0; j < N; j++) { for (size_t j = 0; j <= N; j++) {
if (j == 0 || j == N - 1) { if (j == 0 || j == N) {
w = 0.5; w = 0.5;
} else { } else {
w = 1.0; w = 1.0;
@ -128,13 +128,13 @@ Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) {
double s = 1; // changes sign s at every iteration double s = 1; // changes sign s at every iteration
double g2 = g * g; double g2 = g * g;
for (size_t j = 0; j < N; j++, s = -s) { for (size_t j = 0; j <= N; j++, s = -s) {
// Beginning of interval, j = 0, x0 = -1.0 and end of interval, j = N-1, // Beginning of interval, j = 0, x0 = -1.0
// x0 = 1.0 // and end of interval, j = N, x0 = 1.0
if (j == 0 || j == N - 1) { if (j == 0 || j == N) {
w = 0.5; w = 0.5;
} else { } else {
// All intermediate points j=1:N-2 // All intermediate points j=1:N-1
w = 1.0; w = 1.0;
} }
weightDerivatives(j) = (w * -s / (g * distances(j) * distances(j))) - weightDerivatives(j) = (w * -s / (g * distances(j) * distances(j))) -
@ -146,8 +146,8 @@ Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) {
Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a, Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a,
double b) { double b) {
DiffMatrix D(N, N); DiffMatrix D(N + 1, N + 1);
if (N == 1) { if (N + 1 == 1) {
D(0, 0) = 1; D(0, 0) = 1;
return D; return D;
} }
@ -155,22 +155,22 @@ Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a,
// toggle variable so we don't need to use `pow` for -1 // toggle variable so we don't need to use `pow` for -1
double t = -1; double t = -1;
for (size_t i = 0; i < N; i++) { for (size_t i = 0; i <= N; i++) {
double xi = Point(N, i); double xi = Point(N, i);
double ci = (i == 0 || i == N - 1) ? 2. : 1.; double ci = (i == 0 || i == N) ? 2. : 1.;
for (size_t j = 0; j < N; j++) { for (size_t j = 0; j <= N; j++) {
if (i == 0 && j == 0) { if (i == 0 && j == 0) {
// we reverse the sign since we order the cheb points from -1 to 1 // we reverse the sign since we order the cheb points from -1 to 1
D(i, j) = -(ci * (N - 1) * (N - 1) + 1) / 6.0; D(i, j) = -(ci * N * N + 1) / 6.0;
} else if (i == N - 1 && j == N - 1) { } else if (i == N && j == N) {
// we reverse the sign since we order the cheb points from -1 to 1 // we reverse the sign since we order the cheb points from -1 to 1
D(i, j) = (ci * (N - 1) * (N - 1) + 1) / 6.0; D(i, j) = (ci * N * N + 1) / 6.0;
} else if (i == j) { } else if (i == j) {
double xi2 = xi * xi; double xi2 = xi * xi;
D(i, j) = -xi / (2 * (1 - xi2)); D(i, j) = -xi / (2 * (1 - xi2));
} else { } else {
double xj = Point(N, j); double xj = Point(N, j);
double cj = (j == 0 || j == N - 1) ? 2. : 1.; double cj = (j == 0 || j == N) ? 2. : 1.;
t = ((i + j) % 2) == 0 ? 1 : -1; t = ((i + j) % 2) == 0 ? 1 : -1;
D(i, j) = (ci / cj) * t / (xi - xj); D(i, j) = (ci / cj) * t / (xi - xj);
} }
@ -182,15 +182,15 @@ Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a,
Weights Chebyshev2::IntegrationWeights(size_t N, double a, double b) { Weights Chebyshev2::IntegrationWeights(size_t N, double a, double b) {
// Allocate space for weights // Allocate space for weights
Weights weights(N); Weights weights(N + 1);
size_t K = N - 1, // number of intervals between N points size_t K = N, // number of intervals between N points
K2 = K * K; K2 = K * K;
weights(0) = 0.5 * (b - a) / (K2 + K % 2 - 1); weights(0) = 0.5 * (b - a) / (K2 + K % 2 - 1);
weights(N - 1) = weights(0); weights(N) = weights(0);
size_t last_k = K / 2 + K % 2 - 1; size_t last_k = K / 2 + K % 2 - 1;
for (size_t i = 1; i <= N - 2; ++i) { for (size_t i = 1; i <= N - 1; ++i) {
double theta = i * M_PI / K; double theta = i * M_PI / K;
weights(i) = (K % 2 == 0) ? 1 - cos(K * theta) / (K2 - 1) : 1; weights(i) = (K % 2 == 0) ? 1 - cos(K * theta) / (K2 - 1) : 1;

14
gtsam/basis/Chebyshev2.h
View File

@ -62,8 +62,8 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
* @return double * @return double
*/ */
static double Point(size_t N, int j, double a = -1, double b = 1) { static double Point(size_t N, int j, double a = -1, double b = 1) {
assert(j >= 0 && size_t(j) < N); assert(j >= 0 && size_t(j) <= N);
const double dtheta = M_PI / (N > 1 ? (N - 1) : 1); const double dtheta = M_PI / (N > 1 ? N : 1);
// We add -PI so that we get values ordered from -1 to +1 // We add -PI so that we get values ordered from -1 to +1
// sin(-M_PI_2 + dtheta*j); also works // sin(-M_PI_2 + dtheta*j); also works
return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2; return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2;
@ -71,8 +71,8 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
/// All Chebyshev points /// All Chebyshev points
static Vector Points(size_t N) { static Vector Points(size_t N) {
Vector points(N); Vector points(N + 1);
for (size_t j = 0; j < N; j++) { for (size_t j = 0; j <= N; j++) {
points(j) = Point(N, j); points(j) = Point(N, j);
} }
return points; return points;
@ -92,7 +92,7 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
} }
/** /**
* Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values) * Evaluate Chebyshev Weights on [-1,1] at any x up to order N (N+1 values)
* These weights implement barycentric interpolation at a specific x. * These weights implement barycentric interpolation at a specific x.
* More precisely, f(x) ~ [w0;...;wN] * [f0;...;fN], where the fj are the * More precisely, f(x) ~ [w0;...;wN] * [f0;...;fN], where the fj are the
* values of the function f at the Chebyshev points. As such, for a given x we * values of the function f at the Chebyshev points. As such, for a given x we
@ -142,8 +142,8 @@ class GTSAM_EXPORT Chebyshev2 : public Basis<Chebyshev2> {
template <size_t M> template <size_t M>
static Matrix matrix(std::function<Eigen::Matrix<double, M, 1>(double)> f, static Matrix matrix(std::function<Eigen::Matrix<double, M, 1>(double)> f,
size_t N, double a = -1, double b = 1) { size_t N, double a = -1, double b = 1) {
Matrix Xmat(M, N); Matrix Xmat(M, N + 1);
for (size_t j = 0; j < N; j++) { for (size_t j = 0; j <= N; j++) {
Xmat.col(j) = f(Point(N, j, a, b)); Xmat.col(j) = f(Point(N, j, a, b));
} }
return Xmat; return Xmat;

12
gtsam/basis/tests/testBasisFactors.cpp
View File

@ -57,7 +57,7 @@ TEST(BasisFactors, EvaluationFactor) {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.add(factor); graph.add(factor);
Vector functionValues(N); Vector functionValues(N + 1);
functionValues.setZero(); functionValues.setZero();
Values initial; Values initial;
@ -84,7 +84,7 @@ TEST(BasisFactors, VectorEvaluationFactor) {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.add(factor); graph.add(factor);
gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N); gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N + 1);
Values initial; Values initial;
initial.insert<gtsam::Matrix>(key, stateMatrix); initial.insert<gtsam::Matrix>(key, stateMatrix);
@ -132,7 +132,7 @@ TEST(BasisFactors, VectorComponentFactor) {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.add(factor); graph.add(factor);
gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(P, N); gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(P, N + 1);
Values initial; Values initial;
initial.insert<gtsam::Matrix>(key, stateMatrix); initial.insert<gtsam::Matrix>(key, stateMatrix);
@ -157,7 +157,7 @@ TEST(BasisFactors, ManifoldEvaluationFactor) {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.add(factor); graph.add(factor);
gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(3, N); gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(3, N + 1);
Values initial; Values initial;
initial.insert<gtsam::Matrix>(key, stateMatrix); initial.insert<gtsam::Matrix>(key, stateMatrix);
@ -184,7 +184,7 @@ TEST(BasisFactors, VecDerivativePrior) {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.add(vecDPrior); graph.add(vecDPrior);
gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N); gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N + 1);
Values initial; Values initial;
initial.insert<gtsam::Matrix>(key, stateMatrix); initial.insert<gtsam::Matrix>(key, stateMatrix);
@ -211,7 +211,7 @@ TEST(BasisFactors, ComponentDerivativeFactor) {
graph.add(controlDPrior); graph.add(controlDPrior);
Values initial; Values initial;
gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N); gtsam::Matrix stateMatrix = gtsam::Matrix::Zero(M, N + 1);
initial.insert<gtsam::Matrix>(key, stateMatrix); initial.insert<gtsam::Matrix>(key, stateMatrix);
LevenbergMarquardtParams parameters; LevenbergMarquardtParams parameters;

119
gtsam/basis/tests/testChebyshev2.cpp
View File

@ -39,9 +39,9 @@ const size_t N = 32;
//****************************************************************************** //******************************************************************************
TEST(Chebyshev2, Point) { TEST(Chebyshev2, Point) {
static const int N = 5; static const int N = 4;
auto points = Chebyshev2::Points(N); auto points = Chebyshev2::Points(N);
Vector expected(N); Vector expected(N + 1);
expected << -1., -sqrt(2.) / 2., 0., sqrt(2.) / 2., 1.; expected << -1., -sqrt(2.) / 2., 0., sqrt(2.) / 2., 1.;
static const double tol = 1e-15; // changing this reveals errors static const double tol = 1e-15; // changing this reveals errors
EXPECT_DOUBLES_EQUAL(expected(0), points(0), tol); EXPECT_DOUBLES_EQUAL(expected(0), points(0), tol);
@ -57,9 +57,9 @@ TEST(Chebyshev2, Point) {
//****************************************************************************** //******************************************************************************
TEST(Chebyshev2, PointInInterval) { TEST(Chebyshev2, PointInInterval) {
static const int N = 5; static const int N = 4;
auto points = Chebyshev2::Points(N, 0, 20); auto points = Chebyshev2::Points(N, 0, 20);
Vector expected(N); Vector expected(N + 1);
expected << 0., 1. - sqrt(2.) / 2., 1., 1. + sqrt(2.) / 2., 2.; expected << 0., 1. - sqrt(2.) / 2., 1., 1. + sqrt(2.) / 2., 2.;
expected *= 10.0; expected *= 10.0;
static const double tol = 1e-15; // changing this reveals errors static const double tol = 1e-15; // changing this reveals errors
@ -77,9 +77,9 @@ TEST(Chebyshev2, PointInInterval) {
//****************************************************************************** //******************************************************************************
// InterpolatingPolynomial[{{-1, 4}, {0, 2}, {1, 6}}, 0.5] // InterpolatingPolynomial[{{-1, 4}, {0, 2}, {1, 6}}, 0.5]
TEST(Chebyshev2, Interpolate2) { TEST(Chebyshev2, Interpolate2) {
size_t N = 3; size_t N = 2;
Chebyshev2::EvaluationFunctor fx(N, 0.5); Chebyshev2::EvaluationFunctor fx(N, 0.5);
Vector f(N); Vector f(N + 1);
f << 4, 2, 6; f << 4, 2, 6;
EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9); EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9);
} }
@ -87,7 +87,7 @@ TEST(Chebyshev2, Interpolate2) {
//****************************************************************************** //******************************************************************************
// InterpolatingPolynomial[{{0, 4}, {1, 2}, {2, 6}}, 1.5] // InterpolatingPolynomial[{{0, 4}, {1, 2}, {2, 6}}, 1.5]
TEST(Chebyshev2, Interpolate2_Interval) { TEST(Chebyshev2, Interpolate2_Interval) {
Chebyshev2::EvaluationFunctor fx(3, 1.5, 0, 2); Chebyshev2::EvaluationFunctor fx(2, 1.5, 0, 2);
Vector3 f(4, 2, 6); Vector3 f(4, 2, 6);
EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9); EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9);
} }
@ -96,7 +96,7 @@ TEST(Chebyshev2, Interpolate2_Interval) {
// InterpolatingPolynomial[{{-1, 4}, {-Sqrt[2]/2, 2}, {0, 6}, {Sqrt[2]/2,3}, {1, // InterpolatingPolynomial[{{-1, 4}, {-Sqrt[2]/2, 2}, {0, 6}, {Sqrt[2]/2,3}, {1,
// 3}}, 0.5] // 3}}, 0.5]
TEST(Chebyshev2, Interpolate5) { TEST(Chebyshev2, Interpolate5) {
Chebyshev2::EvaluationFunctor fx(5, 0.5); Chebyshev2::EvaluationFunctor fx(4, 0.5);
Vector f(5); Vector f(5);
f << 4, 2, 6, 3, 3; f << 4, 2, 6, 3, 3;
EXPECT_DOUBLES_EQUAL(4.34283, fx(f), 1e-5); EXPECT_DOUBLES_EQUAL(4.34283, fx(f), 1e-5);
@ -108,13 +108,13 @@ TEST(Chebyshev2, InterpolateVector) {
double t = 30, a = 0, b = 100; double t = 30, a = 0, b = 100;
const size_t N = 3; const size_t N = 3;
// Create 2x3 matrix with Vectors at Chebyshev points // Create 2x3 matrix with Vectors at Chebyshev points
Matrix X = Matrix::Zero(2, N); Matrix X = Matrix::Zero(2, N + 1);
X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp
// Check value // Check value
Vector expected(2); Vector expected(2);
expected << t, 0; expected << t, 0;
Eigen::Matrix<double, /*2x2N*/ -1, -1> actualH(2, 2 * N); Eigen::Matrix<double, /*2x2N*/ -1, -1> actualH(2, 2 * (N + 1));
Chebyshev2::VectorEvaluationFunctor fx(2, N, t, a, b); Chebyshev2::VectorEvaluationFunctor fx(2, N, t, a, b);
EXPECT(assert_equal(expected, fx(X, actualH), 1e-9)); EXPECT(assert_equal(expected, fx(X, actualH), 1e-9));
@ -123,8 +123,7 @@ TEST(Chebyshev2, InterpolateVector) {
std::function<Vector2(Matrix)> f = std::function<Vector2(Matrix)> f =
std::bind(&Chebyshev2::VectorEvaluationFunctor::operator(), fx, std::bind(&Chebyshev2::VectorEvaluationFunctor::operator(), fx,
std::placeholders::_1, nullptr); std::placeholders::_1, nullptr);
Matrix numericalH = Matrix numericalH = numericalDerivative11<Vector2, Matrix, 2 * (N + 1)>(f, X);
numericalDerivative11<Vector2, Matrix, 2 * N>(f, X);
EXPECT(assert_equal(numericalH, actualH, 1e-9)); EXPECT(assert_equal(numericalH, actualH, 1e-9));
} }
@ -133,10 +132,10 @@ TEST(Chebyshev2, InterpolateVector) {
TEST(Chebyshev2, InterpolatePose2) { TEST(Chebyshev2, InterpolatePose2) {
double t = 30, a = 0, b = 100; double t = 30, a = 0, b = 100;
Matrix X(3, N); Matrix X(3, N + 1);
X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp
X.row(1) = Vector::Zero(N); X.row(1) = Vector::Zero(N + 1);
X.row(2) = 0.1 * Vector::Ones(N); X.row(2) = 0.1 * Vector::Ones(N + 1);
Vector xi(3); Vector xi(3);
xi << t, 0, 0.1; xi << t, 0, 0.1;
@ -151,8 +150,7 @@ TEST(Chebyshev2, InterpolatePose2) {
std::function<Pose2(Matrix)> f = std::function<Pose2(Matrix)> f =
std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose2>::operator(), fx, std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose2>::operator(), fx,
std::placeholders::_1, nullptr); std::placeholders::_1, nullptr);
Matrix numericalH = Matrix numericalH = numericalDerivative11<Pose2, Matrix, 3 * (N + 1)>(f, X);
numericalDerivative11<Pose2, Matrix, 3 * N>(f, X);
EXPECT(assert_equal(numericalH, actualH, 1e-9)); EXPECT(assert_equal(numericalH, actualH, 1e-9));
} }
@ -167,9 +165,9 @@ TEST(Chebyshev2, InterpolatePose3) {
Pose3 pose(R, Point3(1, 2, 3)); Pose3 pose(R, Point3(1, 2, 3));
Vector6 xi = Pose3::ChartAtOrigin::Local(pose); Vector6 xi = Pose3::ChartAtOrigin::Local(pose);
Eigen::Matrix<double, /*6x6N*/ -1, -1> actualH(6, 6 * N); Eigen::Matrix<double, /*6x6N*/ -1, -1> actualH(6, 6 * N + 1);
Matrix X = Matrix::Zero(6, N); Matrix X = Matrix::Zero(6, N + 1);
X.col(11) = xi; X.col(11) = xi;
Chebyshev2::ManifoldEvaluationFunctor<Pose3> fx(N, t, a, b); Chebyshev2::ManifoldEvaluationFunctor<Pose3> fx(N, t, a, b);
@ -181,8 +179,7 @@ TEST(Chebyshev2, InterpolatePose3) {
std::function<Pose3(Matrix)> f = std::function<Pose3(Matrix)> f =
std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose3>::operator(), fx, std::bind(&Chebyshev2::ManifoldEvaluationFunctor<Pose3>::operator(), fx,
std::placeholders::_1, nullptr); std::placeholders::_1, nullptr);
Matrix numericalH = Matrix numericalH = numericalDerivative11<Pose3, Matrix, 6 * (N + 1)>(f, X);
numericalDerivative11<Pose3, Matrix, 6 * N>(f, X);
EXPECT(assert_equal(numericalH, actualH, 1e-8)); EXPECT(assert_equal(numericalH, actualH, 1e-8));
} }
#endif #endif
@ -200,16 +197,16 @@ TEST(Chebyshev2, Decomposition) {
FitBasis<Chebyshev2> actual(sequence, model, 3); FitBasis<Chebyshev2> actual(sequence, model, 3);
// Check // Check
Vector expected(3); Vector expected(4);
expected << -1, 0, 1; expected << -1, -0.5, 0.5, 1;
EXPECT(assert_equal(expected, actual.parameters(), 1e-4)); EXPECT(assert_equal(expected, actual.parameters(), 1e-4));
} }
//****************************************************************************** //******************************************************************************
TEST(Chebyshev2, DifferentiationMatrix3) { TEST(Chebyshev2, DifferentiationMatrix3) {
// Trefethen00book, p.55 // Trefethen00book, p.55
const size_t N = 3; const size_t N = 2;
Matrix expected(N, N); Matrix expected(N + 1, N + 1);
// Differentiation matrix computed from chebfun // Differentiation matrix computed from chebfun
expected << 1.5000, -2.0000, 0.5000, // expected << 1.5000, -2.0000, 0.5000, //
0.5000, -0.0000, -0.5000, // 0.5000, -0.0000, -0.5000, //
@ -225,8 +222,8 @@ TEST(Chebyshev2, DifferentiationMatrix3) {
//****************************************************************************** //******************************************************************************
TEST(Chebyshev2, DerivativeMatrix6) { TEST(Chebyshev2, DerivativeMatrix6) {
// Trefethen00book, p.55 // Trefethen00book, p.55
const size_t N = 6; const size_t N = 5;
Matrix expected(N, N); Matrix expected(N + 1, N + 1);
expected << 8.5000, -10.4721, 2.8944, -1.5279, 1.1056, -0.5000, // expected << 8.5000, -10.4721, 2.8944, -1.5279, 1.1056, -0.5000, //
2.6180, -1.1708, -2.0000, 0.8944, -0.6180, 0.2764, // 2.6180, -1.1708, -2.0000, 0.8944, -0.6180, 0.2764, //
-0.7236, 2.0000, -0.1708, -1.6180, 0.8944, -0.3820, // -0.7236, 2.0000, -0.1708, -1.6180, 0.8944, -0.3820, //
@ -238,7 +235,7 @@ TEST(Chebyshev2, DerivativeMatrix6) {
expected = -expected; expected = -expected;
Matrix actual = Chebyshev2::DifferentiationMatrix(N); Matrix actual = Chebyshev2::DifferentiationMatrix(N);
EXPECT(assert_equal((Matrix)expected, actual, 1e-4)); EXPECT(assert_equal(expected, actual, 1e-4));
} }
// test function for CalculateWeights and DerivativeWeights // test function for CalculateWeights and DerivativeWeights
@ -255,8 +252,8 @@ double fprime(double x) {
//****************************************************************************** //******************************************************************************
TEST(Chebyshev2, CalculateWeights) { TEST(Chebyshev2, CalculateWeights) {
Eigen::Matrix<double, -1, 1> fvals(N); Eigen::Matrix<double, -1, 1> fvals(N + 1);
for (size_t i = 0; i < N; i++) { for (size_t i = 0; i <= N; i++) {
fvals(i) = f(Chebyshev2::Point(N, i)); fvals(i) = f(Chebyshev2::Point(N, i));
} }
double x1 = 0.7, x2 = -0.376; double x1 = 0.7, x2 = -0.376;
@ -269,8 +266,8 @@ TEST(Chebyshev2, CalculateWeights) {
TEST(Chebyshev2, CalculateWeights2) { TEST(Chebyshev2, CalculateWeights2) {
double a = 0, b = 10, x1 = 7, x2 = 4.12; double a = 0, b = 10, x1 = 7, x2 = 4.12;
Eigen::Matrix<double, -1, 1> fvals(N); Eigen::Matrix<double, -1, 1> fvals(N + 1);
for (size_t i = 0; i < N; i++) { for (size_t i = 0; i <= N; i++) {
fvals(i) = f(Chebyshev2::Point(N, i, a, b)); fvals(i) = f(Chebyshev2::Point(N, i, a, b));
} }
@ -284,8 +281,8 @@ TEST(Chebyshev2, CalculateWeights2) {
} }
TEST(Chebyshev2, DerivativeWeights) { TEST(Chebyshev2, DerivativeWeights) {
Eigen::Matrix<double, -1, 1> fvals(N); Eigen::Matrix<double, -1, 1> fvals(N + 1);
for (size_t i = 0; i < N; i++) { for (size_t i = 0; i <= N; i++) {
fvals(i) = f(Chebyshev2::Point(N, i)); fvals(i) = f(Chebyshev2::Point(N, i));
} }
double x1 = 0.7, x2 = -0.376, x3 = 0.0; double x1 = 0.7, x2 = -0.376, x3 = 0.0;
@ -307,8 +304,8 @@ TEST(Chebyshev2, DerivativeWeights) {
TEST(Chebyshev2, DerivativeWeights2) { TEST(Chebyshev2, DerivativeWeights2) {
double x1 = 5, x2 = 4.12, a = 0, b = 10; double x1 = 5, x2 = 4.12, a = 0, b = 10;
Eigen::Matrix<double, -1, 1> fvals(N); Eigen::Matrix<double, -1, 1> fvals(N + 1);
for (size_t i = 0; i < N; i++) { for (size_t i = 0; i <= N; i++) {
fvals(i) = f(Chebyshev2::Point(N, i, a, b)); fvals(i) = f(Chebyshev2::Point(N, i, a, b));
} }
@ -347,7 +344,7 @@ TEST(Chebyshev2, DerivativeWeights6) {
const size_t N6 = 6; const size_t N6 = 6;
Matrix D6 = Chebyshev2::DifferentiationMatrix(N6); Matrix D6 = Chebyshev2::DifferentiationMatrix(N6);
Chebyshev2::Parameters x = Chebyshev2::Points(N6); // ramp with slope 1 Chebyshev2::Parameters x = Chebyshev2::Points(N6); // ramp with slope 1
EXPECT(assert_equal(Vector::Ones(N6), Vector(D6 * x))); EXPECT(assert_equal(Vector::Ones(N6 + 1), Vector(D6 * x)));
} }
//****************************************************************************** //******************************************************************************
@ -356,14 +353,14 @@ TEST(Chebyshev2, DerivativeWeights7) {
const size_t N7 = 7; const size_t N7 = 7;
Matrix D7 = Chebyshev2::DifferentiationMatrix(N7); Matrix D7 = Chebyshev2::DifferentiationMatrix(N7);
Chebyshev2::Parameters x = Chebyshev2::Points(N7); // ramp with slope 1 Chebyshev2::Parameters x = Chebyshev2::Points(N7); // ramp with slope 1
EXPECT(assert_equal(Vector::Ones(N7), Vector(D7 * x))); EXPECT(assert_equal(Vector::Ones(N7 + 1), Vector(D7 * x)));
} }
//****************************************************************************** //******************************************************************************
// Check derivative in two different ways: numerical and using D on f // Check derivative in two different ways: numerical and using D on f
Vector6 f3_at_6points = (Vector6() << 4, 2, 6, 2, 4, 3).finished(); Vector6 f3_at_6points = (Vector6() << 4, 2, 6, 2, 4, 3).finished();
double proxy3(double x) { double proxy3(double x) {
return Chebyshev2::EvaluationFunctor(6, x)(f3_at_6points); return Chebyshev2::EvaluationFunctor(5, x)(f3_at_6points);
} }
TEST(Chebyshev2, Derivative6) { TEST(Chebyshev2, Derivative6) {
@ -373,21 +370,23 @@ TEST(Chebyshev2, Derivative6) {
const double x = 0.2; const double x = 0.2;
Matrix numeric_dTdx = numericalDerivative11<double, double>(proxy3, x); Matrix numeric_dTdx = numericalDerivative11<double, double>(proxy3, x);
size_t N = 5;
// Calculate derivatives at Chebyshev points using D3, interpolate // Calculate derivatives at Chebyshev points using D3, interpolate
Matrix D6 = Chebyshev2::DifferentiationMatrix(6); Matrix D6 = Chebyshev2::DifferentiationMatrix(N);
Vector derivative_at_points = D6 * f3_at_6points; Vector derivative_at_points = D6 * f3_at_6points;
Chebyshev2::EvaluationFunctor fx(6, x); Chebyshev2::EvaluationFunctor fx(N, x);
EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8); EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8);
// Do directly // Do directly
Chebyshev2::DerivativeFunctor dfdx(6, x); Chebyshev2::DerivativeFunctor dfdx(N, x);
EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8); EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8);
} }
//****************************************************************************** //******************************************************************************
// Assert that derivative also works in non-standard interval [0,3] // Assert that derivative also works in non-standard interval [0,3]
double proxy4(double x) { double proxy4(double x) {
return Chebyshev2::EvaluationFunctor(6, x, 0, 3)(f3_at_6points); return Chebyshev2::EvaluationFunctor(5, x, 0, 3)(f3_at_6points);
} }
TEST(Chebyshev2, Derivative6_03) { TEST(Chebyshev2, Derivative6_03) {
@ -397,14 +396,16 @@ TEST(Chebyshev2, Derivative6_03) {
const double x = 0.2; const double x = 0.2;
Matrix numeric_dTdx = numericalDerivative11<double, double>(proxy4, x); Matrix numeric_dTdx = numericalDerivative11<double, double>(proxy4, x);
size_t N = 5;
// Calculate derivatives at Chebyshev points using D3, interpolate // Calculate derivatives at Chebyshev points using D3, interpolate
Matrix D6 = Chebyshev2::DifferentiationMatrix(6, 0, 3); Matrix D6 = Chebyshev2::DifferentiationMatrix(N, 0, 3);
Vector derivative_at_points = D6 * f3_at_6points; Vector derivative_at_points = D6 * f3_at_6points;
Chebyshev2::EvaluationFunctor fx(6, x, 0, 3); Chebyshev2::EvaluationFunctor fx(N, x, 0, 3);
EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8); EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8);
// Do directly // Do directly
Chebyshev2::DerivativeFunctor dfdx(6, x, 0, 3); Chebyshev2::DerivativeFunctor dfdx(N, x, 0, 3);
EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8); EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8);
} }
@ -415,12 +416,12 @@ TEST(Chebyshev2, VectorDerivativeFunctor) {
const double x = 0.2; const double x = 0.2;
using VecD = Chebyshev2::VectorDerivativeFunctor; using VecD = Chebyshev2::VectorDerivativeFunctor;
VecD fx(M, N, x, 0, 3); VecD fx(M, N, x, 0, 3);
Matrix X = Matrix::Zero(M, N); Matrix X = Matrix::Zero(M, N + 1);
Matrix actualH(M, M * N); Matrix actualH(M, M * (N + 1));
EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8)); EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8));
// Test Jacobian // Test Jacobian
Matrix expectedH = numericalDerivative11<Vector2, Matrix, M * N>( Matrix expectedH = numericalDerivative11<Vector2, Matrix, M*(N + 1)>(
std::bind(&VecD::operator(), fx, std::placeholders::_1, nullptr), X); std::bind(&VecD::operator(), fx, std::placeholders::_1, nullptr), X);
EXPECT(assert_equal(expectedH, actualH, 1e-7)); EXPECT(assert_equal(expectedH, actualH, 1e-7));
} }
@ -434,24 +435,24 @@ TEST(Chebyshev2, VectorDerivativeFunctor2) {
const Vector points = Chebyshev2::Points(N, 0, T); const Vector points = Chebyshev2::Points(N, 0, T);
// Assign the parameter matrix 1xN // Assign the parameter matrix 1xN
Matrix X(1, N); Matrix X(1, N + 1);
for (size_t i = 0; i < N; ++i) { for (size_t i = 0; i <= N; ++i) {
X(i) = f(points(i)); X(i) = f(points(i));
} }
// Evaluate the derivative at the chebyshev points using // Evaluate the derivative at the chebyshev points using
// VectorDerivativeFunctor. // VectorDerivativeFunctor.
for (size_t i = 0; i < N; ++i) { for (size_t i = 0; i <= N; ++i) {
VecD d(M, N, points(i), 0, T); VecD d(M, N, points(i), 0, T);
Vector1 Dx = d(X); Vector1 Dx = d(X);
EXPECT_DOUBLES_EQUAL(fprime(points(i)), Dx(0), 1e-6); EXPECT_DOUBLES_EQUAL(fprime(points(i)), Dx(0), 1e-6);
} }
// Test Jacobian at the first chebyshev point. // Test Jacobian at the first chebyshev point.
Matrix actualH(M, M * N); Matrix actualH(M, M * (N + 1));
VecD vecd(M, N, points(0), 0, T); VecD vecd(M, N, points(0), 0, T);
vecd(X, actualH); vecd(X, actualH);
Matrix expectedH = numericalDerivative11<Vector1, Matrix, M * N>( Matrix expectedH = numericalDerivative11<Vector1, Matrix, M*(N + 1)>(
std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X); std::bind(&VecD::operator(), vecd, std::placeholders::_1, nullptr), X);
EXPECT(assert_equal(expectedH, actualH, 1e-6)); EXPECT(assert_equal(expectedH, actualH, 1e-6));
} }
@ -464,11 +465,11 @@ TEST(Chebyshev2, ComponentDerivativeFunctor) {
using CompFunc = Chebyshev2::ComponentDerivativeFunctor; using CompFunc = Chebyshev2::ComponentDerivativeFunctor;
size_t row = 1; size_t row = 1;
CompFunc fx(M, N, row, x, 0, 3); CompFunc fx(M, N, row, x, 0, 3);
Matrix X = Matrix::Zero(M, N); Matrix X = Matrix::Zero(M, (N + 1));
Matrix actualH(1, M * N); Matrix actualH(1, M * (N + 1));
EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8); EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8);
Matrix expectedH = numericalDerivative11<double, Matrix, M * N>( Matrix expectedH = numericalDerivative11<double, Matrix, M*(N + 1)>(
std::bind(&CompFunc::operator(), fx, std::placeholders::_1, nullptr), X); std::bind(&CompFunc::operator(), fx, std::placeholders::_1, nullptr), X);
EXPECT(assert_equal(expectedH, actualH, 1e-7)); EXPECT(assert_equal(expectedH, actualH, 1e-7));
} }
@ -476,7 +477,7 @@ TEST(Chebyshev2, ComponentDerivativeFunctor) {
//****************************************************************************** //******************************************************************************
TEST(Chebyshev2, IntegralWeights) { TEST(Chebyshev2, IntegralWeights) {
const size_t N7 = 7; const size_t N7 = 7;
Vector actual = Chebyshev2::IntegrationWeights(N7); Vector actual = Chebyshev2::IntegrationWeights(N7 - 1);
Vector expected = (Vector(N7) << 0.0285714285714286, 0.253968253968254, Vector expected = (Vector(N7) << 0.0285714285714286, 0.253968253968254,
0.457142857142857, 0.520634920634921, 0.457142857142857, 0.457142857142857, 0.520634920634921, 0.457142857142857,
0.253968253968254, 0.0285714285714286) 0.253968253968254, 0.0285714285714286)
@ -484,7 +485,7 @@ TEST(Chebyshev2, IntegralWeights) {
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
const size_t N8 = 8; const size_t N8 = 8;
Vector actual2 = Chebyshev2::IntegrationWeights(N8); Vector actual2 = Chebyshev2::IntegrationWeights(N8 - 1);
Vector expected2 = (Vector(N8) << 0.0204081632653061, 0.190141007218208, Vector expected2 = (Vector(N8) << 0.0204081632653061, 0.190141007218208,
0.352242423718159, 0.437208405798326, 0.437208405798326, 0.352242423718159, 0.437208405798326, 0.437208405798326,
0.352242423718159, 0.190141007218208, 0.0204081632653061) 0.352242423718159, 0.190141007218208, 0.0204081632653061)