From cda97828d1ec3ec089c85330c5f4a4b5215940b7 Mon Sep 17 00:00:00 2001 From: Brett Downing Date: Mon, 4 Aug 2025 12:33:00 +1000 Subject: [PATCH 01/16] adds CartesianProduct manifolds --- gtsam/geometry/CartesianProduct.h | 354 ++++++++++++++++++ gtsam/geometry/tests/testCartesianProduct.cpp | 120 ++++++ 2 files changed, 474 insertions(+) create mode 100644 gtsam/geometry/CartesianProduct.h create mode 100644 gtsam/geometry/tests/testCartesianProduct.cpp diff --git a/gtsam/geometry/CartesianProduct.h b/gtsam/geometry/CartesianProduct.h new file mode 100644 index 0000000000..98adef6e3f --- /dev/null +++ b/gtsam/geometry/CartesianProduct.h @@ -0,0 +1,354 @@ +#pragma once + +#include + + +/* +Cartesian product to combine unrelated manifolds into one data structure +conceptually similar to a std::pair but you get to keep all the manifold properties +This provides syntactic sugar allowing Lie::interpolate to be applied to multiple objects at once. +For example, a trajectory involving control points for pose and calibration for a motorised zoom lens on a moving camera. + */ + + +// LieGroup requires: identity(), inverse(), composition operator*(p), static ExpMap, static LogMap +// provides: dimension, identity(h), inverse(h), compose(p,h1,h2), between, localCoordinates, retract, +// LieGroupTraits requires: dimension, identity(h), inverse(h), compose(p,h1,h2), between, localCoordinates, retract, Logmap, Expmap, AdjointMap, + +// ExpmapDerivative, LogmapDerivative are normally defined to provide jacobians for expmap and logmap, +// we can just refer to the expmap jacobians of the stored classes + + + + // XXX needs a compile time flag to check that the stored classes satisfy relevant properties + // (IsGroup && IsGroup) + // (IsManifold && IsManifold) + // (IsVectorSpace && IsVectorSpace) + // Except that I'm mostly interested in interpolate, so I only care about stuff with expmap and logmap + // camera calibration is a manifold not a group, so it can't encode a guarantee of continuity into an expmap. + // Manifold at least offers a rudimentary local vector space `Local(origin, other)->vector` and `Retract(origin, vector)->other` + // So I might be able to monkey-patch an interpolate method into those, + // except that it doesn't expose jacobians so it's useless for Expressions + + +namespace gtsam { + +template +class CartesianProduct +: public LieGroup, traits::dimension + traits::dimension> +{ +public: + constexpr static auto a_dim = traits::dimension; + constexpr static auto b_dim = traits::dimension; + constexpr static auto dimension = traits::dimension + traits::dimension; + //using LieGroup, traits::dimension + traits::dimension>::dimension; + + //using LieGroup, dimension>::ChartJacobian; + //using LieGroup, dimension>::Jacobian; + //using LieGroup, dimension>::TangentVector; + using LieGroup, dimension>::inverse; + + + typedef OptionalJacobian ChartJacobian; + typedef Eigen::Matrix Jacobian; + typedef Eigen::Matrix TangentVector; + + + // XXX For whatever reason, MatrixLieGroup::Jacobian isn't exposed on Pose3 + typedef Eigen::Matrix A_Jacobian; + typedef Eigen::Matrix B_Jacobian; + + +private: + A a_; + B b_; + +public: + /** Default constructor */ + CartesianProduct(const A& a = traits::Identity(), const B& b = traits::Identity()) : a_(a), b_(b) {} + + /** Copy constructor */ + CartesianProduct(const CartesianProduct& copy_ref) = default; + + + static CartesianProduct Create(const A& a, const B& b, + OptionalJacobian Ha = {}, + OptionalJacobian Hb = {}) + { + // XXX use transpose of getter jacobians + /* + ret = CartesianProduct(a, b) + ret.a(Ha.T()) + ret.b(Hb.T()) + */ + + if(Ha) + { + *Ha << Eigen::Matrix::Identity(), + Eigen::Matrix::Zero(); + } + if(Hb) + { + *Hb << Eigen::Matrix::Zero(), + Eigen::Matrix::Identity(); + } + return CartesianProduct(a, b); + } + + /** Getter for the first stored value */ + const A& a(OptionalJacobian H={}) const + { + if(H) + { + *H << Eigen::Matrix::Identity(), + Eigen::Matrix::Zero(); + } + return a_; + } + + /** Getter for the second stored value */ + const B& b(OptionalJacobian H={}) const + { + if(H) + { + *H << Eigen::Matrix::Zero(), + Eigen::Matrix::Identity(); + } + return b_; + } + + // ## pre-requisites for LieGroup + + /** inverse without derivative is required for LieGroup::inverse(H) */ + const CartesianProduct inverse() const + { + return CartesianProduct(traits::Inverse(a_), traits::Inverse(b_)); + } + + /** group composition operator, members of the cartesian product are are treated as fully orthogonal. */ + inline CartesianProduct operator*(const CartesianProduct& p) const + { + return CartesianProduct( + traits::Compose(a_, p.a()), + traits::Compose(b_, p.b())); + } + + + /// @} + /// @name Testable + /// @{ + + friend std::ostream& operator<< (std::ostream &os, const CartesianProduct& p) + { + os << "a: " << p.a() << "\n"; + os << "b: " << p.b(); + return os; + } + + /// print with optional string + void print(const std::string& s = "") const { + std::cout << (s.empty() ? s : s + " ") << *this << std::endl; + } + + /// assert equality up to a tolerance + bool equals(const CartesianProduct& p, double tol = 1e-9) const { + return traits::Equals(a_, p.a_, tol) && traits::Equals(b_, p.b_, tol); + } + + + + /// @} + /// @name Group + /// @{ + + static CartesianProduct Identity() { + return CartesianProduct(traits::Identity(), traits::Identity()); + } + + + static CartesianProduct Expmap(const TangentVector& v, ChartJacobian H = {}) + { + typename traits::TangentVector va = v.template block(0,0); + typename traits::TangentVector vb = v.template block(a_dim,0); + A a; + B b; + if(H) { + A_Jacobian Ha; + B_Jacobian Hb; + a = traits::Expmap(va, Ha); + b = traits::Expmap(vb, Hb); + *H << Eigen::Matrix::Zero(); + (*H).template block(0,0) = Ha; + (*H).template block(a_dim,a_dim) = Hb; + } + else + { + a = traits::Expmap(va); + b = traits::Expmap(vb); + } + return CartesianProduct(a,b); + } + + + static TangentVector Logmap(const CartesianProduct& p, ChartJacobian H = {}) + { + TangentVector tan; + if (H) { + A_Jacobian Ha; + B_Jacobian Hb; + tan << traits::Logmap(p.a_, Ha), + traits::Logmap(p.b_, Hb); + *H << Eigen::Matrix::Zero(); + (*H).template block(0,0) = Ha; + (*H).template block(a_dim,a_dim) = Hb; + // XXX check off-diag row/col order + //*H << A::LogmapDerivative(p.a_), Eigen::Matrix::Zero(), + // Eigen::Matrix::Zero(), B::LogmapDerivative(p.b_); + } + else + { + tan << traits::Logmap(p.a_), + traits::Logmap(p.b_); + } + return tan; + } + + + Jacobian AdjointMap() const + { + Jacobian H = Eigen::Matrix::Zero(); + H.template block(0,0) = traits::AdjointMap(a_); + H.template block(a_dim,a_dim) = traits::AdjointMap(b_); + return H; + } + + + TangentVector localCoordinates(const CartesianProduct& other) const + { + TangentVector v; + v << traits::Local(a(), other.a()), + traits::Local(b(), other.b()); + return v; + } + TangentVector localCoordinates(const CartesianProduct& other, + ChartJacobian H1, ChartJacobian H2 = {}) const + { + TangentVector v; + A_Jacobian H1a, H2a; + B_Jacobian H1b, H2b; + v << traits::Local(a(), other.a(), H1a, H2a), + traits::Local(b(), other.b(), H1b, H2b); + + if(H1){ + (*H1).template block(0,0) = H1a; + (*H1).template block(a_dim,a_dim) = H1b; + } + if(H2){ + (*H2).template block(0,0) = H2a; + (*H2).template block(a_dim,a_dim) = H2b; + } + + return v; + } + + CartesianProduct retract(const TangentVector& v) const + { + typename traits::TangentVector va = v.template block(0,0); + typename traits::TangentVector vb = v.template block(a_dim,0); + return CartesianProduct(traits::Retract(a(), va),traits::Retract(b(), vb)); + } + + CartesianProduct retract(const TangentVector& v, + ChartJacobian H1 = {}, ChartJacobian H2 = {}) const + { + typename traits::TangentVector va = v.template block(0,0); + typename traits::TangentVector vb = v.template block(a_dim,0); + A_Jacobian H1a, H2a; + B_Jacobian H1b, H2b; + CartesianProduct ret = CartesianProduct( + traits::Retract(a(), va, H1a, H2a), + traits::Retract(b(), vb, H1b, H2b)); + if(H1){ + (*H1).template block(0,0) = H1a; + (*H1).template block(a_dim,a_dim) = H1b; + } + if(H2){ + (*H2).template block(0,0) = H2a; + (*H2).template block(a_dim,a_dim) = H2b; + } + + return ret; + } + + + + + struct ChartAtOrigin { + static CartesianProduct Retract(const TangentVector& v, ChartJacobian H = {}) + { + typename traits::TangentVector va = v.template block(0,0); + typename traits::TangentVector vb = v.template block(a_dim,0); + A a; + B b; + if(H) { + A_Jacobian Ha; + B_Jacobian Hb; + a = traits::ChartAtOrigin::Retract(va, Ha); + b = traits::ChartAtOrigin::Retract(vb, Hb); + *H << Eigen::Matrix::Zero(); + (*H).template block(0,0) = Ha; + (*H).template block(a_dim,a_dim) = Hb; + } + else + { + a = traits::ChartAtOrigin::Retract(va); + b = traits::ChartAtOrigin::Retract(vb); + } + return CartesianProduct(a,b); + } + static TangentVector Local(const CartesianProduct& p, ChartJacobian H = {}) + { + TangentVector t; + if(H){ + A_Jacobian Ha; + B_Jacobian Hb; + t << traits::ChartAtOrigin::Local(p.a(), Ha), + traits::ChartAtOrigin::Local(p.b(), Hb); + *H << Eigen::Matrix::Zero(); + (*H).template block(0,0) = Ha; + (*H).template block(a_dim,a_dim) = Hb; + } + else + { + t << traits::ChartAtOrigin::Local(p.a()), + traits::ChartAtOrigin::Local(p.b()); + } + return t; + } + }; + + +}; + + +// create the Traits structure for the class + +template +struct traits > : +public internal::LieGroupTraits >, +public Testable > +{}; + + + + +/* +template +struct traits > : +public internal::Manifold > +{}; +*/ + + + +} \ No newline at end of file diff --git a/gtsam/geometry/tests/testCartesianProduct.cpp b/gtsam/geometry/tests/testCartesianProduct.cpp new file mode 100644 index 0000000000..ed396c9df0 --- /dev/null +++ b/gtsam/geometry/tests/testCartesianProduct.cpp @@ -0,0 +1,120 @@ +/** + * @file testCartesinProduct.cpp + * @brief Test manifold properties of combined manifolds + * @author Brett Downing + * @date April 2025 + */ + + + +#include +#include +#include +#include +#include +#include +#include + + +using namespace gtsam; + +// These types are mainly testing examples, +// see Event and Gal3 for more capable manifolds +typedef CartesianProduct StampedPoint3; +typedef CartesianProduct StampedPose3; +// split variant of pose3 allows comparing interpolate to interpolateRT +typedef CartesianProduct SplitPose3; +typedef CartesianProduct StampedSplitPose3; + + + +GTSAM_CONCEPT_TESTABLE_INST(StampedPoint3) +//GTSAM_CONCEPT_LIE_INST(StampedPoint3) + +static Point3 P(0.2, 0.7, -2); +static StampedPoint3 S(P, 0.1); +//****************************************************************************** +TEST( CartesianProduct , Constructor) { + StampedPoint3 p; +} + +//****************************************************************************** +TEST( CartesianProduct , StampedPoint3_Concept) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + //GTSAM_CONCEPT_ASSERT(IsVectorSpace); +} + +//****************************************************************************** +TEST( CartesianProduct , StampedPoint3_Invariants) { + StampedPoint3 p1(Point3(1, 2, 3),7), p2(Point3(4, 5, 6),8); + EXPECT(check_group_invariants(p1, p2)); + EXPECT(check_manifold_invariants(p1, p2)); +} +//****************************************************************************** + +TEST( CartesianProduct, StampedPoint3_interpolate) { + EXPECT(StampedPoint3(Point3(2,2,2), 2).equals(interpolate( + StampedPoint3(Point3(1,2,3),0), + StampedPoint3(Point3(3,2,1),4), 0.5), 1e-9)); +} + + + +//****************************************************************************** +TEST( CartesianProduct , StampedPose3_Concept) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + //GTSAM_CONCEPT_ASSERT(IsVectorSpace); +} + +//****************************************************************************** +TEST( CartesianProduct , StampedPose3_Invariants) { + StampedPose3 p1(Pose3(Rot3::Rodrigues(0.3,0.2,0.1), Point3(1, 2, 3)),7), + p2(Pose3(Rot3::Rodrigues(0.1,0.2,0.3), Point3(4, 5, 6)),8); + EXPECT(check_group_invariants(p1, p2)); + EXPECT(check_manifold_invariants(p1, p2)); +} +//****************************************************************************** + +TEST( CartesianProduct, StampedPose3_interpolate) { + EXPECT(StampedPose3(Pose3(Rot3::Rx(0), Point3(2,2,2)), 2).equals(interpolate( + StampedPose3(Pose3(Rot3::Rx(0), Point3(1,2,3)),0), + StampedPose3(Pose3(Rot3::Rx(0), Point3(3,2,1)),4), 0.5), 1e-7)); + EXPECT(StampedPose3(Pose3(Rot3::Rx(0.2), Point3(0,0,0)), 2).equals(interpolate( + StampedPose3(Pose3(Rot3::Rx(0), Point3(0,0,0)),0), + StampedPose3(Pose3(Rot3::Rx(0.4), Point3(0,0,0)),4), 0.5), 1e-9)); +} + + + +//****************************************************************************** +TEST( CartesianProduct , StampedSplitPose3_Concept) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + //GTSAM_CONCEPT_ASSERT(IsVectorSpace); +} + +//****************************************************************************** +TEST( CartesianProduct , StampedSplitPose3_Invariants) { + StampedSplitPose3 p1(SplitPose3(Rot3::Rodrigues(0.3,0.2,0.1), Point3(1, 2, 3)),7), + p2(SplitPose3(Rot3::Rodrigues(0.1,0.2,0.3), Point3(4, 5, 6)),8); + EXPECT(check_group_invariants(p1, p2)); + EXPECT(check_manifold_invariants(p1, p2)); +} +//****************************************************************************** + +TEST( CartesianProduct, StampedSplitPose3_interpolate) { + EXPECT(StampedSplitPose3(SplitPose3(Rot3::Rx(0.2), Point3(2,2,2)), 2).equals(interpolate( + StampedSplitPose3(SplitPose3(Rot3::Rx(0), Point3(1,2,3)),0), + StampedSplitPose3(SplitPose3(Rot3::Rx(0.4), Point3(3,2,1)),4), 0.5), 1e-9)); +} + + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + From fa4f31180f53e2e5ecc710b9c7c0a223b187dd12 Mon Sep 17 00:00:00 2001 From: Brett Downing Date: Mon, 4 Aug 2025 12:36:06 +1000 Subject: [PATCH 02/16] adds IrwinHall basis functions --- gtsam/basis/polynomial/IrwinHall.cpp | 78 +++++++++++++ gtsam/basis/polynomial/IrwinHall.h | 51 +++++++++ gtsam/basis/polynomial/kernels.h | 144 ++++++++++++++++++++++++ gtsam/basis/tests/testIrwinHall.cpp | 157 +++++++++++++++++++++++++++ 4 files changed, 430 insertions(+) create mode 100644 gtsam/basis/polynomial/IrwinHall.cpp create mode 100644 gtsam/basis/polynomial/IrwinHall.h create mode 100644 gtsam/basis/polynomial/kernels.h create mode 100644 gtsam/basis/tests/testIrwinHall.cpp diff --git a/gtsam/basis/polynomial/IrwinHall.cpp b/gtsam/basis/polynomial/IrwinHall.cpp new file mode 100644 index 0000000000..553312781a --- /dev/null +++ b/gtsam/basis/polynomial/IrwinHall.cpp @@ -0,0 +1,78 @@ + +#include "IrwinHall.h" +namespace gtsam { + +namespace kernels { + +/* +Irwin Hall coefficients provide canonical spline basis functions +https://oeis.org/A188816 + +Mathematica: +f[n_, k_] := + f[n, k] = Sum[(-1)^j Binomial[n, j] (x - j)^(n - 1), {j, 0, k}]; +T[n_, k_] := Table[Coefficient[f[n, k], x, t], {t, 0, n - 1}]; +Table[Table[T[n, k]/((n - 1)!), {k, 0, n - 1}], {n, 1, 7}] + +*/ +const piecewise_polynomial<1,1> IrwinHall0({ + {1.}, + {0.,1.}, + 1./2. +}); +const piecewise_polynomial<2,2> IrwinHall1({ + { 0., 1., + 2., -1.}, + {0.,1.,2.}, + 1. +}); +const piecewise_polynomial<3,3> IrwinHall2({ + { 0., 0., 1./2., + -(3./2.), 3., -1., + 9./2., -3., 1./2.}, + {0.,1.,2.,3.}, + 3./2. +}); +// this captures cubic splines +const piecewise_polynomial<4,4> IrwinHall3({ + { 0., 0., 0., 1./6., + 2./3., -2., 2., -(1./2.), + -(22./3.), 10., -4., 1./2., + 32./3., -8., 2., -(1/6.)}, + {0.,1.,2.,3.,4.}, + 2. +}); +const piecewise_polynomial<5,5> IrwinHall4({ + { 0., 0., 0., 0., 1./24., + -(5./24.), 5./6., -(5./4.), 5./6., -(1./6.), + 155./24., -(25./2.), 35./4., -(5./2.), 1./4., + -(655./24.), 65./2., -(55./4.), 5./2., -(1./6.), + 625./24., -(125./6.), 25./4., -(5./6.), 1./24.}, + {0.,1.,2.,3.,4.,5.}, + 5./2. +}); +const piecewise_polynomial<6,6> IrwinHall5({ + { 0., 0., 0., 0., 0., 1./120., + 1./20., -(1./4.), 1./2., -(1./2.), 1./4., -(1./24.), + -(79./20.), 39./4., -(19./2.), 9./2., -1., 1./12., + 731./20., -(231./4.), 71./2., -(21./2.), 3./2., -(1./12.), + -(1829./20.), 409./4., -(89./2.), 19./2., -1., 1./24., + 324./5., -54., 18., -3., 1/4., -(1./120.)}, + {0.,1.,2.,3.,4.,5.,6.}, + 3. +}); +const piecewise_polynomial<7,7> IrwinHall6({ + { 0., 0., 0., 0., 0., 0., 1./720., + -(7./720.), 7./120., -(7./48.), 7./36., -(7./48.), 7./120., -(1./120.), + 1337./720., -(133./24.), 329./48., -(161./36.), 77./48., -(7./24.), 1./48., + -(12089./360.), 196./3., -(1253./24.), 196./9., -(119./24.), 7./12., -(1./36.), + 59591./360., -(700./3.), 3227./24., -(364./9.), 161./24., -(7./12.), 1./48., + -(208943./720.), 7525./24., -(6671./48.), 1169./36., -(203./48.), 7./24., -(1./120.), + 117649./720., -(16807./120.), 2401./48., -(343./36.), 49./48., -(7./120.), 1./720.}, + {0.,1.,2.,3.,4.,5.,6.,7.}, + 7./2. +}); + +} // namespace kernels + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/basis/polynomial/IrwinHall.h b/gtsam/basis/polynomial/IrwinHall.h new file mode 100644 index 0000000000..9106a1e4e2 --- /dev/null +++ b/gtsam/basis/polynomial/IrwinHall.h @@ -0,0 +1,51 @@ +#pragma once + +#include "kernels.h" + +namespace gtsam { +namespace kernels { + +/* +Irwin Hall coefficients provide canonical spline basis functions +These functions are piecewise-defined polynomials with continuity up to their (N-1)th derivative +Irwin Hall 0 is a zeroth order spline (a discontinuous staircase) +Irwin Hall 3 is a cubic polynomial, so it has continuous (piecewise linear) accelerations + +https://oeis.org/A188816 + +Mathematica: +f[n_, k_] := + f[n, k] = Sum[(-1)^j Binomial[n, j] (x - j)^(n - 1), {j, 0, k}]; +T[n_, k_] := Table[Coefficient[f[n, k], x, t], {t, 0, n - 1}]; +Table[Table[T[n, k]/((n - 1)!), {k, 0, n - 1}], {n, 1, 7}] + + +This series of fucntions converges pretty quickly to a gaussian. +If you need higher orders (N), you can use a gaussian kernel directly +e^(-(1/2)((x-N/2)/(0.3*N/2)^2 ) +or defined in the fourier domain, cardinal splines are defined as sinc^N, and gaussians remain gaussian: + 'sinc to the n' has the same form as a gaussian with a simple domain transformation + e^(-0.5(w^2) * N/3) ~= sinc^N(w) + +the derivatives of a gaussian depend on Hermite polynomials which are a little complicated + +The inverse filter (1/sinc^N) for this explodes pretty fast at higher frequencies, + so filters beyond order 4 should step back a long way (up to an order of magnitude) from the nyquist limit + in order to avoid the vector sum crossing the edge of the Lie Algebra's Map + +High frequencies can be attenuated by processing the control points under a sharp + high-pass filter and passing factors that bais the result to zero. + The high-pass factors form a 'penalty' term under a generalisation over P-Splines. + + +*/ +extern const piecewise_polynomial<1,1> IrwinHall0; +extern const piecewise_polynomial<2,2> IrwinHall1; +extern const piecewise_polynomial<3,3> IrwinHall2; +extern const piecewise_polynomial<4,4> IrwinHall3; // equivalent to cubic spline +extern const piecewise_polynomial<5,5> IrwinHall4; +extern const piecewise_polynomial<6,6> IrwinHall5; +extern const piecewise_polynomial<7,7> IrwinHall6; + +} // namespace kernels +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/basis/polynomial/kernels.h b/gtsam/basis/polynomial/kernels.h new file mode 100644 index 0000000000..3993c93f81 --- /dev/null +++ b/gtsam/basis/polynomial/kernels.h @@ -0,0 +1,144 @@ +/** + * Kernels for continuous interpolating Finite Impulse Response filters + * This can be used to define continuous-time trajectory models + */ + +#pragma once + +#include + +namespace gtsam { + + +class kernel_base +{ +public: + virtual double evaluate(const double& t, OptionalJacobian<1, 1> H = {}) const = 0; + // earliest value of t that returns a non-zero kernel value + virtual double get_beginning() const = 0; + // centre of the distribution + virtual double get_center() const = 0; + // last value of t that returns a non-zero kernel value + virtual double get_end() const = 0; + + + virtual ~kernel_base() = default; + +}; + + +template +class piecewise_polynomial : public kernel_base +{ +public: + inline constexpr static auto order = O; + inline constexpr static auto pieces = P; + +protected: + + struct param_s{ + std::array, order > coefficients; + std::array intervals; + double center; + }; + const param_s params; + + +public: + piecewise_polynomial(param_s init_list):params(init_list){} + + double get_center() const override + { + return params.center; + } + double get_beginning() const override + { + return params.intervals[0]; + } + + double get_end() const override + { + return params.intervals[pieces]; + } + + std::array get_intervals() const { + return params.intervals; + } + +/** + evaluates the continuous kernel function at time t + + derivative: how many times to differentiate the kernel function + t: the time to evaluate it relative to the kernel's datum + H: the jacobian for gtsam (the next derivative, calls this function again.) + + */ + double evaluate(const double& t, OptionalJacobian<1, 1> H = {}) const override + { + return evaluate_d(0, t, H); + } + + double evaluate_d(size_t derivative, const double& t, OptionalJacobian<1, 1> H = {}) const + { + // quick bounds check + if(tparams.intervals[pieces]) + { + if(H) *H << 0; + return 0; + } + // locate the interval + for(size_t p = 0; p < pieces; p++) + { + if(t +T inverse_filter(double timestamp, const std::vector>& time_series) +{ + +// error is interpolate(control_points @ timestamp) +// least sum of squares ( kinda O(N^2) over the order of polynomial) +// + +// weighted_sum({ + // kernel(cp_time) * control_point + //}) + return T(); // XXX + +} + +*/ +// XXX a collection of inverse kernels for the spline kernels, +// defined within some frequency bounds. + +// XXX Consider using an acausal Butterworth or Chebyshev filter instead, +// this provides a 1:1 correspondence between the control points and the trajectory points +// and (mostly) eliminates the need for an inverse filter + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/basis/tests/testIrwinHall.cpp b/gtsam/basis/tests/testIrwinHall.cpp new file mode 100644 index 0000000000..126505d9e5 --- /dev/null +++ b/gtsam/basis/tests/testIrwinHall.cpp @@ -0,0 +1,157 @@ +/** + * @file TestIrwinHall.cpp + * @brief validate Irwin Hall coefficients and derivatives + * @author Brett Downing + * @date August 2025 + */ + + + +#include +#include +#include +#include +#include +#include +#include +#include + + +using namespace gtsam; +using namespace gtsam::kernels; + +// The summation of so many terms does get noisy, +// so split the epsilon and the tolerance +double epsilon = 1e-9; +double tolerance = 1e-7; + +// test continuity where the polynomial pieces join +TEST( IrwinHall , Continuity1 ) { + auto poly = IrwinHall1; + for(const double &t : poly.get_intervals()) { + EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), + poly.evaluate(t-epsilon), tolerance ); + } +} +TEST( IrwinHall , Continuity2 ) { + auto poly = IrwinHall2; + for(const double &t : poly.get_intervals()) { + EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), + poly.evaluate(t-epsilon), tolerance ); + } +} +TEST( IrwinHall , Continuity3 ) { + auto poly = IrwinHall3; + for(const double &t : poly.get_intervals()) { + EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), + poly.evaluate(t-epsilon), tolerance ); + } +} +TEST( IrwinHall , Continuity4 ) { + auto poly = IrwinHall4; + for(const double &t : poly.get_intervals()) { + EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), + poly.evaluate(t-epsilon), tolerance ); + } +} +TEST( IrwinHall , Continuity5 ) { + auto poly = IrwinHall5; + for(const double &t : poly.get_intervals()) { + EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), + poly.evaluate(t-epsilon), tolerance ); + } +} +TEST( IrwinHall , Continuity6 ) { + auto poly = IrwinHall6; + for(const double &t : poly.get_intervals()) { + EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), + poly.evaluate(t-epsilon), tolerance ); + } +} + + +// test continuity of derivatives where the polynomial pieces join +TEST( IrwinHall , DerivativeContinuity1 ) { + auto poly = IrwinHall1; + int max_d = poly.order-1; + for(const double &t : poly.get_intervals()) { + for(int d=0; d jacobian; + poly.evaluate_d(d, t, &jacobian); + double derivative = poly.evaluate_d(d+1, t); + EXPECT_DOUBLES_EQUAL(jacobian[0], derivative, tolerance ); + } + } +} + + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + From 8b85bee7b65f3e3171566ffde061e221577d40af Mon Sep 17 00:00:00 2001 From: Brett Downing Date: Wed, 6 Aug 2025 14:38:49 +1000 Subject: [PATCH 03/16] adds CDF for IrwinHall and exposes PiecewisePolynomial for testing --- gtsam/basis/polynomial/IrwinHall.cpp | 71 ++++--- gtsam/basis/polynomial/IrwinHall.h | 25 ++- gtsam/basis/polynomial/IrwinHallCDF.cpp | 92 +++++++++ gtsam/basis/polynomial/PiecewisePolynomial.h | 105 ++++++++++ gtsam/basis/polynomial/kernels.h | 114 +---------- gtsam/basis/tests/testIrwinHall.cpp | 60 +++--- gtsam/basis/tests/testIrwinHallCDF.cpp | 184 ++++++++++++++++++ gtsam/basis/tests/testPiecewisePolynomial.cpp | 67 +++++++ 8 files changed, 539 insertions(+), 179 deletions(-) create mode 100644 gtsam/basis/polynomial/IrwinHallCDF.cpp create mode 100644 gtsam/basis/polynomial/PiecewisePolynomial.h create mode 100644 gtsam/basis/tests/testIrwinHallCDF.cpp create mode 100644 gtsam/basis/tests/testPiecewisePolynomial.cpp diff --git a/gtsam/basis/polynomial/IrwinHall.cpp b/gtsam/basis/polynomial/IrwinHall.cpp index 553312781a..b228209902 100644 --- a/gtsam/basis/polynomial/IrwinHall.cpp +++ b/gtsam/basis/polynomial/IrwinHall.cpp @@ -8,67 +8,62 @@ namespace kernels { Irwin Hall coefficients provide canonical spline basis functions https://oeis.org/A188816 -Mathematica: -f[n_, k_] := - f[n, k] = Sum[(-1)^j Binomial[n, j] (x - j)^(n - 1), {j, 0, k}]; -T[n_, k_] := Table[Coefficient[f[n, k], x, t], {t, 0, n - 1}]; -Table[Table[T[n, k]/((n - 1)!), {k, 0, n - 1}], {n, 1, 7}] */ -const piecewise_polynomial<1,1> IrwinHall0({ +const piecewise_polynomial<0,1> IrwinHall0({ {1.}, {0.,1.}, 1./2. }); -const piecewise_polynomial<2,2> IrwinHall1({ - { 0., 1., +const piecewise_polynomial<1,2> IrwinHall1({ + { 0., 1., 2., -1.}, {0.,1.,2.}, 1. }); -const piecewise_polynomial<3,3> IrwinHall2({ - { 0., 0., 1./2., - -(3./2.), 3., -1., - 9./2., -3., 1./2.}, +const piecewise_polynomial<2,3> IrwinHall2({ + { 0. , 0. , 1./2., + -3./2., 6./2., -2./2., + 9./2., -6./2., 1./2.}, {0.,1.,2.,3.}, 3./2. }); // this captures cubic splines -const piecewise_polynomial<4,4> IrwinHall3({ - { 0., 0., 0., 1./6., - 2./3., -2., 2., -(1./2.), - -(22./3.), 10., -4., 1./2., - 32./3., -8., 2., -(1/6.)}, +const piecewise_polynomial<3,4> IrwinHall3({ + { 0. , 0., 0., 1./6., + 2./3., -2., 2., -1./2., + -22./3., 10., -4., 1./2., + 32./3., -8., 2., -1./6.}, {0.,1.,2.,3.,4.}, 2. }); -const piecewise_polynomial<5,5> IrwinHall4({ - { 0., 0., 0., 0., 1./24., - -(5./24.), 5./6., -(5./4.), 5./6., -(1./6.), - 155./24., -(25./2.), 35./4., -(5./2.), 1./4., - -(655./24.), 65./2., -(55./4.), 5./2., -(1./6.), - 625./24., -(125./6.), 25./4., -(5./6.), 1./24.}, +const piecewise_polynomial<4,5> IrwinHall4({ + { 0. , 0. , 0. , 0. , 1./24., + -5./24., 5./6., -5./4., 5./6., -1./6. , + 155./24., -25./2., 35./4., -5./2., 1./4. , + -655./24., 65./2., -55./4., 5./2., -1./6. , + 625./24., -125./6., 25./4., -5./6., 1./24.}, {0.,1.,2.,3.,4.,5.}, 5./2. }); -const piecewise_polynomial<6,6> IrwinHall5({ - { 0., 0., 0., 0., 0., 1./120., - 1./20., -(1./4.), 1./2., -(1./2.), 1./4., -(1./24.), - -(79./20.), 39./4., -(19./2.), 9./2., -1., 1./12., - 731./20., -(231./4.), 71./2., -(21./2.), 3./2., -(1./12.), - -(1829./20.), 409./4., -(89./2.), 19./2., -1., 1./24., - 324./5., -54., 18., -3., 1/4., -(1./120.)}, +const piecewise_polynomial<5,6> IrwinHall5({ + { 0. , 0. , 0. , 0. , 0. , 1./120., + 1./20., -1./4., 1./2., -1./2., 1./4., -1./24. , + -79./20., 39./4., -19./2., 9./2., -1. , 1./12. , + 731./20., -231./4., 71./2., -21./2., 3./2., -1./12. , + -1829./20., 409./4., -89./2., 19./2., -1. , 1./24. , + 324./5. , -54. , 18. , -3. , 1./4., -1./120.}, {0.,1.,2.,3.,4.,5.,6.}, 3. }); -const piecewise_polynomial<7,7> IrwinHall6({ - { 0., 0., 0., 0., 0., 0., 1./720., - -(7./720.), 7./120., -(7./48.), 7./36., -(7./48.), 7./120., -(1./120.), - 1337./720., -(133./24.), 329./48., -(161./36.), 77./48., -(7./24.), 1./48., - -(12089./360.), 196./3., -(1253./24.), 196./9., -(119./24.), 7./12., -(1./36.), - 59591./360., -(700./3.), 3227./24., -(364./9.), 161./24., -(7./12.), 1./48., - -(208943./720.), 7525./24., -(6671./48.), 1169./36., -(203./48.), 7./24., -(1./120.), - 117649./720., -(16807./120.), 2401./48., -(343./36.), 49./48., -(7./120.), 1./720.}, +const piecewise_polynomial<6,7> IrwinHall6({ + { 0. , 0. , 0. , 0. , 0. , 0. , 1./720., + -7./720., 7./120., -7./48., 7./36., -7./48., 7./120., -1./120., + 1337./720., -133./24. , 329./48., -161./36., 77./48., -7./24. , 1./48. , + -12089./360., 196./3. , -1253./24., 196./9. , -119./24., 7./12. , -1./36. , + 59591./360., -700./3. , 3227./24., -364./9. , 161./24., -7./12. , 1./48. , + -208943./720., 7525./24. , -6671./48., 1169./36., -203./48., 7./24. , -1./120., + 117649./720., -16807./120., 2401./48., -343./36., 49./48., -7./120., 1./720.}, {0.,1.,2.,3.,4.,5.,6.,7.}, 7./2. }); diff --git a/gtsam/basis/polynomial/IrwinHall.h b/gtsam/basis/polynomial/IrwinHall.h index 9106a1e4e2..7720fb655b 100644 --- a/gtsam/basis/polynomial/IrwinHall.h +++ b/gtsam/basis/polynomial/IrwinHall.h @@ -1,6 +1,6 @@ #pragma once -#include "kernels.h" +#include "PiecewisePolynomial.h" namespace gtsam { namespace kernels { @@ -39,13 +39,22 @@ High frequencies can be attenuated by processing the control points under a shar */ -extern const piecewise_polynomial<1,1> IrwinHall0; -extern const piecewise_polynomial<2,2> IrwinHall1; -extern const piecewise_polynomial<3,3> IrwinHall2; -extern const piecewise_polynomial<4,4> IrwinHall3; // equivalent to cubic spline -extern const piecewise_polynomial<5,5> IrwinHall4; -extern const piecewise_polynomial<6,6> IrwinHall5; -extern const piecewise_polynomial<7,7> IrwinHall6; +extern const piecewise_polynomial<0,1> IrwinHall0; // equivalent to a linear interpolator +extern const piecewise_polynomial<1,2> IrwinHall1; +extern const piecewise_polynomial<2,3> IrwinHall2; +extern const piecewise_polynomial<3,4> IrwinHall3; // equivalent to cubic spline +extern const piecewise_polynomial<4,5> IrwinHall4; +extern const piecewise_polynomial<5,6> IrwinHall5; +extern const piecewise_polynomial<6,7> IrwinHall6; + + +extern const piecewise_polynomial<1,1> IrwinHallCDF0; // equivalent to a linear interpolator +extern const piecewise_polynomial<2,2> IrwinHallCDF1; +extern const piecewise_polynomial<3,3> IrwinHallCDF2; +extern const piecewise_polynomial<4,4> IrwinHallCDF3; // equivalent to cubic spline +extern const piecewise_polynomial<5,5> IrwinHallCDF4; +extern const piecewise_polynomial<6,6> IrwinHallCDF5; +extern const piecewise_polynomial<7,7> IrwinHallCDF6; } // namespace kernels } // namespace gtsam \ No newline at end of file diff --git a/gtsam/basis/polynomial/IrwinHallCDF.cpp b/gtsam/basis/polynomial/IrwinHallCDF.cpp new file mode 100644 index 0000000000..86bf3f7c74 --- /dev/null +++ b/gtsam/basis/polynomial/IrwinHallCDF.cpp @@ -0,0 +1,92 @@ + +#include "IrwinHall.h" +namespace gtsam { + +namespace kernels { + + +/* + +Irwin Hall Cumulative Distribution allows efficient cannonical spline evaluation on Lie groups. +https://oeis.org/A188668 + +*/ + + +// IrwinHallCDF0 is a linear interpolator with a domain [0-1] +const piecewise_polynomial<1,1> IrwinHallCDF0({ + {0., 1.}, + {0., 1.}, // intervals + 1./2. // centre +}); + + +const piecewise_polynomial<2,2> IrwinHallCDF1({ + { 0. , 0. , 1./2., + -2./2., 4./2., -1./2.}, + {0.,1.,2.}, // intervals + 1. // centre +}); + + +const piecewise_polynomial<3,3> IrwinHallCDF2({ + { 0. , 0. , 0. , 1./6., + 3./6., -9./6., 9./6., -2./6., + -21./6., 27./6., -9./6., 1./6.}, + {0.,1.,2.,3.}, + 3./2. +}); + +// IrwinHallCDF3 captures cubic splines +const piecewise_polynomial<4,4> IrwinHallCDF3({ + { 0. , 0. , 0. , 0. , 1./24., + -4./24., 16./24., -24./24., 16./24., -3./24., + 92./24., -176./24., 120./24., -32./24., 3./24., + -232./24., 256./24., -96./24., 16./24., -1./24.}, + {0.,1.,2.,3.,4.}, + 2. +}); + + +const piecewise_polynomial<5,5> IrwinHallCDF4({ + { 0. , 0. , 0. , 0., 0. , 1./120., + 5./120., -25./120., 50./120., -50./120., 25./120., -4./120., + -315./120., 775./120., -750./120., 350./120., -75./120., 6./120., + 2115./120., -3275./120., 1950./120., -550./120., 75./120., -4./120., + -3005./120., 3125./120., -1250./120., 250./120., -25./120., 1./120.}, + {0.,1.,2.,3.,4.,5.}, + 5./2. +}); + + +const piecewise_polynomial<6,6> IrwinHallCDF5({ + { 0. , 0. , 0. , 0. , 0. , 0. , 1./720, + -6./720, 36./720, -90./720, 120./720, -90./720, 36./720, -5./720, + 954./720, -2844./720, 3510./720, -2280./720, 810./720, -144./720, 10./720, + -13626./720, 26316./720, -20790./720, 8520./720, -1890./720, 216./720, -10./720, + 47814./720, -65844./720, 36810./720, -10680./720, 1710./720, -144./720, 5./720, + -45936./720, 46656./720, -19440./720, 4320./720, -540./720, 36./720, -1./720}, + {0.,1.,2.,3.,4.,5.,6.}, + 3. +}); + + +const piecewise_polynomial<7,7> IrwinHallCDF6({ + { 0. , 0. , 0. , 0. , 0. , 0. , 0. , 1./5040., + 7./5040., -49./5040., 147./5040., -245./5040., 245./5040., -147./5040., 49./5040., -6./5040., + -2681./5040., 9359./5040., -13965./5040., 11515./5040., -5635./5040., 1617./5040., -245./5040., 15./5040., + 73864./5040., -169246./5040., 164640./5040., -87710./5040., 27440./5040., -4998./5040., 490./5040., -20./5040., + -499576./5040., 834274./5040., -588000./5040., 225890./5040., -50960./5040., 6762./5040., -490./5040., 15./5040., + 1141049./5040., -1462601./5040., 790125./5040., -233485./5040., 40915./5040., -4263./5040., 245./5040., -6./5040., + -818503./5040., 823543./5040., -352947./5040., 84035./5040., -12005./5040., 1029./5040., -49./5040., 1./5040.}, + {0.,1.,2.,3.,4.,5.,6.,7.}, + 7./2. +}); + +// beyond IrwinHallCDF6 is so close to a gaussian it hardly matters any more. + + + +} // namespace kernels + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/basis/polynomial/PiecewisePolynomial.h b/gtsam/basis/polynomial/PiecewisePolynomial.h new file mode 100644 index 0000000000..1c569f205b --- /dev/null +++ b/gtsam/basis/polynomial/PiecewisePolynomial.h @@ -0,0 +1,105 @@ +#pragma once + +#include + +namespace gtsam { + + + +template +class piecewise_polynomial : public kernel_base +{ +public: + inline constexpr static auto order = O; + inline constexpr static auto pieces = P; + +protected: + + struct param_s{ + std::array, pieces > coefficients; + std::array intervals; + double center; + }; + const param_s params; + + +public: + piecewise_polynomial(param_s init_list):params(init_list){} + + double get_center() const override + { + return params.center; + } + double get_beginning() const override + { + return params.intervals[0]; + } + + double get_end() const override + { + return params.intervals[pieces]; + } + + std::array get_intervals() const { + return params.intervals; + } + +/** + evaluates the continuous kernel function at time t + + derivative: how many times to differentiate the kernel function + t: the time to evaluate it relative to the kernel's datum + H: the jacobian for gtsam (the next derivative, calls this function again.) + + */ + double evaluate(const double& t, OptionalJacobian<1, 1> H = {}) const override + { + return evaluate_d(0, t, H); + } + + double evaluate_d(size_t derivative, double t, OptionalJacobian<1, 1> H = {}) const + { + // bounds check + if(t < params.intervals[0]) + { + t = params.intervals[0]; + } + if(t > params.intervals[pieces]) + { + t = params.intervals[pieces]; + } + + // locate the interval + for(size_t p = 0; p < pieces; p++) + { + if(t<=params.intervals[p+1]) + { + double t_power = 1; // powers of t + double coeff = 0; // kernel coefficient result + // iterate through the order of the polynomial + for(size_t o = derivative; o < order+1; o++) + { + // power rule on repeated derivatives is halfway factorial + // o! / (o-derivative)! + size_t power_rule = 1; + for(size_t d=0; d -class piecewise_polynomial : public kernel_base -{ -public: - inline constexpr static auto order = O; - inline constexpr static auto pieces = P; - -protected: - - struct param_s{ - std::array, order > coefficients; - std::array intervals; - double center; - }; - const param_s params; - - -public: - piecewise_polynomial(param_s init_list):params(init_list){} - - double get_center() const override - { - return params.center; - } - double get_beginning() const override - { - return params.intervals[0]; - } - double get_end() const override - { - return params.intervals[pieces]; - } + // generates a vector of Expressions that will sample the kernel's basis function at `timestamp` during optimisation. + std::vector sample_kernel( + const Double_& timestamp, + size_t start, size_t end + ); - std::array get_intervals() const { - return params.intervals; - } - -/** - evaluates the continuous kernel function at time t - - derivative: how many times to differentiate the kernel function - t: the time to evaluate it relative to the kernel's datum - H: the jacobian for gtsam (the next derivative, calls this function again.) - - */ - double evaluate(const double& t, OptionalJacobian<1, 1> H = {}) const override - { - return evaluate_d(0, t, H); - } - - double evaluate_d(size_t derivative, const double& t, OptionalJacobian<1, 1> H = {}) const - { - // quick bounds check - if(tparams.intervals[pieces]) - { - if(H) *H << 0; - return 0; - } - // locate the interval - for(size_t p = 0; p < pieces; p++) - { - if(t -T inverse_filter(double timestamp, const std::vector>& time_series) -{ - -// error is interpolate(control_points @ timestamp) -// least sum of squares ( kinda O(N^2) over the order of polynomial) -// - -// weighted_sum({ - // kernel(cp_time) * control_point - //}) - return T(); // XXX - -} -*/ -// XXX a collection of inverse kernels for the spline kernels, -// defined within some frequency bounds. -// XXX Consider using an acausal Butterworth or Chebyshev filter instead, -// this provides a 1:1 correspondence between the control points and the trajectory points -// and (mostly) eliminates the need for an inverse filter } // namespace gtsam \ No newline at end of file diff --git a/gtsam/basis/tests/testIrwinHall.cpp b/gtsam/basis/tests/testIrwinHall.cpp index 126505d9e5..e43aee21e7 100644 --- a/gtsam/basis/tests/testIrwinHall.cpp +++ b/gtsam/basis/tests/testIrwinHall.cpp @@ -13,7 +13,6 @@ #include #include #include -#include #include @@ -25,6 +24,42 @@ using namespace gtsam::kernels; double epsilon = 1e-9; double tolerance = 1e-7; + + +// test boundary conditions at the limits of the domain +TEST( poly , Boundaries1 ) { + auto poly = IrwinHall1; + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().back()), tolerance); +} +TEST( poly , Boundaries2 ) { + auto poly = IrwinHall2; + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().back()), tolerance); +} +TEST( poly , Boundaries3 ) { + auto poly = IrwinHall3; + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().back()), tolerance); +} +TEST( poly , Boundaries4 ) { + auto poly = IrwinHall4; + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().back()), tolerance); +} +TEST( poly , Boundaries5 ) { + auto poly = IrwinHall5; + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().back()), tolerance); +} +TEST( poly , Boundaries6 ) { + auto poly = IrwinHall6; + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().back()), tolerance); +} + + + // test continuity where the polynomial pieces join TEST( IrwinHall , Continuity1 ) { auto poly = IrwinHall1; @@ -132,26 +167,3 @@ TEST( IrwinHall , DerivativeContinuity6 ) { } } - -// test jacobian of evaluate against the next derivative -TEST( IrwinHall , DerivativeIsJacobian ) { - auto poly = IrwinHall6; - int max_d = poly.order-2; - for(const double &t : poly.get_intervals()) { - for(int d=0; d jacobian; - poly.evaluate_d(d, t, &jacobian); - double derivative = poly.evaluate_d(d+1, t); - EXPECT_DOUBLES_EQUAL(jacobian[0], derivative, tolerance ); - } - } -} - - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ - diff --git a/gtsam/basis/tests/testIrwinHallCDF.cpp b/gtsam/basis/tests/testIrwinHallCDF.cpp new file mode 100644 index 0000000000..e85e1690c8 --- /dev/null +++ b/gtsam/basis/tests/testIrwinHallCDF.cpp @@ -0,0 +1,184 @@ +/** + * @file TestIrwinHall.cpp + * @brief validate Irwin Hall coefficients and derivatives + * @author Brett Downing + * @date August 2025 + */ + + + +#include +#include +#include +#include +#include +#include +#include + + +using namespace gtsam; +using namespace gtsam::kernels; + +// The summation of so many terms does get noisy, +// so split the epsilon and the tolerance +double epsilon = 1e-9; +double tolerance = 1e-7; + + + + +// test boundary conditions for cumulative distribution +TEST( IrwinHallCDF , Boundaries1 ) { + auto poly = IrwinHallCDF1; + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 1.0, poly.evaluate(poly.get_intervals().back()), tolerance); +} +TEST( IrwinHallCDF , Boundaries2 ) { + auto poly = IrwinHallCDF2; + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 1.0, poly.evaluate(poly.get_intervals().back()), tolerance); +} +TEST( IrwinHallCDF , Boundaries3 ) { + auto poly = IrwinHallCDF3; + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 1.0, poly.evaluate(poly.get_intervals().back()), tolerance); +} +TEST( IrwinHallCDF , Boundaries4 ) { + auto poly = IrwinHallCDF4; + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 1.0, poly.evaluate(poly.get_intervals().back()), tolerance); +} +TEST( IrwinHallCDF , Boundaries5 ) { + auto poly = IrwinHallCDF5; + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 1.0, poly.evaluate(poly.get_intervals().back()), tolerance); +} +TEST( IrwinHallCDF , Boundaries6 ) { + auto poly = IrwinHallCDF6; + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 1.0, poly.evaluate(poly.get_intervals().back()), tolerance); +} + + + + +// test continuity where the polynomial pieces join +TEST( IrwinHallCDF , Continuity1 ) { + auto poly = IrwinHallCDF1; + for(const double &t : poly.get_intervals()) { + EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), + poly.evaluate(t-epsilon), tolerance ); + } +} +TEST( IrwinHallCDF , Continuity2 ) { + auto poly = IrwinHallCDF2; + for(const double &t : poly.get_intervals()) { + EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), + poly.evaluate(t-epsilon), tolerance ); + } +} +TEST( IrwinHallCDF , Continuity3 ) { + auto poly = IrwinHallCDF3; + for(const double &t : poly.get_intervals()) { + EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), + poly.evaluate(t-epsilon), tolerance ); + } +} +TEST( IrwinHallCDF , Continuity4 ) { + auto poly = IrwinHallCDF4; + for(const double &t : poly.get_intervals()) { + EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), + poly.evaluate(t-epsilon), tolerance ); + } +} +TEST( IrwinHallCDF , Continuity5 ) { + auto poly = IrwinHallCDF5; + for(const double &t : poly.get_intervals()) { + EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), + poly.evaluate(t-epsilon), tolerance ); + } +} +TEST( IrwinHallCDF , Continuity6 ) { + auto poly = IrwinHallCDF6; + for(const double &t : poly.get_intervals()) { + EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), + poly.evaluate(t-epsilon), tolerance ); + } +} + + +// test continuity of derivatives where the polynomial pieces join +TEST( IrwinHallCDF , DerivativeContinuity1 ) { + auto poly = IrwinHallCDF1; + int max_d = poly.order-1; + for(const double &t : poly.get_intervals()) { + for(int d=0; d +#include +#include +#include +#include +#include +#include +#include + + +using namespace gtsam; +using namespace gtsam::kernels; + +// The summation of so many terms does get noisy, +// so split the epsilon and the tolerance +double epsilon = 1e-9; +double tolerance = 1e-7; + + + + +// test the jacobian of evaluate is the same as evaluate at the next derivative, for every valid derivative +TEST( IrwinHall , DerivativeIsJacobian1 ) { + auto poly = IrwinHall6; + int max_d = poly.order-2; + for(const double &t : poly.get_intervals()) { + for(int d=0; d jacobian; + poly.evaluate_d(d, t, &jacobian); + double derivative = poly.evaluate_d(d+1, t); + EXPECT_DOUBLES_EQUAL(jacobian[0], derivative, tolerance ); + } + } +} + +// test jacobian of evaluate against the next derivative +TEST( IrwinHallCDF , DerivativeIsJacobian2 ) { + auto poly = IrwinHallCDF6; + int max_d = poly.order-2; + for(const double &t : poly.get_intervals()) { + for(int d=0; d jacobian; + poly.evaluate_d(d, t, &jacobian); + double derivative = poly.evaluate_d(d+1, t); + EXPECT_DOUBLES_EQUAL(jacobian[0], derivative, tolerance ); + } + } +} + + + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + From bbf6d5438da7019a2663c280710dfbe1ab7bd80b Mon Sep 17 00:00:00 2001 From: Brett Downing Date: Thu, 7 Aug 2025 15:07:35 +1000 Subject: [PATCH 04/16] clean up and adds cumulative spline trajectory --- gtsam/basis/polynomial/IrwinHall.cpp | 8 +- gtsam/basis/polynomial/IrwinHall.h | 49 ++---- gtsam/basis/polynomial/IrwinHallCDF.cpp | 32 +--- gtsam/basis/polynomial/TrajectoryModel.h | 202 +++++++++++++++++++++++ gtsam/basis/polynomial/kernels.h | 8 - gtsam/slam/expressions.h | 54 ++++++ 6 files changed, 277 insertions(+), 76 deletions(-) create mode 100644 gtsam/basis/polynomial/TrajectoryModel.h diff --git a/gtsam/basis/polynomial/IrwinHall.cpp b/gtsam/basis/polynomial/IrwinHall.cpp index b228209902..210a03640b 100644 --- a/gtsam/basis/polynomial/IrwinHall.cpp +++ b/gtsam/basis/polynomial/IrwinHall.cpp @@ -1,15 +1,13 @@ #include "IrwinHall.h" namespace gtsam { - namespace kernels { /* -Irwin Hall coefficients provide canonical spline basis functions +Irwin Hall coefficients https://oeis.org/A188816 - - */ + const piecewise_polynomial<0,1> IrwinHall0({ {1.}, {0.,1.}, @@ -28,7 +26,6 @@ const piecewise_polynomial<2,3> IrwinHall2({ {0.,1.,2.,3.}, 3./2. }); -// this captures cubic splines const piecewise_polynomial<3,4> IrwinHall3({ { 0. , 0., 0., 1./6., 2./3., -2., 2., -1./2., @@ -69,5 +66,4 @@ const piecewise_polynomial<6,7> IrwinHall6({ }); } // namespace kernels - } // namespace gtsam \ No newline at end of file diff --git a/gtsam/basis/polynomial/IrwinHall.h b/gtsam/basis/polynomial/IrwinHall.h index 7720fb655b..dd5fa57208 100644 --- a/gtsam/basis/polynomial/IrwinHall.h +++ b/gtsam/basis/polynomial/IrwinHall.h @@ -6,52 +6,31 @@ namespace gtsam { namespace kernels { /* -Irwin Hall coefficients provide canonical spline basis functions -These functions are piecewise-defined polynomials with continuity up to their (N-1)th derivative -Irwin Hall 0 is a zeroth order spline (a discontinuous staircase) -Irwin Hall 3 is a cubic polynomial, so it has continuous (piecewise linear) accelerations - -https://oeis.org/A188816 - -Mathematica: -f[n_, k_] := - f[n, k] = Sum[(-1)^j Binomial[n, j] (x - j)^(n - 1), {j, 0, k}]; -T[n_, k_] := Table[Coefficient[f[n, k], x, t], {t, 0, n - 1}]; -Table[Table[T[n, k]/((n - 1)!), {k, 0, n - 1}], {n, 1, 7}] - +Irwin Hall coefficients are used by TrajectoryModel to implement + canonical (monotonic) spline basis functions. +These functions are piecewise-defined polynomials continuous up to the (N-1)th derivative +Irwin Hall CDF 0 is a zeroth order spline (linear interpolation) +Irwin Hall CDF 2 is a cubic polynomial, so it has linear accelerations This series of fucntions converges pretty quickly to a gaussian. -If you need higher orders (N), you can use a gaussian kernel directly +If you need higher orders (N), you can use a gaussian kernel and CDF directly e^(-(1/2)((x-N/2)/(0.3*N/2)^2 ) -or defined in the fourier domain, cardinal splines are defined as sinc^N, and gaussians remain gaussian: - 'sinc to the n' has the same form as a gaussian with a simple domain transformation - e^(-0.5(w^2) * N/3) ~= sinc^N(w) - -the derivatives of a gaussian depend on Hermite polynomials which are a little complicated - -The inverse filter (1/sinc^N) for this explodes pretty fast at higher frequencies, - so filters beyond order 4 should step back a long way (up to an order of magnitude) from the nyquist limit - in order to avoid the vector sum crossing the edge of the Lie Algebra's Map - -High frequencies can be attenuated by processing the control points under a sharp - high-pass filter and passing factors that bais the result to zero. - The high-pass factors form a 'penalty' term under a generalisation over P-Splines. - - */ -extern const piecewise_polynomial<0,1> IrwinHall0; // equivalent to a linear interpolator + +// https://oeis.org/A188816 +extern const piecewise_polynomial<0,1> IrwinHall0; extern const piecewise_polynomial<1,2> IrwinHall1; extern const piecewise_polynomial<2,3> IrwinHall2; -extern const piecewise_polynomial<3,4> IrwinHall3; // equivalent to cubic spline +extern const piecewise_polynomial<3,4> IrwinHall3; extern const piecewise_polynomial<4,5> IrwinHall4; extern const piecewise_polynomial<5,6> IrwinHall5; extern const piecewise_polynomial<6,7> IrwinHall6; - -extern const piecewise_polynomial<1,1> IrwinHallCDF0; // equivalent to a linear interpolator +// https://oeis.org/A188668 +extern const piecewise_polynomial<1,1> IrwinHallCDF0; // produces a linear interpolator extern const piecewise_polynomial<2,2> IrwinHallCDF1; -extern const piecewise_polynomial<3,3> IrwinHallCDF2; -extern const piecewise_polynomial<4,4> IrwinHallCDF3; // equivalent to cubic spline +extern const piecewise_polynomial<3,3> IrwinHallCDF2; // produces a cubic spline +extern const piecewise_polynomial<4,4> IrwinHallCDF3; extern const piecewise_polynomial<5,5> IrwinHallCDF4; extern const piecewise_polynomial<6,6> IrwinHallCDF5; extern const piecewise_polynomial<7,7> IrwinHallCDF6; diff --git a/gtsam/basis/polynomial/IrwinHallCDF.cpp b/gtsam/basis/polynomial/IrwinHallCDF.cpp index 86bf3f7c74..78bb77cc3e 100644 --- a/gtsam/basis/polynomial/IrwinHallCDF.cpp +++ b/gtsam/basis/polynomial/IrwinHallCDF.cpp @@ -1,34 +1,24 @@ #include "IrwinHall.h" namespace gtsam { - namespace kernels { - /* - -Irwin Hall Cumulative Distribution allows efficient cannonical spline evaluation on Lie groups. +Irwin Hall Cumulative Distribution Function Coefficients https://oeis.org/A188668 - */ - -// IrwinHallCDF0 is a linear interpolator with a domain [0-1] const piecewise_polynomial<1,1> IrwinHallCDF0({ {0., 1.}, - {0., 1.}, // intervals - 1./2. // centre + {0., 1.}, + 1./2. }); - - const piecewise_polynomial<2,2> IrwinHallCDF1({ { 0. , 0. , 1./2., -2./2., 4./2., -1./2.}, - {0.,1.,2.}, // intervals - 1. // centre + {0.,1.,2.}, + 1. }); - - const piecewise_polynomial<3,3> IrwinHallCDF2({ { 0. , 0. , 0. , 1./6., 3./6., -9./6., 9./6., -2./6., @@ -37,7 +27,6 @@ const piecewise_polynomial<3,3> IrwinHallCDF2({ 3./2. }); -// IrwinHallCDF3 captures cubic splines const piecewise_polynomial<4,4> IrwinHallCDF3({ { 0. , 0. , 0. , 0. , 1./24., -4./24., 16./24., -24./24., 16./24., -3./24., @@ -46,8 +35,6 @@ const piecewise_polynomial<4,4> IrwinHallCDF3({ {0.,1.,2.,3.,4.}, 2. }); - - const piecewise_polynomial<5,5> IrwinHallCDF4({ { 0. , 0. , 0. , 0., 0. , 1./120., 5./120., -25./120., 50./120., -50./120., 25./120., -4./120., @@ -57,8 +44,6 @@ const piecewise_polynomial<5,5> IrwinHallCDF4({ {0.,1.,2.,3.,4.,5.}, 5./2. }); - - const piecewise_polynomial<6,6> IrwinHallCDF5({ { 0. , 0. , 0. , 0. , 0. , 0. , 1./720, -6./720, 36./720, -90./720, 120./720, -90./720, 36./720, -5./720, @@ -69,8 +54,6 @@ const piecewise_polynomial<6,6> IrwinHallCDF5({ {0.,1.,2.,3.,4.,5.,6.}, 3. }); - - const piecewise_polynomial<7,7> IrwinHallCDF6({ { 0. , 0. , 0. , 0. , 0. , 0. , 0. , 1./5040., 7./5040., -49./5040., 147./5040., -245./5040., 245./5040., -147./5040., 49./5040., -6./5040., @@ -83,10 +66,5 @@ const piecewise_polynomial<7,7> IrwinHallCDF6({ 7./2. }); -// beyond IrwinHallCDF6 is so close to a gaussian it hardly matters any more. - - - } // namespace kernels - } // namespace gtsam \ No newline at end of file diff --git a/gtsam/basis/polynomial/TrajectoryModel.h b/gtsam/basis/polynomial/TrajectoryModel.h new file mode 100644 index 0000000000..9a03af3f95 --- /dev/null +++ b/gtsam/basis/polynomial/TrajectoryModel.h @@ -0,0 +1,202 @@ +#pragma once + +#include "PiecewisePolynomial.h" + + + +/* +This trajectory model uses a lie-algebra accumulation form described in: +https://openaccess.thecvf.com/content_CVPR_2020/papers/Sommer_Efficient_Derivative_Computation_for_Cumulative_B-Splines_on_Lie_Groups_CVPR_2020_paper.pdf + +In the special case of spines with uniformly-spaced control points ('canonical splines'), + the basis funcitons reduce down to the Irwin Hall CDF. + +This formulation is equivalent to a describing the trajactory as a convolution + between the control points and the CDF. + +Conceptually this is like to taking a numerical derivative in the Lie Algebra, + then encoding an integral as a (very) long FIR filter to recover the poses. + this leaves the actual FIR kernel up to the user + points x kernel + points x kernel x derivative x integral + (points x derivative) x kernel x integral + (derivative(points) x kernel) x integral + derivative(points) x integral(kernel) +This convolution form clarifies the need for a time-reversal applied to the integrated kernel + +When used with a time-reversed integral of an impulse response, + this replicates the behaviour of a FIR filter. +using an Irwin Hall probability density function as the impulse, + replicates cardinal splines. + +*/ + +namespace gtsam { + +template +class TrajectoryModel +{ + const kernel_base& kernel; + std::vector> points; +public: + +/** + Sample the trajectory model at a given timestamp + window_start and window_end are used to limit the number of control points + considered in the resulting Expression. +*/ + Expression sample_trajectory( + Double_ timestamp, + double window_start, + double window_end, + ); + +private: + + /** + interpolate calls sample_kernel and weighted_sum to apply the kernel over subset of the control points. + the return type is an Expression that depends on points and timestamp, + performing an automatically differentiated kernel convolution at linearisation time. + */ + Expression kernel_interpolate( + const kernel_base& kernel, // the kernel to use for interpolation + const Double_& timestamp, // the timestamp to interpolate at + const std::vector>& points, // a reference to the control points to interpolate + size_t start, size_t end // the range of control points to interpolate + ); + + + /** + sample the kernel at a range of points + timestamp is is in units of samples. Be sure to divide your timestamp by the sample rate + start and end are indicies into (and exact timestamps) the vector of control points. + */ + std::vector sample_kernel( + const kernel_base& kernel, + const Double_& timestamp, + size_t start, size_t end); + + + /** + accumulates the log-space differences between points, + with each log-space vector scaled by its respective weight. + */ + Expression cumulative_path_sum( + const std::vector>& points, + const std::vector& cdf_weights); + +}; + + + +/** + Sample the trajectory model at a given timestamp + window_start and window_end are used to limit the number of control points + considered in the resulting Expression. +*/ +template +Expression TrajectoryModel::sample_trajectory( + Double_ timestamp, + double window_start = 0, + double window_end = -1, +) +{ + size_t start = floor(window_start); + size_t end = floor(window_end); + if(window_start<0) + { + start = 0; + } + if(window_end < 0 || window_end > points.size()) + { + end = points.size(); + } + + return kernel_interpolate( + kernel, + timestamp, + points, + start, end); + +} + + + +template +Expression TrajectoryModel::cumulative_path_sum( + const std::vector>& points, + const std::vector& cdf_weights +){ + Expression::TangentVector> v = typename traits::TangentVector::Zero(); + // compute logspace vectors between adjacent points, + // scale the vectors by the CDF based weight for the relevant point + // accumulate the logspace path as a relative vector from the first point + for (size_t i = 0; i < points.size()-1; i++) + { + v += cdf_weights[i] * logmap(points[i], points[i+1]); + } + // apply the logspace vector to the first point + return expmap(points[0], v); +} + + +/** +interpolate calls sample_kernel and weighted_sum to apply the kernel over subset of the control points. + the return type is an Expression that depends on points and timestamp, + performing an automatically differentiated kernel convolution at linearisation time. +*/ +template +Expression TrajectoryModel::kernel_interpolate( + const kernel_base& kernel, // the kernel to use for interpolation + const Double_& timestamp, // the timestamp to interpolate at + const std::vector>& points, // a reference to the control points to interpolate + size_t start, size_t end // the range of control points to interpolate +) +{ + // copy the interval of control points that we want to interpolate within + std::vector> points_range(points.begin()+start, points.begin()+end); + + Double_ span_time = timestamp - Double_(double(start)); + // time-reverse the cdf + Double_ kernel_time = Double_(kernel.get_end()) - span_time + std::vector weights_range = kernel.sample_kernel(timestamp, start, end); + return cumulative_path_sum(points_range, weights_range); +} + + + + +/** +sample the kernel at a range of points +timestamp is is in units of samples. Be sure to divide your timestamp by the sample rate +start and end are indicies into (and exact timestamps) the vector of control points. +*/ +template +std::vector TrajectoryModel::sample_kernel( + const kernel_base& kernel, + const Double_& timestamp, + size_t start, size_t end) +{ + // expression constructor requires us to bind to the class member method + std::function)> keval = + [&](const double& x, OptionalJacobian<1, 1> j) {return kernel.evaluate(x, j);}; + std::vector vec; + for(size_t i = start; i < end; i++) + { + Double_ kernel_time = + Double_(get_end()) + + (timestamp - Double_(double(start))) + - Double_(double(i)); + vec.push_back(Double_(keval, kernel_time)); + } + return vec; +} + + + + + + +} // namespace gtsam + + diff --git a/gtsam/basis/polynomial/kernels.h b/gtsam/basis/polynomial/kernels.h index ee431228be..cfb8eec37f 100644 --- a/gtsam/basis/polynomial/kernels.h +++ b/gtsam/basis/polynomial/kernels.h @@ -24,14 +24,6 @@ class kernel_base virtual ~kernel_base() = default; - - // generates a vector of Expressions that will sample the kernel's basis function at `timestamp` during optimisation. - std::vector sample_kernel( - const Double_& timestamp, - size_t start, size_t end - ); - - }; diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index 5a65810aaf..2252b994b8 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -192,6 +192,7 @@ inline Pose3_ getPose(const Expression > & cam) { // x1, &T::logmap, x2); // } + template gtsam::Expression::TangentVector> logmap( const gtsam::Expression &x1, const gtsam::Expression &x2) { @@ -199,6 +200,14 @@ gtsam::Expression::TangentVector> logmap( gtsam::traits::Logmap, between(x1, x2)); } + +template +Expression expmap(const Expression &x, + const Expression::TangentVector> &v){ + return compose(x, Expression(&traits::Expmap, v)); +} + + template inline Expression interpolate(const Expression& p, const Expression& q, const Expression& t){ @@ -209,4 +218,49 @@ inline Expression interpolate(const Expression& p, const Expression& q, return Expression(f, p, q, t); } + + +// Syntax sugar +namespace internal { + // multiply by scalar + template + T scale_vec( + double k, + T x, + typename MakeOptionalJacobian::type Hk, + typename MakeOptionalJacobian::type Hx + ) + { + if(Hk) *Hk << x; + if(Hx) *Hx << k * MakeJacobian::type::Identity(); + return k * x; + } + + //// invert a scalar + //double scalar_inverse(const double& x, OptionalJacobian<1, 1> H = {}){ + // double ret = 1/x; + // if(H) *H << -1/(x*x); + // return ret; + //} + +} +// allow multiplication by scalar +// TODO: limit code gen to types that obey vector_space +template ::value>::type* = nullptr> +Expression operator*(const Double_& k, const Expression& x) +{ + return Expression(internal::scale_vec, k, x); +} + +// provide derivatives for ratios of scalars +//Double_ operator/(const Double_& lhs, const Double_& rhs) +//{ +// return lhs * Double_(internal::scalar_inverse, rhs); +//} + + + + + + } // \namespace gtsam From 6abd2172709205f052ac22ad3deeec7f498cb1d9 Mon Sep 17 00:00:00 2001 From: Brett Downing Date: Fri, 8 Aug 2025 18:24:14 +1000 Subject: [PATCH 05/16] fix trajectory and adjust kernel api --- gtsam/basis/polynomial/PiecewisePolynomial.h | 4 +- gtsam/basis/polynomial/TrajectoryModel.h | 249 ++++++++++++++----- gtsam/basis/polynomial/kernels.h | 15 +- gtsam/basis/tests/testIrwinHall.cpp | 12 + gtsam/basis/tests/testTrajectoryModel.cpp | 223 +++++++++++++++++ 5 files changed, 433 insertions(+), 70 deletions(-) create mode 100644 gtsam/basis/tests/testTrajectoryModel.cpp diff --git a/gtsam/basis/polynomial/PiecewisePolynomial.h b/gtsam/basis/polynomial/PiecewisePolynomial.h index 1c569f205b..b49bfda000 100644 --- a/gtsam/basis/polynomial/PiecewisePolynomial.h +++ b/gtsam/basis/polynomial/PiecewisePolynomial.h @@ -1,6 +1,6 @@ #pragma once -#include +#include namespace gtsam { @@ -57,7 +57,7 @@ class piecewise_polynomial : public kernel_base return evaluate_d(0, t, H); } - double evaluate_d(size_t derivative, double t, OptionalJacobian<1, 1> H = {}) const + double evaluate_d(size_t derivative, double t, OptionalJacobian<1, 1> H = {}) const override { // bounds check if(t < params.intervals[0]) diff --git a/gtsam/basis/polynomial/TrajectoryModel.h b/gtsam/basis/polynomial/TrajectoryModel.h index 9a03af3f95..9db4b1666a 100644 --- a/gtsam/basis/polynomial/TrajectoryModel.h +++ b/gtsam/basis/polynomial/TrajectoryModel.h @@ -1,31 +1,39 @@ #pragma once -#include "PiecewisePolynomial.h" +#include // API for the basis functions +#include // provides the default kernel /* +TrajectoryModel generates Expressions that encode the sampling of interpolating funcitons like splines. +The sample coordinate (sampling time) is an Expression, allowing the user to optimise for a time of best fit within a known curve. +The control points are Expressions, allowing a model to interpolate between results of other models +The template type supports gtsam Lie Groups + This trajectory model uses a lie-algebra accumulation form described in: -https://openaccess.thecvf.com/content_CVPR_2020/papers/Sommer_Efficient_Derivative_Computation_for_Cumulative_B-Splines_on_Lie_Groups_CVPR_2020_paper.pdf +https://openaccess.thecvf.com/content_CVPR_2020/papers/Sommer_Efficient_D_Computation_for_Cumulative_B-Splines_on_Lie_Groups_CVPR_2020_paper.pdf In the special case of spines with uniformly-spaced control points ('canonical splines'), - the basis funcitons reduce down to the Irwin Hall CDF. + the basis functions reduce down to the Irwin Hall CDF. This formulation is equivalent to a describing the trajactory as a convolution - between the control points and the CDF. + between the control points and the PDF. -Conceptually this is like to taking a numerical derivative in the Lie Algebra, - then encoding an integral as a (very) long FIR filter to recover the poses. - this leaves the actual FIR kernel up to the user +Conceptually this convolution is like to taking a numerical derivative in the Lie Algebra, + then encoding an integral operator as a (very) long FIR filter (the step function) + this leaves the actual FIR kernel up to the user, as long as it it integrated at compile time. + points x kernel points x kernel x derivative x integral (points x derivative) x kernel x integral (derivative(points) x kernel) x integral derivative(points) x integral(kernel) -This convolution form clarifies the need for a time-reversal applied to the integrated kernel +This convolution form shows why the time-reversal gets applied to the integrated kernel When used with a time-reversed integral of an impulse response, this replicates the behaviour of a FIR filter. + The impulse should integrate to 1.0 using an Irwin Hall probability density function as the impulse, replicates cardinal splines. @@ -36,25 +44,83 @@ namespace gtsam { template class TrajectoryModel { - const kernel_base& kernel; - std::vector> points; public: -/** - Sample the trajectory model at a given timestamp - window_start and window_end are used to limit the number of control points - considered in the resulting Expression. -*/ + TrajectoryModel( + const kernel_base& kernel = kernels::IrwinHallCDF2, // default to cubic spline + const std::vector>& points = {}, + bool pad_front = false + ): + kernel_(kernel), + points_(points), + kernel_offset_( + pad_front ? + kernel.get_end() : + kernel.get_beginning() + ) + {} + +private: + const kernel_base& kernel_; + std::vector> points_; + const double kernel_offset_; +public: + + /** + Append control points to the trajectory + point can wrap Keys, constants, or other computations. + A 2d interpolation mesh can be constructed by using evaluations of + an orthogonal TrajectoryModel as the control points. + */ + void add_control_point(Expression point) + { + points_.push_back(point); + } + + /** + get a read-only reference to the current control points + */ + const std::vector> get_control_points(){ + return points_; + } + + + /** + Sample the trajectory model at a given timestamp + The return value is an expression containing the spline evalation algorithm. + timestamp is an Expression, allowing the user to inject solution-dependent sample rate corrections before sampling the spline. + + window_start and window_end are used to limit the number of control points + considered in the resulting Expression. + Setting window_end beyond `get_points.size()` will not add future points to the resulting expression. + Timestamps always start at zero, and control points are spaced with an interval of exactly 1.0. + Default window uses all available control points. + + */ Expression sample_trajectory( Double_ timestamp, - double window_start, - double window_end, - ); + double window_start = 0, + double window_end = -1); -private: /** - interpolate calls sample_kernel and weighted_sum to apply the kernel over subset of the control points. + Sample the Nth derivative of the trajectory. + TangentVector obeys vector-space rules, so all subsequent derivatives share the same type. + setting derivative to zero yields the logspace difference between `sample_trajectory()` and the pose just prior to window_start + */ + Expression::TangentVector> sample_trajectory_d( + Double_ timestamp, + double window_start = 0, + double window_end = -1, + size_t derivative = 1); + + + +public: +//private: + + /** + interpolate calls sample_kernel and cumulative_path_sum to apply the kernel over a subset of the control points. the return type is an Expression that depends on points and timestamp, performing an automatically differentiated kernel convolution at linearisation time. */ @@ -63,9 +129,14 @@ class TrajectoryModel const Double_& timestamp, // the timestamp to interpolate at const std::vector>& points, // a reference to the control points to interpolate size_t start, size_t end // the range of control points to interpolate - ); - + ); + Expression::TangentVector> kernel_interpolate_d( + const kernel_base& kernel, // the kernel to use for interpolation + const Double_& timestamp, // the timestamp to interpolate at + const std::vector>& points, // a reference to the control points to interpolate + size_t start, size_t end, // the range of control points to interpolate + size_t derivative = 1); /** sample the kernel at a range of points timestamp is is in units of samples. Be sure to divide your timestamp by the sample rate @@ -74,7 +145,8 @@ class TrajectoryModel std::vector sample_kernel( const kernel_base& kernel, const Double_& timestamp, - size_t start, size_t end); + size_t start, size_t end, + size_t derivative = 0); /** @@ -85,61 +157,81 @@ class TrajectoryModel const std::vector>& points, const std::vector& cdf_weights); -}; + /** + logspace form of the cumulative path sum, used for derivatives + */ + Expression::TangentVector> cumulative_path_sum_d( + const std::vector>& points, + const std::vector& weights); + +}; // class TrajectoryModel -/** - Sample the trajectory model at a given timestamp - window_start and window_end are used to limit the number of control points - considered in the resulting Expression. -*/ template Expression TrajectoryModel::sample_trajectory( Double_ timestamp, - double window_start = 0, - double window_end = -1, -) + double window_start, + double window_end) { size_t start = floor(window_start); size_t end = floor(window_end); - if(window_start<0) - { - start = 0; - } - if(window_end < 0 || window_end > points.size()) - { - end = points.size(); - } - - return kernel_interpolate( - kernel, - timestamp, - points, - start, end); - + // sanitise inputs + if(window_start<0) start = 0; + if(window_end < 0 || window_end > points_.size()) end = points_.size(); + // pass to internal method + return kernel_interpolate(kernel_, timestamp, points_, start, end); } +template +Expression::TangentVector> TrajectoryModel::sample_trajectory_d( + Double_ timestamp, + double window_start, + double window_end, + size_t derivative) +{ + // sanitise inputs + size_t start = floor(window_start); + size_t end = floor(window_end); + if(window_start<0) start = 0; + if(window_end < 0 || window_end > points_.size()) end = points_.size(); + return kernel_interpolate_d(kernel_, timestamp, points_, start, end, derivative); +} +/* +// compute logspace vectors between adjacent points, +// scale the vectors by the CDF based weight for the relevant point +// accumulate the logspace path as a relative vector from the first point +*/ template Expression TrajectoryModel::cumulative_path_sum( const std::vector>& points, - const std::vector& cdf_weights -){ - Expression::TangentVector> v = typename traits::TangentVector::Zero(); + const std::vector& cdf_weights) +{ + return expmap(points[0], cumulative_path_sum_d(points, cdf_weights)); +} + + +template +Expression::TangentVector> TrajectoryModel::cumulative_path_sum_d( + const std::vector>& points, + const std::vector& weights) +{ + Expression::TangentVector> v(traits::TangentVector::Zero()); // compute logspace vectors between adjacent points, - // scale the vectors by the CDF based weight for the relevant point - // accumulate the logspace path as a relative vector from the first point - for (size_t i = 0; i < points.size()-1; i++) + // scale the vectors by the weight for the relevant point + // accumulate the logspace vector + // we skip the first weight; cumulative_path_sum assumes it is equal to 1.0 + for (size_t i = 1; i < points.size(); i++) { - v += cdf_weights[i] * logmap(points[i], points[i+1]); + v += weights[i] * logmap(points[i-1], points[i]); } - // apply the logspace vector to the first point - return expmap(points[0], v); + return v; } + /** interpolate calls sample_kernel and weighted_sum to apply the kernel over subset of the control points. the return type is an Expression that depends on points and timestamp, @@ -155,14 +247,33 @@ Expression TrajectoryModel::kernel_interpolate( { // copy the interval of control points that we want to interpolate within std::vector> points_range(points.begin()+start, points.begin()+end); - - Double_ span_time = timestamp - Double_(double(start)); - // time-reverse the cdf - Double_ kernel_time = Double_(kernel.get_end()) - span_time - std::vector weights_range = kernel.sample_kernel(timestamp, start, end); + // generate the basis functions as expressions depending on the timestamp + std::vector weights_range = sample_kernel(kernel, timestamp, start, end, 0); + // apply the weighted sum return cumulative_path_sum(points_range, weights_range); } +/** +interpolate calls sample_kernel and weighted_sum to apply the kernel over subset of the control points. + the return type is an Expression that depends on points and timestamp, + performing an automatically differentiated kernel convolution at linearisation time. +*/ +template +Expression::TangentVector> TrajectoryModel::kernel_interpolate_d( + const kernel_base& kernel, // the kernel to use for interpolation + const Double_& timestamp, // the timestamp to interpolate at + const std::vector>& points, // a reference to the control points to interpolate + size_t start, size_t end, // the range of control points to interpolate + size_t derivative) +{ + // copy the interval of control points that we want to interpolate within + std::vector> points_range(points.begin()+start, points.begin()+end); + // generate the basis functions as expressions depending on the timestamp + std::vector weights_range = sample_kernel(kernel, timestamp, start, end, derivative); + // apply the weighted sum + return cumulative_path_sum_d(points_range, weights_range); +} + @@ -175,21 +286,25 @@ template std::vector TrajectoryModel::sample_kernel( const kernel_base& kernel, const Double_& timestamp, - size_t start, size_t end) + size_t start, size_t end, + size_t derivative) { + std::vector kernel_samples; + // expression constructor requires us to bind to the class member method std::function)> keval = - [&](const double& x, OptionalJacobian<1, 1> j) {return kernel.evaluate(x, j);}; - std::vector vec; + [&kernel, derivative](const double& x, OptionalJacobian<1, 1> j) {return kernel.evaluate_d(0, x, j);}; + for(size_t i = start; i < end; i++) { Double_ kernel_time = - Double_(get_end()) + Double_(kernel_offset_) + (timestamp - Double_(double(start))) - Double_(double(i)); - vec.push_back(Double_(keval, kernel_time)); + kernel_samples.push_back(Double_(keval, kernel_time)); } - return vec; + + return kernel_samples; } diff --git a/gtsam/basis/polynomial/kernels.h b/gtsam/basis/polynomial/kernels.h index cfb8eec37f..e94c761575 100644 --- a/gtsam/basis/polynomial/kernels.h +++ b/gtsam/basis/polynomial/kernels.h @@ -13,14 +13,27 @@ namespace gtsam { class kernel_base { public: + + /** + evaluate the kernel function at t + */ virtual double evaluate(const double& t, OptionalJacobian<1, 1> H = {}) const = 0; + /** + evaluate the kernel function's nth derivative at t + */ + virtual double evaluate_d(size_t derivative, double t, OptionalJacobian<1, 1> H = {}) const = 0; + // earliest value of t that returns a non-zero kernel value virtual double get_beginning() const = 0; // centre of the distribution virtual double get_center() const = 0; // last value of t that returns a non-zero kernel value virtual double get_end() const = 0; - + inline double get_length() const + { + return get_end() - get_beginning(); + }; + virtual ~kernel_base() = default; diff --git a/gtsam/basis/tests/testIrwinHall.cpp b/gtsam/basis/tests/testIrwinHall.cpp index e43aee21e7..cc42e5c8c0 100644 --- a/gtsam/basis/tests/testIrwinHall.cpp +++ b/gtsam/basis/tests/testIrwinHall.cpp @@ -167,3 +167,15 @@ TEST( IrwinHall , DerivativeContinuity6 ) { } } + + + + + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/basis/tests/testTrajectoryModel.cpp b/gtsam/basis/tests/testTrajectoryModel.cpp new file mode 100644 index 0000000000..1be8f8139e --- /dev/null +++ b/gtsam/basis/tests/testTrajectoryModel.cpp @@ -0,0 +1,223 @@ +/** + * @file TestGeodesicWeightedSum.cpp + * @brief unit tests for weighted sum + * @author Brett Downing + * @date August 2025 + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + + +// The summation of so many terms does get noisy, +// so split the epsilon and the tolerance +double epsilon = 1e-9; +double tolerance = 1e-7; + + +TEST( TrajectoryModel , BasicBounds ) { + gtsam::Values v; // needed for evaluating Expressions + + // choose a cubic spline + const kernel_base& basis_function = kernels::IrwinHallCDF2; + + // specify some control points + std::vector path({ + Double_(0.0), + Double_(1.0), + Double_(0.0), + Double_(1.0), + }); + + // rear padding by default + // bool pad_front = false; + // with padding at the back, + // non-constant timestamps will be in + // [0 : path.size()-1 + basis_function.get_length()] + TrajectoryModel model(basis_function, path/*, pad_front*/); + + Key t_key = Key(0); // choose a key to carry the timestamp + v.insert(t_key, 0.0); // assign a value to the timestamp + Double_ timestamp = Double_(t_key); // associate the timestamp with the key + + // construct an Expression that samples the trajectory + Double_ sample = model.sample_trajectory(timestamp); + + + // bounds check + v.update(t_key, 0); + CHECK(assert_equal(path.front().value(v), sample.value(v), tolerance)); + + v.update(t_key, path.size()-1 + basis_function.get_length()); + CHECK(assert_equal(path.back().value(v), sample.value(v), tolerance)); + + // mid value + v.update(t_key, (path.size() + basis_function.get_length())/2); + CHECK(assert_equal((path.front().value(v)+path.back().value(v))/2, sample.value(v), tolerance)); + +} + + + +TEST( TrajectoryModel , FrontPadding ) { + gtsam::Values v; // needed for evaluating Expressions + + // choose a cubic spline + const kernel_base& basis_function = kernels::IrwinHallCDF2; + + // specify some control points + std::vector path({ + Double_(0.0), + Double_(1.0), + Double_(0.0), + Double_(1.0), + }); + bool pad_front = true; + // with padding at the front, + // non-constant timestamps will be in + // [-basis_function.get_length() : path.size()-1] + TrajectoryModel model(basis_function, path, pad_front); + + Key t_key = Key(0); // choose a key to carry the timestamp + v.insert(t_key, 0.0); // assign a value to the timestamp + Double_ timestamp = Double_(t_key); // associate the timestamp with the key + + // construct an Expression that samples the trajectory + Double_ sample = model.sample_trajectory(timestamp); + + + // bounds check + v.update(t_key, -basis_function.get_length()); + CHECK(assert_equal(path.front().value(v), sample.value(v), tolerance)); + + v.update(t_key, path.size()-1); + CHECK(assert_equal(path.back().value(v), sample.value(v), tolerance)); + + // mid value + v.update(t_key, (-basis_function.get_length() + (path.size()))/2); + CHECK(assert_equal((path.front().value(v)+path.back().value(v))/2, sample.value(v), tolerance)); + +} + + + + + +TEST( TrajectoryModel , WindowTruncation ) { + // TrajectoryModel can interpolate within a range of control points + // the result should be the same as long as enough points are included + // before or after the window + + gtsam::Values v; // needed for evaluating Expressions + + // choose a cubic spline + const kernel_base& basis_function = kernels::IrwinHallCDF2; + + // specify some control points + std::vector path({ + Double_(4.0), + Double_(-4.0), + Double_(3.0), + Double_(7.0), + + Double_(3.0), + Double_(-8.0), + Double_(2.0), + Double_(6.0), + + Double_(4.0), + Double_(1.0), + Double_(3.0), + Double_(-2.0), + + Double_(5.0), + Double_(2.0), + Double_(9.0), + Double_(-2.0) + }); + + //rear padding by default + TrajectoryModel model(basis_function, path); + + Key t_key = Key(0); // choose a key to carry the timestamp + v.insert(t_key, 0.0); // assign a value to the timestamp + Double_ timestamp = Double_(t_key); // associate the timestamp with the key + + // construct an Expression that samples from the entire trajectory + Double_ sample = model.sample_trajectory(timestamp); + + // construct an Expression that samples from a subset of the trajectory + double window_start = 4; + double window_end = 7; + // TODO: API choice, window automatically calculate pre and post + Double_ trunc_back = model.sample_trajectory(timestamp, 0, window_end); + Double_ trunc_front = model.sample_trajectory(timestamp, window_start - (basis_function.get_length()+1), -1); + Double_ trunc_both = model.sample_trajectory(timestamp, window_start - (basis_function.get_length()+1), window_end); + + for(double t=window_start; t(t_key, t); + CHECK(assert_equal(sample.value(v), trunc_back.value(v), tolerance)); + CHECK(assert_equal(sample.value(v), trunc_front.value(v), tolerance)); + CHECK(assert_equal(sample.value(v), trunc_both.value(v), tolerance)); + + } +} + + +// Pose + +/* + +TEST( TrajectoryModel , WindowTruncation ) { + gtsam::Values v; // needed for evaluating Expressions + + // choose a cubic spline + const kernel_base& basis_function = kernels::IrwinHallCDF2; + + // specify some control points + std::vector path({ + Pose3_(Pose3(Rot3(), Point3(0,0,0))), + Pose3_(Pose3(Rot3(), Point3(0,0,0))), + Pose3_(Pose3(Rot3(), Point3(0,0,0))), + Pose3_(Pose3(Rot3(), Point3(0,0,0))), + }); + + //rear padding by default + TrajectoryModel model(basis_function, path); + + Key t_key = Key(0); // choose a key to carry the timestamp + v.insert(t_key, 0.0); // assign a value to the timestamp + Double_ timestamp = Double_(t_key); // associate the timestamp with the key + + // construct an Expression that samples from the entire trajectory + Pose3_ sample = model.sample_trajectory(timestamp); + + for(double t=window_start; t(t_key, t); + CHECK(assert_equal(sample.value(v), trunc_back.value(v), tolerance)); + CHECK(assert_equal(sample.value(v), trunc_front.value(v), tolerance)); + CHECK(assert_equal(sample.value(v), trunc_both.value(v), tolerance)); + } +} +*/ + +// Mesh + + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + From dc2f1cc41633ef93ecc49cc8afd890777fc88ffa Mon Sep 17 00:00:00 2001 From: Brett Downing Date: Fri, 8 Aug 2025 20:39:23 +1000 Subject: [PATCH 06/16] makes window padding automatic --- gtsam/basis/polynomial/TrajectoryModel.h | 50 ++++++++---- gtsam/basis/tests/testTrajectoryModel.cpp | 92 ++++++++++++++++++++--- 2 files changed, 115 insertions(+), 27 deletions(-) diff --git a/gtsam/basis/polynomial/TrajectoryModel.h b/gtsam/basis/polynomial/TrajectoryModel.h index 9db4b1666a..526f94d246 100644 --- a/gtsam/basis/polynomial/TrajectoryModel.h +++ b/gtsam/basis/polynomial/TrajectoryModel.h @@ -37,6 +37,12 @@ When used with a time-reversed integral of an impulse response, using an Irwin Hall probability density function as the impulse, replicates cardinal splines. + +There is possibly a more numerically way of doing this where reference point + is chosen as close to the control point as possible, and the logspace + accumulation is performed both ways out from the reference point. + The CDF would need to be evaluated and split at the reference point + */ namespace gtsam { @@ -53,17 +59,25 @@ class TrajectoryModel ): kernel_(kernel), points_(points), - kernel_offset_( - pad_front ? + pad_front_(pad_front), + kernel_offset_(pad_front ? kernel.get_end() : - kernel.get_beginning() - ) + kernel.get_beginning()), + window_pre_(pad_front ? + 0 : + ceil(kernel.get_length())), + window_post_(pad_front ? + ceil(kernel.get_length()) : + 0 ) {} private: const kernel_base& kernel_; std::vector> points_; + const bool pad_front_; const double kernel_offset_; + const int window_pre_; + const int window_post_; public: /** @@ -174,11 +188,13 @@ Expression TrajectoryModel::sample_trajectory( double window_start, double window_end) { - size_t start = floor(window_start); - size_t end = floor(window_end); + // sanitise inputs - if(window_start<0) start = 0; - if(window_end < 0 || window_end > points_.size()) end = points_.size(); + int start = floor(window_start) - window_pre_; + int end = ceil(window_end) + window_post_; + if(start < 0) start = 0; + if(window_end < 0 || end > points_.size()) end = points_.size(); + // pass to internal method return kernel_interpolate(kernel_, timestamp, points_, start, end); } @@ -191,10 +207,12 @@ Expression::TangentVector> TrajectoryModel::sample_traject size_t derivative) { // sanitise inputs - size_t start = floor(window_start); - size_t end = floor(window_end); - if(window_start<0) start = 0; - if(window_end < 0 || window_end > points_.size()) end = points_.size(); + int start = floor(window_start) - window_pre_; + int end = ceil(window_end) + window_post_; + if(start < 0) start = 0; + if(window_end < 0 || end > points_.size()) end = points_.size(); + + // pass to internal method return kernel_interpolate_d(kernel_, timestamp, points_, start, end, derivative); } @@ -276,7 +294,6 @@ Expression::TangentVector> TrajectoryModel::kernel_interpo - /** sample the kernel at a range of points timestamp is is in units of samples. Be sure to divide your timestamp by the sample rate @@ -293,13 +310,16 @@ std::vector TrajectoryModel::sample_kernel( // expression constructor requires us to bind to the class member method std::function)> keval = - [&kernel, derivative](const double& x, OptionalJacobian<1, 1> j) {return kernel.evaluate_d(0, x, j);}; + [&kernel, derivative](const double& x, OptionalJacobian<1, 1> j) + { + return kernel.evaluate_d(derivative, x, j); + }; for(size_t i = start; i < end; i++) { Double_ kernel_time = Double_(kernel_offset_) - + (timestamp - Double_(double(start))) + + timestamp - Double_(double(i)); kernel_samples.push_back(Double_(keval, kernel_time)); } diff --git a/gtsam/basis/tests/testTrajectoryModel.cpp b/gtsam/basis/tests/testTrajectoryModel.cpp index 1be8f8139e..888dec52e1 100644 --- a/gtsam/basis/tests/testTrajectoryModel.cpp +++ b/gtsam/basis/tests/testTrajectoryModel.cpp @@ -113,8 +113,10 @@ TEST( TrajectoryModel , FrontPadding ) { TEST( TrajectoryModel , WindowTruncation ) { // TrajectoryModel can interpolate within a range of control points + // bringing the compute cost down to O(window_size). // the result should be the same as long as enough points are included - // before or after the window + // before or after the window. + // padding is automatically applied to the window arguments gtsam::Values v; // needed for evaluating Expressions @@ -155,12 +157,11 @@ TEST( TrajectoryModel , WindowTruncation ) { Double_ sample = model.sample_trajectory(timestamp); // construct an Expression that samples from a subset of the trajectory - double window_start = 4; - double window_end = 7; - // TODO: API choice, window automatically calculate pre and post + double window_start = 6; + double window_end = 8; Double_ trunc_back = model.sample_trajectory(timestamp, 0, window_end); - Double_ trunc_front = model.sample_trajectory(timestamp, window_start - (basis_function.get_length()+1), -1); - Double_ trunc_both = model.sample_trajectory(timestamp, window_start - (basis_function.get_length()+1), window_end); + Double_ trunc_front = model.sample_trajectory(timestamp, window_start, -1); + Double_ trunc_both = model.sample_trajectory(timestamp, window_start, window_end); for(double t=window_start; t(t_key, t); - CHECK(assert_equal(sample.value(v), trunc_back.value(v), tolerance)); - CHECK(assert_equal(sample.value(v), trunc_front.value(v), tolerance)); - CHECK(assert_equal(sample.value(v), trunc_both.value(v), tolerance)); + } } */ -// Mesh +/* + +TEST( TrajectoryModel , MeshModel ) { + // TrajectoryModel accepts Expressions as control points, + // allowing arbitrarily complex control point definitions, + // such as entire other TrajectoryModel samples + + gtsam::Values v; // needed for evaluating Expressions + + // choose a cubic spline + const kernel_base& basis_function = kernels::IrwinHallCDF2; + + // specify some control points + std::vector> mesh({ + { + Double_(4.0), + Double_(-4.0), + Double_(3.0), + Double_(7.0), + }, + { + Double_(3.0), + Double_(-8.0), + Double_(2.0), + Double_(6.0), + }, + { + Double_(4.0), + Double_(1.0), + Double_(3.0), + Double_(-2.0), + }, + { + + Double_(5.0), + Double_(2.0), + Double_(9.0), + Double_(-2.0) + } + }); + + + Key x_key = Key(0); + Key y_key = Key(1); + v.insert(x_key, 0.0); + v.insert(y_key, 0.0); + Double_ x_expr(x_key); + Double_ y_expr(y_key); + + std::vector col_path; + std::vector> rows; + + for(const auto& path : mesh){ + auto row = TrajectoryModel model(basis_function, path); + rows.push_back(row); + auto row_sample = row.sample_trajectory(x_expr); + col_path.push_back(row_sample); + } + auto col = TrajectoryModel model(basis_function, col_path); + auto sample = col.sample_trajectory(y_expr); + for(double x=0; x<4; x+=0.1) + { + for(double y=0; y<4; y+=0.1) + { + v.update(x_key, x); + v.update(y_key, y); + CHECK(assert_equal(, tolerance)); + } +} +*/ + /* ************************************************************************* */ int main() { TestResult tr; From 7619ddb50f1649b4767b7ffad3dda834ced82590 Mon Sep 17 00:00:00 2001 From: Brett Downing Date: Fri, 8 Aug 2025 20:52:45 +1000 Subject: [PATCH 07/16] adds pose3 and mesh tests --- gtsam/basis/tests/testTrajectoryModel.cpp | 28 +++++++++++------------ 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/gtsam/basis/tests/testTrajectoryModel.cpp b/gtsam/basis/tests/testTrajectoryModel.cpp index 888dec52e1..70abedbd53 100644 --- a/gtsam/basis/tests/testTrajectoryModel.cpp +++ b/gtsam/basis/tests/testTrajectoryModel.cpp @@ -176,7 +176,7 @@ TEST( TrajectoryModel , WindowTruncation ) { // Pose -/* + TEST( TrajectoryModel , TypesPose3 ) { gtsam::Values v; // needed for evaluating Expressions @@ -188,8 +188,8 @@ TEST( TrajectoryModel , TypesPose3 ) { std::vector path({ Pose3_(Pose3(Rot3(), Point3(0,0,0))), Pose3_(Pose3(Rot3(), Point3(0,0,0))), - Pose3_(Pose3(Rot3(), Point3(0,0,0))), - Pose3_(Pose3(Rot3(), Point3(0,0,0))), + Pose3_(Pose3(Rot3(), Point3(0,2,0))), + Pose3_(Pose3(Rot3(), Point3(0,2,0))), }); //rear padding by default @@ -202,14 +202,12 @@ TEST( TrajectoryModel , TypesPose3 ) { // construct an Expression that samples from the entire trajectory Pose3_ sample = model.sample_trajectory(timestamp); - for(double t=window_start; t(t_key, 3.5); + CHECK(assert_equal(Pose3(Rot3(), Point3(0,1,0)), sample.value(v), tolerance)); - } } -*/ -/* + TEST( TrajectoryModel , MeshModel ) { // TrajectoryModel accepts Expressions as control points, @@ -262,25 +260,27 @@ TEST( TrajectoryModel , MeshModel ) { std::vector> rows; for(const auto& path : mesh){ - auto row = TrajectoryModel model(basis_function, path); + TrajectoryModel row(basis_function, path); rows.push_back(row); auto row_sample = row.sample_trajectory(x_expr); col_path.push_back(row_sample); } - auto col = TrajectoryModel model(basis_function, col_path); + TrajectoryModel col(basis_function, col_path); auto sample = col.sample_trajectory(y_expr); - for(double x=0; x<4; x+=0.1) + for(double x=0; x<7; x+=0.1) { - for(double y=0; y<4; y+=0.1) + for(double y=0; y<7; y+=0.1) { v.update(x_key, x); v.update(y_key, y); - CHECK(assert_equal(, tolerance)); + CHECK(sample.value(v)<10); + CHECK(sample.value(v)>-10); + } } } -*/ + /* ************************************************************************* */ int main() { From acd924b6a63534862a8ef7334133b5a75cfa0b6b Mon Sep 17 00:00:00 2001 From: Brett Downing Date: Mon, 11 Aug 2025 10:20:07 +1000 Subject: [PATCH 08/16] adds test for Nth derivatives of Pose3 path --- gtsam/basis/polynomial/PiecewisePolynomial.h | 5 +- gtsam/basis/polynomial/kernels.h | 2 + gtsam/basis/tests/testTrajectoryModel.cpp | 112 ++++++++++++++++++- 3 files changed, 112 insertions(+), 7 deletions(-) diff --git a/gtsam/basis/polynomial/PiecewisePolynomial.h b/gtsam/basis/polynomial/PiecewisePolynomial.h index b49bfda000..8379e83f72 100644 --- a/gtsam/basis/polynomial/PiecewisePolynomial.h +++ b/gtsam/basis/polynomial/PiecewisePolynomial.h @@ -34,11 +34,14 @@ class piecewise_polynomial : public kernel_base { return params.intervals[0]; } - double get_end() const override { return params.intervals[pieces]; } + size_t get_valid_derivatives() const override + { + return order; + } std::array get_intervals() const { return params.intervals; diff --git a/gtsam/basis/polynomial/kernels.h b/gtsam/basis/polynomial/kernels.h index e94c761575..a47ee64d68 100644 --- a/gtsam/basis/polynomial/kernels.h +++ b/gtsam/basis/polynomial/kernels.h @@ -29,6 +29,8 @@ class kernel_base virtual double get_center() const = 0; // last value of t that returns a non-zero kernel value virtual double get_end() const = 0; + virtual size_t get_valid_derivatives() const = 0; + inline double get_length() const { return get_end() - get_beginning(); diff --git a/gtsam/basis/tests/testTrajectoryModel.cpp b/gtsam/basis/tests/testTrajectoryModel.cpp index 70abedbd53..f11d7f4af9 100644 --- a/gtsam/basis/tests/testTrajectoryModel.cpp +++ b/gtsam/basis/tests/testTrajectoryModel.cpp @@ -1,6 +1,6 @@ /** - * @file TestGeodesicWeightedSum.cpp - * @brief unit tests for weighted sum + * @file TestTrajectoryModel.cpp + * @brief unit tests for cardinal spline interpolation * @author Brett Downing * @date August 2025 */ @@ -17,9 +17,9 @@ using namespace gtsam; -// The summation of so many terms does get noisy, -// so split the epsilon and the tolerance -double epsilon = 1e-9; + +// Lie comopsitions stay valid for long tangent traversals +double epsilon = 1e-6; double tolerance = 1e-7; @@ -186,7 +186,7 @@ TEST( TrajectoryModel , TypesPose3 ) { // specify some control points std::vector path({ - Pose3_(Pose3(Rot3(), Point3(0,0,0))), + Pose3_(Pose3(Rot3(), Point3(0,0,0))), // XXX use non-trival rotations Pose3_(Pose3(Rot3(), Point3(0,0,0))), Pose3_(Pose3(Rot3(), Point3(0,2,0))), Pose3_(Pose3(Rot3(), Point3(0,2,0))), @@ -209,6 +209,106 @@ TEST( TrajectoryModel , TypesPose3 ) { + + +TEST( TrajectoryModel , DerivativePose3 ) { + gtsam::Values v; // needed for evaluating Expressions + + // choose a cubic spline + const kernel_base& basis_function = kernels::IrwinHallCDF2; + + // specify some control points + std::vector path({ + Pose3_(Pose3(Rot3(), Point3(0,0,0))), // XXX use non-trival rotations + Pose3_(Pose3(Rot3(), Point3(0,0,0))), + Pose3_(Pose3(Rot3(), Point3(0,2,0))), + Pose3_(Pose3(Rot3(), Point3(0,2,0))), + }); + + TrajectoryModel model(basis_function, path); + + Key t_key = Key(0); // key to carry the timestamp + Key t_eps_key = Key(1); // key to carry the timestamp + epsilon + + // assign types to Keys + v.insert(t_key, 0.0); + v.insert(t_eps_key, 0.0); + + // cast the Keys to Expressions + Double_ t_ref = Double_(t_key); + Double_ t_eps = Double_(t_eps_key); + + // construct Expressions that sample the trajectory + Pose3_ sample_ref = model.sample_trajectory(t_ref); + Pose3_ sample_eps = model.sample_trajectory(t_eps); + // construct an expression that captures the tangent + auto sample_d = model.sample_trajectory_d(t_ref,0,-1,1); + + for(double sample_time = 0; sample_time<7; sample_time+=0.1) + { + v.update(t_key, sample_time); + v.update(t_eps_key, sample_time + epsilon); + CHECK(assert_equal( + sample_eps.value(v), + expmap(sample_ref, epsilon * sample_d).value(v), + tolerance + )); + } + +} + + + +TEST( TrajectoryModel , NthDerivativePose3 ) { + gtsam::Values v; // needed for evaluating Expressions + + // choose a cubic spline + const kernel_base& basis_function = kernels::IrwinHallCDF2; + + // specify some control points + std::vector path({ + Pose3_(Pose3(Rot3(), Point3(0,0,0))), // XXX use non-trival rotations + Pose3_(Pose3(Rot3(), Point3(0,0,0))), + Pose3_(Pose3(Rot3(), Point3(0,2,0))), + Pose3_(Pose3(Rot3(), Point3(0,2,0))), + }); + + TrajectoryModel model(basis_function, path); + + Key t_key = Key(0); // key to carry the timestamp + Key t_eps_key = Key(1); // key to carry the timestamp + epsilon + + // assign types to Keys + v.insert(t_key, 0.0); + v.insert(t_eps_key, 0.0); + + // cast the Keys to Expressions + Double_ t_ref = Double_(t_key); + Double_ t_eps = Double_(t_eps_key); + + for(size_t n=0; n(t_key, sample_time); + v.update(t_eps_key, sample_time + epsilon); + CHECK(assert_equal( + sample_eps.value(v), + expmap(sample_ref, epsilon * sample_d).value(v), + tolerance + )); + } + } + +} + + + TEST( TrajectoryModel , MeshModel ) { // TrajectoryModel accepts Expressions as control points, // allowing arbitrarily complex control point definitions, From 55bdd9baa40089715e6e865f1cd4eccc4ac49663 Mon Sep 17 00:00:00 2001 From: Brett Downing Date: Mon, 11 Aug 2025 10:53:00 +1000 Subject: [PATCH 09/16] sharpens the tolerances on tests --- gtsam/basis/tests/testIrwinHall.cpp | 9 +++--- gtsam/basis/tests/testTrajectoryModel.cpp | 38 +++++++++++------------ 2 files changed, 24 insertions(+), 23 deletions(-) diff --git a/gtsam/basis/tests/testIrwinHall.cpp b/gtsam/basis/tests/testIrwinHall.cpp index cc42e5c8c0..9042764c2e 100644 --- a/gtsam/basis/tests/testIrwinHall.cpp +++ b/gtsam/basis/tests/testIrwinHall.cpp @@ -19,10 +19,11 @@ using namespace gtsam; using namespace gtsam::kernels; -// The summation of so many terms does get noisy, -// so split the epsilon and the tolerance -double epsilon = 1e-9; -double tolerance = 1e-7; +// these tests look for curve continuity across control-flow boundaries +// tolerance should be about 2*(maximum gradient)*epsilon +// actual polynomial construction mistakes will be extremely large +double epsilon = 1e-7; +double tolerance = 100*epsilon; diff --git a/gtsam/basis/tests/testTrajectoryModel.cpp b/gtsam/basis/tests/testTrajectoryModel.cpp index f11d7f4af9..8d5c2b1d2c 100644 --- a/gtsam/basis/tests/testTrajectoryModel.cpp +++ b/gtsam/basis/tests/testTrajectoryModel.cpp @@ -18,9 +18,10 @@ using namespace gtsam; -// Lie comopsitions stay valid for long tangent traversals -double epsilon = 1e-6; -double tolerance = 1e-7; +// Lie comopsitions stay valid for long tangent traversals, +// tolerance should accomodate second-order effects +double epsilon = 1e-4; +double tolerance = epsilon/4.0; TEST( TrajectoryModel , BasicBounds ) { @@ -174,11 +175,9 @@ TEST( TrajectoryModel , WindowTruncation ) { } -// Pose - - TEST( TrajectoryModel , TypesPose3 ) { + // tests TrajectoryModel can be constructed for Pose3 gtsam::Values v; // needed for evaluating Expressions // choose a cubic spline @@ -186,7 +185,7 @@ TEST( TrajectoryModel , TypesPose3 ) { // specify some control points std::vector path({ - Pose3_(Pose3(Rot3(), Point3(0,0,0))), // XXX use non-trival rotations + Pose3_(Pose3(Rot3(), Point3(0,0,0))), Pose3_(Pose3(Rot3(), Point3(0,0,0))), Pose3_(Pose3(Rot3(), Point3(0,2,0))), Pose3_(Pose3(Rot3(), Point3(0,2,0))), @@ -219,23 +218,23 @@ TEST( TrajectoryModel , DerivativePose3 ) { // specify some control points std::vector path({ - Pose3_(Pose3(Rot3(), Point3(0,0,0))), // XXX use non-trival rotations - Pose3_(Pose3(Rot3(), Point3(0,0,0))), - Pose3_(Pose3(Rot3(), Point3(0,2,0))), - Pose3_(Pose3(Rot3(), Point3(0,2,0))), + Pose3_(Pose3(Rot3::Rodrigues(0.3,2.2,0.1), Point3(0,0,0))), + Pose3_(Pose3(Rot3::Rodrigues(0.2,2.5,0.1), Point3(0,0,0))), + Pose3_(Pose3(Rot3::Rodrigues(0.0,2.2,0.1), Point3(0,2,0))), + Pose3_(Pose3(Rot3::Rodrigues(0.2,2.1,0.4), Point3(0,2,0))), }); TrajectoryModel model(basis_function, path); - Key t_key = Key(0); // key to carry the timestamp + Key t_ref_key = Key(0); // key to carry the timestamp Key t_eps_key = Key(1); // key to carry the timestamp + epsilon // assign types to Keys - v.insert(t_key, 0.0); + v.insert(t_ref_key, 0.0); v.insert(t_eps_key, 0.0); // cast the Keys to Expressions - Double_ t_ref = Double_(t_key); + Double_ t_ref = Double_(t_ref_key); Double_ t_eps = Double_(t_eps_key); // construct Expressions that sample the trajectory @@ -246,10 +245,11 @@ TEST( TrajectoryModel , DerivativePose3 ) { for(double sample_time = 0; sample_time<7; sample_time+=0.1) { - v.update(t_key, sample_time); + v.update(t_ref_key, sample_time); v.update(t_eps_key, sample_time + epsilon); CHECK(assert_equal( sample_eps.value(v), + //sample_ref.value(v), expmap(sample_ref, epsilon * sample_d).value(v), tolerance )); @@ -275,15 +275,15 @@ TEST( TrajectoryModel , NthDerivativePose3 ) { TrajectoryModel model(basis_function, path); - Key t_key = Key(0); // key to carry the timestamp + Key t_ref_key = Key(0); // key to carry the timestamp Key t_eps_key = Key(1); // key to carry the timestamp + epsilon // assign types to Keys - v.insert(t_key, 0.0); + v.insert(t_ref_key, 0.0); v.insert(t_eps_key, 0.0); // cast the Keys to Expressions - Double_ t_ref = Double_(t_key); + Double_ t_ref = Double_(t_ref_key); Double_ t_eps = Double_(t_eps_key); for(size_t n=0; n(t_key, sample_time); + v.update(t_ref_key, sample_time); v.update(t_eps_key, sample_time + epsilon); CHECK(assert_equal( sample_eps.value(v), From 842e9d367510e13e91ac2ed4474b8dba43f1569d Mon Sep 17 00:00:00 2001 From: Brett Downing Date: Mon, 11 Aug 2025 10:55:32 +1000 Subject: [PATCH 10/16] uses non-trivial rotations in Nth derivative tests --- gtsam/basis/tests/testTrajectoryModel.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/basis/tests/testTrajectoryModel.cpp b/gtsam/basis/tests/testTrajectoryModel.cpp index 8d5c2b1d2c..458625ff4e 100644 --- a/gtsam/basis/tests/testTrajectoryModel.cpp +++ b/gtsam/basis/tests/testTrajectoryModel.cpp @@ -267,10 +267,10 @@ TEST( TrajectoryModel , NthDerivativePose3 ) { // specify some control points std::vector path({ - Pose3_(Pose3(Rot3(), Point3(0,0,0))), // XXX use non-trival rotations - Pose3_(Pose3(Rot3(), Point3(0,0,0))), - Pose3_(Pose3(Rot3(), Point3(0,2,0))), - Pose3_(Pose3(Rot3(), Point3(0,2,0))), + Pose3_(Pose3(Rot3::Rodrigues(0.3,2.2,0.1), Point3(0,0,0))), // XXX use non-trival rotations + Pose3_(Pose3(Rot3::Rodrigues(0.2,2.5,0.1), Point3(0,0,0))), + Pose3_(Pose3(Rot3::Rodrigues(0.0,2.2,0.1), Point3(0,2,0))), + Pose3_(Pose3(Rot3::Rodrigues(0.2,2.1,0.4), Point3(0,2,0))), }); TrajectoryModel model(basis_function, path); From 580db7e50db912d4800a4b5feda767ef6da259f2 Mon Sep 17 00:00:00 2001 From: Brett Downing Date: Mon, 11 Aug 2025 14:52:03 +1000 Subject: [PATCH 11/16] swtiches style and polishes some comments --- gtsam/basis/polynomial/IrwinHall.cpp | 22 +- gtsam/basis/polynomial/IrwinHall.h | 50 +-- gtsam/basis/polynomial/IrwinHallCDF.cpp | 20 +- gtsam/basis/polynomial/PiecewisePolynomial.h | 53 +-- gtsam/basis/polynomial/TrajectoryModel.h | 315 +++++++++++------- gtsam/basis/polynomial/kernels.h | 45 +-- gtsam/basis/tests/testIrwinHall.cpp | 72 ++-- gtsam/basis/tests/testIrwinHallCDF.cpp | 72 ++-- gtsam/basis/tests/testPiecewisePolynomial.cpp | 12 +- gtsam/basis/tests/testTrajectoryModel.cpp | 61 ++-- 10 files changed, 406 insertions(+), 316 deletions(-) diff --git a/gtsam/basis/polynomial/IrwinHall.cpp b/gtsam/basis/polynomial/IrwinHall.cpp index 210a03640b..cc68fa6b67 100644 --- a/gtsam/basis/polynomial/IrwinHall.cpp +++ b/gtsam/basis/polynomial/IrwinHall.cpp @@ -3,30 +3,30 @@ namespace gtsam { namespace kernels { -/* -Irwin Hall coefficients -https://oeis.org/A188816 -*/ +/** + * coefficients for the Irwin Hall Probability Density Functions + * https://oeis.org/A188816 + */ -const piecewise_polynomial<0,1> IrwinHall0({ +const PiecewisePolynomial<0,1> IrwinHall0({ {1.}, {0.,1.}, 1./2. }); -const piecewise_polynomial<1,2> IrwinHall1({ +const PiecewisePolynomial<1,2> IrwinHall1({ { 0., 1., 2., -1.}, {0.,1.,2.}, 1. }); -const piecewise_polynomial<2,3> IrwinHall2({ +const PiecewisePolynomial<2,3> IrwinHall2({ { 0. , 0. , 1./2., -3./2., 6./2., -2./2., 9./2., -6./2., 1./2.}, {0.,1.,2.,3.}, 3./2. }); -const piecewise_polynomial<3,4> IrwinHall3({ +const PiecewisePolynomial<3,4> IrwinHall3({ { 0. , 0., 0., 1./6., 2./3., -2., 2., -1./2., -22./3., 10., -4., 1./2., @@ -34,7 +34,7 @@ const piecewise_polynomial<3,4> IrwinHall3({ {0.,1.,2.,3.,4.}, 2. }); -const piecewise_polynomial<4,5> IrwinHall4({ +const PiecewisePolynomial<4,5> IrwinHall4({ { 0. , 0. , 0. , 0. , 1./24., -5./24., 5./6., -5./4., 5./6., -1./6. , 155./24., -25./2., 35./4., -5./2., 1./4. , @@ -43,7 +43,7 @@ const piecewise_polynomial<4,5> IrwinHall4({ {0.,1.,2.,3.,4.,5.}, 5./2. }); -const piecewise_polynomial<5,6> IrwinHall5({ +const PiecewisePolynomial<5,6> IrwinHall5({ { 0. , 0. , 0. , 0. , 0. , 1./120., 1./20., -1./4., 1./2., -1./2., 1./4., -1./24. , -79./20., 39./4., -19./2., 9./2., -1. , 1./12. , @@ -53,7 +53,7 @@ const piecewise_polynomial<5,6> IrwinHall5({ {0.,1.,2.,3.,4.,5.,6.}, 3. }); -const piecewise_polynomial<6,7> IrwinHall6({ +const PiecewisePolynomial<6,7> IrwinHall6({ { 0. , 0. , 0. , 0. , 0. , 0. , 1./720., -7./720., 7./120., -7./48., 7./36., -7./48., 7./120., -1./120., 1337./720., -133./24. , 329./48., -161./36., 77./48., -7./24. , 1./48. , diff --git a/gtsam/basis/polynomial/IrwinHall.h b/gtsam/basis/polynomial/IrwinHall.h index dd5fa57208..752ac22f75 100644 --- a/gtsam/basis/polynomial/IrwinHall.h +++ b/gtsam/basis/polynomial/IrwinHall.h @@ -5,35 +5,35 @@ namespace gtsam { namespace kernels { -/* -Irwin Hall coefficients are used by TrajectoryModel to implement - canonical (monotonic) spline basis functions. -These functions are piecewise-defined polynomials continuous up to the (N-1)th derivative -Irwin Hall CDF 0 is a zeroth order spline (linear interpolation) -Irwin Hall CDF 2 is a cubic polynomial, so it has linear accelerations - -This series of fucntions converges pretty quickly to a gaussian. -If you need higher orders (N), you can use a gaussian kernel and CDF directly -e^(-(1/2)((x-N/2)/(0.3*N/2)^2 ) -*/ +/** + * Irwin Hall coefficients are used by TrajectoryModel to implement + * canonical (monotonic) spline basis functions. + * These functions are piecewise-defined polynomials continuous up to the (N-1)th derivative + * Irwin Hall CDF 0 is a basis for a zeroth order spline (linear interpolation) + * Irwin Hall CDF 2 is a basis for a cubic polynomial, so it has linear accelerations + * + * This series of fucntions converges pretty quickly to a gaussian. + * If you need higher orders (N), you can use a gaussian kernel and CDF directly + * approximately e^(-(1/2)((x-N/2)/(0.3*N/2)^2 ) + */ // https://oeis.org/A188816 -extern const piecewise_polynomial<0,1> IrwinHall0; -extern const piecewise_polynomial<1,2> IrwinHall1; -extern const piecewise_polynomial<2,3> IrwinHall2; -extern const piecewise_polynomial<3,4> IrwinHall3; -extern const piecewise_polynomial<4,5> IrwinHall4; -extern const piecewise_polynomial<5,6> IrwinHall5; -extern const piecewise_polynomial<6,7> IrwinHall6; +extern const PiecewisePolynomial<0,1> IrwinHall0; +extern const PiecewisePolynomial<1,2> IrwinHall1; +extern const PiecewisePolynomial<2,3> IrwinHall2; +extern const PiecewisePolynomial<3,4> IrwinHall3; +extern const PiecewisePolynomial<4,5> IrwinHall4; +extern const PiecewisePolynomial<5,6> IrwinHall5; +extern const PiecewisePolynomial<6,7> IrwinHall6; // https://oeis.org/A188668 -extern const piecewise_polynomial<1,1> IrwinHallCDF0; // produces a linear interpolator -extern const piecewise_polynomial<2,2> IrwinHallCDF1; -extern const piecewise_polynomial<3,3> IrwinHallCDF2; // produces a cubic spline -extern const piecewise_polynomial<4,4> IrwinHallCDF3; -extern const piecewise_polynomial<5,5> IrwinHallCDF4; -extern const piecewise_polynomial<6,6> IrwinHallCDF5; -extern const piecewise_polynomial<7,7> IrwinHallCDF6; +extern const PiecewisePolynomial<1,1> IrwinHallCDF0; // produces a linear interpolator +extern const PiecewisePolynomial<2,2> IrwinHallCDF1; +extern const PiecewisePolynomial<3,3> IrwinHallCDF2; // produces a cubic spline +extern const PiecewisePolynomial<4,4> IrwinHallCDF3; +extern const PiecewisePolynomial<5,5> IrwinHallCDF4; +extern const PiecewisePolynomial<6,6> IrwinHallCDF5; +extern const PiecewisePolynomial<7,7> IrwinHallCDF6; } // namespace kernels } // namespace gtsam \ No newline at end of file diff --git a/gtsam/basis/polynomial/IrwinHallCDF.cpp b/gtsam/basis/polynomial/IrwinHallCDF.cpp index 78bb77cc3e..1462f6fad1 100644 --- a/gtsam/basis/polynomial/IrwinHallCDF.cpp +++ b/gtsam/basis/polynomial/IrwinHallCDF.cpp @@ -4,22 +4,22 @@ namespace gtsam { namespace kernels { /* -Irwin Hall Cumulative Distribution Function Coefficients -https://oeis.org/A188668 -*/ + * Coefficients for the Irwin Hall Cumulative Distribution Function + * https://oeis.org/A188668 + */ -const piecewise_polynomial<1,1> IrwinHallCDF0({ +const PiecewisePolynomial<1,1> IrwinHallCDF0({ {0., 1.}, {0., 1.}, 1./2. }); -const piecewise_polynomial<2,2> IrwinHallCDF1({ +const PiecewisePolynomial<2,2> IrwinHallCDF1({ { 0. , 0. , 1./2., -2./2., 4./2., -1./2.}, {0.,1.,2.}, 1. }); -const piecewise_polynomial<3,3> IrwinHallCDF2({ +const PiecewisePolynomial<3,3> IrwinHallCDF2({ { 0. , 0. , 0. , 1./6., 3./6., -9./6., 9./6., -2./6., -21./6., 27./6., -9./6., 1./6.}, @@ -27,7 +27,7 @@ const piecewise_polynomial<3,3> IrwinHallCDF2({ 3./2. }); -const piecewise_polynomial<4,4> IrwinHallCDF3({ +const PiecewisePolynomial<4,4> IrwinHallCDF3({ { 0. , 0. , 0. , 0. , 1./24., -4./24., 16./24., -24./24., 16./24., -3./24., 92./24., -176./24., 120./24., -32./24., 3./24., @@ -35,7 +35,7 @@ const piecewise_polynomial<4,4> IrwinHallCDF3({ {0.,1.,2.,3.,4.}, 2. }); -const piecewise_polynomial<5,5> IrwinHallCDF4({ +const PiecewisePolynomial<5,5> IrwinHallCDF4({ { 0. , 0. , 0. , 0., 0. , 1./120., 5./120., -25./120., 50./120., -50./120., 25./120., -4./120., -315./120., 775./120., -750./120., 350./120., -75./120., 6./120., @@ -44,7 +44,7 @@ const piecewise_polynomial<5,5> IrwinHallCDF4({ {0.,1.,2.,3.,4.,5.}, 5./2. }); -const piecewise_polynomial<6,6> IrwinHallCDF5({ +const PiecewisePolynomial<6,6> IrwinHallCDF5({ { 0. , 0. , 0. , 0. , 0. , 0. , 1./720, -6./720, 36./720, -90./720, 120./720, -90./720, 36./720, -5./720, 954./720, -2844./720, 3510./720, -2280./720, 810./720, -144./720, 10./720, @@ -54,7 +54,7 @@ const piecewise_polynomial<6,6> IrwinHallCDF5({ {0.,1.,2.,3.,4.,5.,6.}, 3. }); -const piecewise_polynomial<7,7> IrwinHallCDF6({ +const PiecewisePolynomial<7,7> IrwinHallCDF6({ { 0. , 0. , 0. , 0. , 0. , 0. , 0. , 1./5040., 7./5040., -49./5040., 147./5040., -245./5040., 245./5040., -147./5040., 49./5040., -6./5040., -2681./5040., 9359./5040., -13965./5040., 11515./5040., -5635./5040., 1617./5040., -245./5040., 15./5040., diff --git a/gtsam/basis/polynomial/PiecewisePolynomial.h b/gtsam/basis/polynomial/PiecewisePolynomial.h index 8379e83f72..df3af2e464 100644 --- a/gtsam/basis/polynomial/PiecewisePolynomial.h +++ b/gtsam/basis/polynomial/PiecewisePolynomial.h @@ -5,9 +5,14 @@ namespace gtsam { - +/** + * A Piecewise-defined Polynomial in 1d + * mostly used for low-order approximations to probability distributions + * automatically computes derivatives + * + */ template -class piecewise_polynomial : public kernel_base +class PiecewisePolynomial : public KernelBase { public: inline constexpr static auto order = O; @@ -24,43 +29,51 @@ class piecewise_polynomial : public kernel_base public: - piecewise_polynomial(param_s init_list):params(init_list){} - - double get_center() const override + PiecewisePolynomial(param_s init_list):params(init_list){} + /** returns the centre of the distribution */ + double getCenter() const override { return params.center; } - double get_beginning() const override + /** returns the lowest value of t where this polynomial has non-constant return */ + double getBeginning() const override { return params.intervals[0]; } - double get_end() const override + /** returns the highest value of t where this polynomial has non-constant return */ + double getEnd() const override { return params.intervals[pieces]; } - size_t get_valid_derivatives() const override + /** returns the number of times this function can be differentiated and still return something useful */ + size_t getValidDerivatives() const override { return order; } - std::array get_intervals() const { + /** returns the number of pieces this polynomial is defined by, mostly use for testing. */ + std::array getIntervals() const { return params.intervals; } -/** - evaluates the continuous kernel function at time t - - derivative: how many times to differentiate the kernel function - t: the time to evaluate it relative to the kernel's datum - H: the jacobian for gtsam (the next derivative, calls this function again.) - - */ + /** + * evaluates the polynomial at t + * @param t the evaluation point + * @param H the first derivative at t + */ double evaluate(const double& t, OptionalJacobian<1, 1> H = {}) const override { - return evaluate_d(0, t, H); + return evaluateDerivative(0, t, H); } - double evaluate_d(size_t derivative, double t, OptionalJacobian<1, 1> H = {}) const override + /** + * evaluates the polynomial or its derivatives at t + * @param derivative the derivative to evaluate + * (0th derivative is the same as evaluate(t,H)) + * @param t the evaluation point + * @param H the next derivative at t + */ + double evaluateDerivative(size_t derivative, double t, OptionalJacobian<1, 1> H = {}) const override { // bounds check if(t < params.intervals[0]) @@ -92,7 +105,7 @@ class piecewise_polynomial : public kernel_base //std::cout << "c += " << t_power << " * " << params.coefficients[p][o] << std::endl; t_power *= t; } - if(H) *H << evaluate_d(derivative+1, t, {}); + if(H) *H << evaluateDerivative(derivative+1, t, {}); return coeff; } } diff --git a/gtsam/basis/polynomial/TrajectoryModel.h b/gtsam/basis/polynomial/TrajectoryModel.h index 526f94d246..4aa1fe1265 100644 --- a/gtsam/basis/polynomial/TrajectoryModel.h +++ b/gtsam/basis/polynomial/TrajectoryModel.h @@ -47,46 +47,61 @@ There is possibly a more numerically way of doing this where reference point namespace gtsam { + +/** + * An Expression factory for cardinal splines and similar convolution-based interpolations + * Used to model continuous systems and associate them with asynchronous measurements + */ template class TrajectoryModel { public: + /** + * Constructor + * @param kernel the interpolating kernel to use, See IrwinHallCDF.cpp for splines + * default is IrwinHallCDF2 for cubic splines + * @param points Expressions representing control points for the curve, defaults to empty + * @param padFront at least `kernel.getLength()` padding control points will be used, + * false places padding at the back, delaying response to the control points (default) + * true places padding at the front, pre-empting response to the control points + */ TrajectoryModel( - const kernel_base& kernel = kernels::IrwinHallCDF2, // default to cubic spline + const KernelBase& kernel = kernels::IrwinHallCDF2, const std::vector>& points = {}, - bool pad_front = false + bool padFront = false ): kernel_(kernel), points_(points), - pad_front_(pad_front), - kernel_offset_(pad_front ? - kernel.get_end() : - kernel.get_beginning()), - window_pre_(pad_front ? + padFront_(padFront), + kernelOffset_(padFront ? + kernel.getEnd() : + kernel.getBeginning()), + windowPre_(padFront ? 0 : - ceil(kernel.get_length())), - window_post_(pad_front ? - ceil(kernel.get_length()) : + ceil(kernel.getLength())), + windowPost_(padFront ? + ceil(kernel.getLength()) : 0 ) {} private: - const kernel_base& kernel_; + const KernelBase& kernel_; std::vector> points_; - const bool pad_front_; - const double kernel_offset_; - const int window_pre_; - const int window_post_; + const bool padFront_; + const double kernelOffset_; + const int windowPre_; + const int windowPost_; public: /** - Append control points to the trajectory - point can wrap Keys, constants, or other computations. - A 2d interpolation mesh can be constructed by using evaluations of - an orthogonal TrajectoryModel as the control points. - */ - void add_control_point(Expression point) + * Append control points to the trajectory + * point can wrap Keys, constants, or other computations. + * A 2d interpolation mesh can be constructed by using evaluations of + * an orthogonal TrajectoryModel as the control points. + * @param point the control point to insert + */ + void addControlPoint(Expression point) { points_.push_back(point); } @@ -94,145 +109,212 @@ class TrajectoryModel /** get a read-only reference to the current control points */ - const std::vector> get_control_points(){ + const std::vector> getControlPoints() const { return points_; } /** - Sample the trajectory model at a given timestamp - The return value is an expression containing the spline evalation algorithm. - timestamp is an Expression, allowing the user to inject solution-dependent sample rate corrections before sampling the spline. - - window_start and window_end are used to limit the number of control points - considered in the resulting Expression. - Setting window_end beyond `get_points.size()` will not add future points to the resulting expression. - Timestamps always start at zero, and control points are spaced with an interval of exactly 1.0. - Default window uses all available control points. - - */ - Expression sample_trajectory( + * Sample the trajectory model at a given timestamp + * The return value is an expression containing the spline evalation algorithm. + * timestamp is an Expression, allowing the user to inject solution-dependent + * corrections before sampling the spline. + * + * windowStart and windowEnd are used to limit the number of control points + * considered in the resulting Expression. + * Setting windowEnd beyond `get_points.size()` will not add future points to the resulting expression. + * Timestamps always start at zero, and control points are spaced with an interval of exactly 1.0. + * Default window uses all available control points. + * + * @param timestamp the evaluation point Expression + * @param windowStart the earliest plausible value for timestamp, + * reduces the number of control points evaluated + * @param windowEnd the latest plausible value for timestamp, + * reduces the number of control points evaluated. + * a negative value (default) sample up until the `get_points.size()-1`-th point. + */ + Expression sampleTrajectory( Double_ timestamp, - double window_start = 0, - double window_end = -1); + double windowStart = 0, + double windowEnd = -1) const; /** - Sample the Nth derivative of the trajectory. - TangentVector obeys vector-space rules, so all subsequent derivatives share the same type. - setting derivative to zero yields the logspace difference between `sample_trajectory()` and the pose just prior to window_start - */ - Expression::TangentVector> sample_trajectory_d( + * Sample the Nth derivative of the trajectory. + * TangentVector obeys vector-space rules, so all subsequent derivatives share the same type. + * + * @param timestamp the evaluation point Expression + * @param windowStart the earliest plausible value for timestamp, + * reduces the number of control points evaluated + * @param windowEnd the latest plausible value for timestamp, + * reduces the number of control points evaluated. + * a negative value (default) sample up until the `get_points.size()-1`-th point. + * @param derivative how many times to differentiate (1: velocity, 2: acceleration, etc) + * a derivative of zero is equivalent to `logmap(sampleTrajectory(windowStart-kernel.getLength()), sampleTrajectory(timestamp))` + */ + Expression::TangentVector> sampleTrajectoryDerivative( Double_ timestamp, - double window_start = 0, - double window_end = -1, - size_t derivative = 1); + double windowStart = 0, + double windowEnd = -1, + size_t derivative = 1) const; -public: -//private: +private: /** - interpolate calls sample_kernel and cumulative_path_sum to apply the kernel over a subset of the control points. - the return type is an Expression that depends on points and timestamp, - performing an automatically differentiated kernel convolution at linearisation time. - */ - Expression kernel_interpolate( - const kernel_base& kernel, // the kernel to use for interpolation + * kernelInterpolate evaluates the spline by collecting a subset of the + * control points and their respective weights, then passes them to cumulativePathSum. + * The return value is an Expression referring to timestamp, and all control points in points[start] : points[end] + * @param kernel a reference to the kernel being sampled for weights + * @param timestamp an Expression for the evaluation point in the resulting curve + * @param points a reference to the vector of control points + * @param start the index of the first control point to sample + * @param end the index of the last control point to sample + */ + static Expression kernelInterpolate( + const KernelBase& kernel, // the kernel to use for interpolation const Double_& timestamp, // the timestamp to interpolate at const std::vector>& points, // a reference to the control points to interpolate size_t start, size_t end // the range of control points to interpolate - ); - Expression::TangentVector> kernel_interpolate_d( - const kernel_base& kernel, // the kernel to use for interpolation + + + /** + * kernelInterpolateDerivative evaluates derivatives of the spline by + * collecting a subset of the control points and their respective weights, + * then passes them to cumulativePathSum. + * The return value is an Expression referring to timestamp and all control points in points[start] : points[end] + * @param kernel a reference to the kernel being sampled for weights + * @param timestamp an Expression for the evaluation point in the resulting curve + * @param points a reference to the vector of control points + * @param start the index of the first control point to sample + * @param end the index of the last control point to sample + * @param derivative which derivative to evaluate (1: velocity, 2:acceleration, etc) + */ + static Expression::TangentVector> kernelInterpolateDerivative( + const KernelBase& kernel, // the kernel to use for interpolation const Double_& timestamp, // the timestamp to interpolate at const std::vector>& points, // a reference to the control points to interpolate size_t start, size_t end, // the range of control points to interpolate size_t derivative = 1); + + /** - sample the kernel at a range of points - timestamp is is in units of samples. Be sure to divide your timestamp by the sample rate - start and end are indicies into (and exact timestamps) the vector of control points. - */ - std::vector sample_kernel( - const kernel_base& kernel, + * Sample the kernel at a range of points + * timestamp is in units of samples. Be sure to divide your timestamp by the sample rate + * start and end are indicies into (and exact timestamps) the vector of control points. + * @param kernel a reference to the kernel being sampled + * @param timestamp an Expression for the evaluation point in the kernel + * @param start the offset into the kernel (subtracted from timestamp) + * @param end generate end - start samples + * @param derivative how many times to differentiate the kernel + */ + static std::vector sampleKernel( + const KernelBase& kernel, const Double_& timestamp, size_t start, size_t end, size_t derivative = 0); /** - accumulates the log-space differences between points, - with each log-space vector scaled by its respective weight. + * computes a geodesic weighted sum, by weighting the steps in the path + * from the first point and performing the accumulation in the logmap. + * + * Weights are the integral of a normal convolutional kernel function + * the convolutional nature of kernel requires a time-reversal. + * interpret this a the sum here performing a numerical derivative, + * and the kernel being pre-baked with the integral operator to compensate. + * the earliest entries of cdfWeights should be about 1.0. + * @param points the control points to accumulate + * @param cdfWeights the weights for the logspace differences. + * cdfWeights are normally drawn from a time-reversed integral of a + * Probability Density Function, ie, a 'Cumulative Distribution Function' */ - Expression cumulative_path_sum( + static Expression cumulativePathSum( const std::vector>& points, - const std::vector& cdf_weights); + const std::vector& cdfWeights); /** - logspace form of the cumulative path sum, used for derivatives + * computes the logmap form of a weighted sum of differences. + * @param points the control points to accumulate + * @param weights the weights to apply to each logspace difference. */ - Expression::TangentVector> cumulative_path_sum_d( + static Expression::TangentVector> cumulativePathSumDerivative( const std::vector>& points, const std::vector& weights); }; // class TrajectoryModel - +/** + * user-facing api, + * do input sanitisation + * apply filter ring-down compensations for the window domain. + * pass references to the member variables + */ template -Expression TrajectoryModel::sample_trajectory( +Expression TrajectoryModel::sampleTrajectory( Double_ timestamp, - double window_start, - double window_end) + double windowStart, + double windowEnd) const { - + // apply padding to the window + int start = floor(windowStart) - windowPre_; + int end = ceil(windowEnd) + windowPost_; // sanitise inputs - int start = floor(window_start) - window_pre_; - int end = ceil(window_end) + window_post_; if(start < 0) start = 0; - if(window_end < 0 || end > points_.size()) end = points_.size(); + if(windowEnd < 0 || end > points_.size()) end = points_.size(); + + // apply filter delay correction + Double_ kernelTime = timestamp + Double_(kernelOffset_); - // pass to internal method - return kernel_interpolate(kernel_, timestamp, points_, start, end); + // pass to internal methods + return kernelInterpolate(kernel_, kernelTime, points_, start, end); } +/** + * user-facing api, + * do input sanitisation + * apply filter ring-down compensations for the window domain. + * pass references to the member variables + */ template -Expression::TangentVector> TrajectoryModel::sample_trajectory_d( +Expression::TangentVector> TrajectoryModel::sampleTrajectoryDerivative( Double_ timestamp, - double window_start, - double window_end, - size_t derivative) + double windowStart, + double windowEnd, + size_t derivative) const { + // apply padding to the window + int start = floor(windowStart) - windowPre_; + int end = ceil(windowEnd) + windowPost_; // sanitise inputs - int start = floor(window_start) - window_pre_; - int end = ceil(window_end) + window_post_; if(start < 0) start = 0; - if(window_end < 0 || end > points_.size()) end = points_.size(); + if(windowEnd < 0 || end > points_.size()) end = points_.size(); - // pass to internal method - return kernel_interpolate_d(kernel_, timestamp, points_, start, end, derivative); + // apply filter delay correction + Double_ kernelTime = timestamp + Double_(kernelOffset_); + + // pass to internal methods + return kernelInterpolateDerivative(kernel_, kernelTime, points_, start, end, derivative); } -/* -// compute logspace vectors between adjacent points, -// scale the vectors by the CDF based weight for the relevant point -// accumulate the logspace path as a relative vector from the first point -*/ +/** + * compute a weighted sum of logmap differences between adjacent points applied to the first point + */ template -Expression TrajectoryModel::cumulative_path_sum( +Expression TrajectoryModel::cumulativePathSum( const std::vector>& points, - const std::vector& cdf_weights) + const std::vector& cdfWeights) { - return expmap(points[0], cumulative_path_sum_d(points, cdf_weights)); + return expmap(points[0], cumulativePathSumDerivative(points, cdfWeights)); } template -Expression::TangentVector> TrajectoryModel::cumulative_path_sum_d( +Expression::TangentVector> TrajectoryModel::cumulativePathSumDerivative( const std::vector>& points, const std::vector& weights) { @@ -240,7 +322,7 @@ Expression::TangentVector> TrajectoryModel::cumulative_pat // compute logspace vectors between adjacent points, // scale the vectors by the weight for the relevant point // accumulate the logspace vector - // we skip the first weight; cumulative_path_sum assumes it is equal to 1.0 + // we skip the first weight; cumulativePathSum assumes it is equal to 1.0 for (size_t i = 1; i < points.size(); i++) { v += weights[i] * logmap(points[i-1], points[i]); @@ -251,45 +333,45 @@ Expression::TangentVector> TrajectoryModel::cumulative_pat /** -interpolate calls sample_kernel and weighted_sum to apply the kernel over subset of the control points. +interpolate calls sampleKernel and weighted_sum to apply the kernel over subset of the control points. the return type is an Expression that depends on points and timestamp, performing an automatically differentiated kernel convolution at linearisation time. */ template -Expression TrajectoryModel::kernel_interpolate( - const kernel_base& kernel, // the kernel to use for interpolation +Expression TrajectoryModel::kernelInterpolate( + const KernelBase& kernel, // the kernel to use for interpolation const Double_& timestamp, // the timestamp to interpolate at const std::vector>& points, // a reference to the control points to interpolate size_t start, size_t end // the range of control points to interpolate ) { // copy the interval of control points that we want to interpolate within - std::vector> points_range(points.begin()+start, points.begin()+end); + std::vector> pointsRange(points.begin()+start, points.begin()+end); // generate the basis functions as expressions depending on the timestamp - std::vector weights_range = sample_kernel(kernel, timestamp, start, end, 0); + std::vector weights_range = sampleKernel(kernel, timestamp, start, end, 0); // apply the weighted sum - return cumulative_path_sum(points_range, weights_range); + return cumulativePathSum(pointsRange, weights_range); } /** -interpolate calls sample_kernel and weighted_sum to apply the kernel over subset of the control points. +interpolate calls sampleKernel and weighted_sum to apply the kernel over subset of the control points. the return type is an Expression that depends on points and timestamp, performing an automatically differentiated kernel convolution at linearisation time. */ template -Expression::TangentVector> TrajectoryModel::kernel_interpolate_d( - const kernel_base& kernel, // the kernel to use for interpolation +Expression::TangentVector> TrajectoryModel::kernelInterpolateDerivative( + const KernelBase& kernel, // the kernel to use for interpolation const Double_& timestamp, // the timestamp to interpolate at const std::vector>& points, // a reference to the control points to interpolate size_t start, size_t end, // the range of control points to interpolate size_t derivative) { // copy the interval of control points that we want to interpolate within - std::vector> points_range(points.begin()+start, points.begin()+end); + std::vector> pointsRange(points.begin()+start, points.begin()+end); // generate the basis functions as expressions depending on the timestamp - std::vector weights_range = sample_kernel(kernel, timestamp, start, end, derivative); + std::vector weights_range = sampleKernel(kernel, timestamp, start, end, derivative); // apply the weighted sum - return cumulative_path_sum_d(points_range, weights_range); + return cumulativePathSumDerivative(pointsRange, weights_range); } @@ -300,31 +382,28 @@ timestamp is is in units of samples. Be sure to divide your timestamp by the sam start and end are indicies into (and exact timestamps) the vector of control points. */ template -std::vector TrajectoryModel::sample_kernel( - const kernel_base& kernel, +std::vector TrajectoryModel::sampleKernel( + const KernelBase& kernel, const Double_& timestamp, size_t start, size_t end, size_t derivative) { - std::vector kernel_samples; + std::vector kernelSamples; // expression constructor requires us to bind to the class member method std::function)> keval = [&kernel, derivative](const double& x, OptionalJacobian<1, 1> j) { - return kernel.evaluate_d(derivative, x, j); + return kernel.evaluateDerivative(derivative, x, j); }; for(size_t i = start; i < end; i++) { - Double_ kernel_time = - Double_(kernel_offset_) - + timestamp - - Double_(double(i)); - kernel_samples.push_back(Double_(keval, kernel_time)); + Double_ kernelTime = timestamp - Double_(double(i)); + kernelSamples.push_back(Double_(keval, kernelTime)); } - return kernel_samples; + return kernelSamples; } diff --git a/gtsam/basis/polynomial/kernels.h b/gtsam/basis/polynomial/kernels.h index a47ee64d68..a3f587e483 100644 --- a/gtsam/basis/polynomial/kernels.h +++ b/gtsam/basis/polynomial/kernels.h @@ -10,34 +10,35 @@ namespace gtsam { -class kernel_base +/** + * A base class for convolutional kernel functions + */ +class KernelBase { public: - - /** - evaluate the kernel function at t - */ + /** evaluate the kernel function at time t */ virtual double evaluate(const double& t, OptionalJacobian<1, 1> H = {}) const = 0; - /** - evaluate the kernel function's nth derivative at t - */ - virtual double evaluate_d(size_t derivative, double t, OptionalJacobian<1, 1> H = {}) const = 0; - - // earliest value of t that returns a non-zero kernel value - virtual double get_beginning() const = 0; - // centre of the distribution - virtual double get_center() const = 0; - // last value of t that returns a non-zero kernel value - virtual double get_end() const = 0; - virtual size_t get_valid_derivatives() const = 0; - - inline double get_length() const + + /** evaluate the kernel function's Nth derivative at t */ + virtual double evaluateDerivative(size_t derivative, double t, OptionalJacobian<1, 1> H = {}) const = 0; + + /** earliest value of t that returns a non-constant kernel value */ + virtual double getBeginning() const = 0; + /** centre of the distribution */ + virtual double getCenter() const = 0; + /** latest value of t that returns a non-constant kernel value */ + virtual double getEnd() const = 0; + /** the number of times this kernel can be sensibly differentiated */ + virtual size_t getValidDerivatives() const = 0; + + /** total length where the kernel is non-zero */ + inline double getLength() const { - return get_end() - get_beginning(); + return getEnd() - getBeginning(); }; - - virtual ~kernel_base() = default; + /** default destructor */ + virtual ~KernelBase() = default; }; diff --git a/gtsam/basis/tests/testIrwinHall.cpp b/gtsam/basis/tests/testIrwinHall.cpp index 9042764c2e..6292e12a20 100644 --- a/gtsam/basis/tests/testIrwinHall.cpp +++ b/gtsam/basis/tests/testIrwinHall.cpp @@ -30,33 +30,33 @@ double tolerance = 100*epsilon; // test boundary conditions at the limits of the domain TEST( poly , Boundaries1 ) { auto poly = IrwinHall1; - EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().front()), tolerance); - EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().back()), tolerance); + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.getIntervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.getIntervals().back()), tolerance); } TEST( poly , Boundaries2 ) { auto poly = IrwinHall2; - EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().front()), tolerance); - EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().back()), tolerance); + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.getIntervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.getIntervals().back()), tolerance); } TEST( poly , Boundaries3 ) { auto poly = IrwinHall3; - EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().front()), tolerance); - EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().back()), tolerance); + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.getIntervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.getIntervals().back()), tolerance); } TEST( poly , Boundaries4 ) { auto poly = IrwinHall4; - EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().front()), tolerance); - EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().back()), tolerance); + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.getIntervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.getIntervals().back()), tolerance); } TEST( poly , Boundaries5 ) { auto poly = IrwinHall5; - EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().front()), tolerance); - EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().back()), tolerance); + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.getIntervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.getIntervals().back()), tolerance); } TEST( poly , Boundaries6 ) { auto poly = IrwinHall6; - EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().front()), tolerance); - EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.get_intervals().back()), tolerance); + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.getIntervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.getIntervals().back()), tolerance); } @@ -64,42 +64,42 @@ TEST( poly , Boundaries6 ) { // test continuity where the polynomial pieces join TEST( IrwinHall , Continuity1 ) { auto poly = IrwinHall1; - for(const double &t : poly.get_intervals()) { + for(const double &t : poly.getIntervals()) { EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), poly.evaluate(t-epsilon), tolerance ); } } TEST( IrwinHall , Continuity2 ) { auto poly = IrwinHall2; - for(const double &t : poly.get_intervals()) { + for(const double &t : poly.getIntervals()) { EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), poly.evaluate(t-epsilon), tolerance ); } } TEST( IrwinHall , Continuity3 ) { auto poly = IrwinHall3; - for(const double &t : poly.get_intervals()) { + for(const double &t : poly.getIntervals()) { EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), poly.evaluate(t-epsilon), tolerance ); } } TEST( IrwinHall , Continuity4 ) { auto poly = IrwinHall4; - for(const double &t : poly.get_intervals()) { + for(const double &t : poly.getIntervals()) { EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), poly.evaluate(t-epsilon), tolerance ); } } TEST( IrwinHall , Continuity5 ) { auto poly = IrwinHall5; - for(const double &t : poly.get_intervals()) { + for(const double &t : poly.getIntervals()) { EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), poly.evaluate(t-epsilon), tolerance ); } } TEST( IrwinHall , Continuity6 ) { auto poly = IrwinHall6; - for(const double &t : poly.get_intervals()) { + for(const double &t : poly.getIntervals()) { EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), poly.evaluate(t-epsilon), tolerance ); } @@ -110,60 +110,60 @@ TEST( IrwinHall , Continuity6 ) { TEST( IrwinHall , DerivativeContinuity1 ) { auto poly = IrwinHall1; int max_d = poly.order-1; - for(const double &t : poly.get_intervals()) { + for(const double &t : poly.getIntervals()) { for(int d=0; d jacobian; - poly.evaluate_d(d, t, &jacobian); - double derivative = poly.evaluate_d(d+1, t); + poly.evaluateDerivative(d, t, &jacobian); + double derivative = poly.evaluateDerivative(d+1, t); EXPECT_DOUBLES_EQUAL(jacobian[0], derivative, tolerance ); } } @@ -46,11 +46,11 @@ TEST( IrwinHall , DerivativeIsJacobian1 ) { TEST( IrwinHallCDF , DerivativeIsJacobian2 ) { auto poly = IrwinHallCDF6; int max_d = poly.order-2; - for(const double &t : poly.get_intervals()) { + for(const double &t : poly.getIntervals()) { for(int d=0; d jacobian; - poly.evaluate_d(d, t, &jacobian); - double derivative = poly.evaluate_d(d+1, t); + poly.evaluateDerivative(d, t, &jacobian); + double derivative = poly.evaluateDerivative(d+1, t); EXPECT_DOUBLES_EQUAL(jacobian[0], derivative, tolerance ); } } diff --git a/gtsam/basis/tests/testTrajectoryModel.cpp b/gtsam/basis/tests/testTrajectoryModel.cpp index 458625ff4e..f2e489b455 100644 --- a/gtsam/basis/tests/testTrajectoryModel.cpp +++ b/gtsam/basis/tests/testTrajectoryModel.cpp @@ -28,7 +28,7 @@ TEST( TrajectoryModel , BasicBounds ) { gtsam::Values v; // needed for evaluating Expressions // choose a cubic spline - const kernel_base& basis_function = kernels::IrwinHallCDF2; + const KernelBase& basis_function = kernels::IrwinHallCDF2; // specify some control points std::vector path({ @@ -42,7 +42,7 @@ TEST( TrajectoryModel , BasicBounds ) { // bool pad_front = false; // with padding at the back, // non-constant timestamps will be in - // [0 : path.size()-1 + basis_function.get_length()] + // [0 : path.size()-1 + basis_function.getLength()] TrajectoryModel model(basis_function, path/*, pad_front*/); Key t_key = Key(0); // choose a key to carry the timestamp @@ -50,18 +50,18 @@ TEST( TrajectoryModel , BasicBounds ) { Double_ timestamp = Double_(t_key); // associate the timestamp with the key // construct an Expression that samples the trajectory - Double_ sample = model.sample_trajectory(timestamp); + Double_ sample = model.sampleTrajectory(timestamp); // bounds check v.update(t_key, 0); CHECK(assert_equal(path.front().value(v), sample.value(v), tolerance)); - v.update(t_key, path.size()-1 + basis_function.get_length()); + v.update(t_key, path.size()-1 + basis_function.getLength()); CHECK(assert_equal(path.back().value(v), sample.value(v), tolerance)); // mid value - v.update(t_key, (path.size() + basis_function.get_length())/2); + v.update(t_key, (path.size() + basis_function.getLength())/2); CHECK(assert_equal((path.front().value(v)+path.back().value(v))/2, sample.value(v), tolerance)); } @@ -72,7 +72,7 @@ TEST( TrajectoryModel , FrontPadding ) { gtsam::Values v; // needed for evaluating Expressions // choose a cubic spline - const kernel_base& basis_function = kernels::IrwinHallCDF2; + const KernelBase& basis_function = kernels::IrwinHallCDF2; // specify some control points std::vector path({ @@ -84,7 +84,7 @@ TEST( TrajectoryModel , FrontPadding ) { bool pad_front = true; // with padding at the front, // non-constant timestamps will be in - // [-basis_function.get_length() : path.size()-1] + // [-basis_function.getLength() : path.size()-1] TrajectoryModel model(basis_function, path, pad_front); Key t_key = Key(0); // choose a key to carry the timestamp @@ -92,18 +92,18 @@ TEST( TrajectoryModel , FrontPadding ) { Double_ timestamp = Double_(t_key); // associate the timestamp with the key // construct an Expression that samples the trajectory - Double_ sample = model.sample_trajectory(timestamp); + Double_ sample = model.sampleTrajectory(timestamp); // bounds check - v.update(t_key, -basis_function.get_length()); + v.update(t_key, -basis_function.getLength()); CHECK(assert_equal(path.front().value(v), sample.value(v), tolerance)); v.update(t_key, path.size()-1); CHECK(assert_equal(path.back().value(v), sample.value(v), tolerance)); // mid value - v.update(t_key, (-basis_function.get_length() + (path.size()))/2); + v.update(t_key, (-basis_function.getLength() + (path.size()))/2); CHECK(assert_equal((path.front().value(v)+path.back().value(v))/2, sample.value(v), tolerance)); } @@ -122,7 +122,7 @@ TEST( TrajectoryModel , WindowTruncation ) { gtsam::Values v; // needed for evaluating Expressions // choose a cubic spline - const kernel_base& basis_function = kernels::IrwinHallCDF2; + const KernelBase& basis_function = kernels::IrwinHallCDF2; // specify some control points std::vector path({ @@ -155,14 +155,14 @@ TEST( TrajectoryModel , WindowTruncation ) { Double_ timestamp = Double_(t_key); // associate the timestamp with the key // construct an Expression that samples from the entire trajectory - Double_ sample = model.sample_trajectory(timestamp); + Double_ sample = model.sampleTrajectory(timestamp); // construct an Expression that samples from a subset of the trajectory double window_start = 6; double window_end = 8; - Double_ trunc_back = model.sample_trajectory(timestamp, 0, window_end); - Double_ trunc_front = model.sample_trajectory(timestamp, window_start, -1); - Double_ trunc_both = model.sample_trajectory(timestamp, window_start, window_end); + Double_ trunc_back = model.sampleTrajectory(timestamp, 0, window_end); + Double_ trunc_front = model.sampleTrajectory(timestamp, window_start, -1); + Double_ trunc_both = model.sampleTrajectory(timestamp, window_start, window_end); for(double t=window_start; t path({ @@ -199,7 +199,7 @@ TEST( TrajectoryModel , TypesPose3 ) { Double_ timestamp = Double_(t_key); // associate the timestamp with the key // construct an Expression that samples from the entire trajectory - Pose3_ sample = model.sample_trajectory(timestamp); + Pose3_ sample = model.sampleTrajectory(timestamp); v.update(t_key, 3.5); CHECK(assert_equal(Pose3(Rot3(), Point3(0,1,0)), sample.value(v), tolerance)); @@ -208,13 +208,11 @@ TEST( TrajectoryModel , TypesPose3 ) { - - TEST( TrajectoryModel , DerivativePose3 ) { gtsam::Values v; // needed for evaluating Expressions // choose a cubic spline - const kernel_base& basis_function = kernels::IrwinHallCDF2; + const KernelBase& basis_function = kernels::IrwinHallCDF2; // specify some control points std::vector path({ @@ -238,10 +236,10 @@ TEST( TrajectoryModel , DerivativePose3 ) { Double_ t_eps = Double_(t_eps_key); // construct Expressions that sample the trajectory - Pose3_ sample_ref = model.sample_trajectory(t_ref); - Pose3_ sample_eps = model.sample_trajectory(t_eps); + Pose3_ sample_ref = model.sampleTrajectory(t_ref); + Pose3_ sample_eps = model.sampleTrajectory(t_eps); // construct an expression that captures the tangent - auto sample_d = model.sample_trajectory_d(t_ref,0,-1,1); + auto sample_d = model.sampleTrajectoryDerivative(t_ref,0,-1,1); for(double sample_time = 0; sample_time<7; sample_time+=0.1) { @@ -263,7 +261,7 @@ TEST( TrajectoryModel , NthDerivativePose3 ) { gtsam::Values v; // needed for evaluating Expressions // choose a cubic spline - const kernel_base& basis_function = kernels::IrwinHallCDF2; + const KernelBase& basis_function = kernels::IrwinHallCDF2; // specify some control points std::vector path({ @@ -286,13 +284,13 @@ TEST( TrajectoryModel , NthDerivativePose3 ) { Double_ t_ref = Double_(t_ref_key); Double_ t_eps = Double_(t_eps_key); - for(size_t n=0; n(t_ref_key, sample_time); @@ -317,7 +315,7 @@ TEST( TrajectoryModel , MeshModel ) { gtsam::Values v; // needed for evaluating Expressions // choose a cubic spline - const kernel_base& basis_function = kernels::IrwinHallCDF2; + const KernelBase& basis_function = kernels::IrwinHallCDF2; // specify some control points std::vector> mesh({ @@ -340,7 +338,6 @@ TEST( TrajectoryModel , MeshModel ) { Double_(-2.0), }, { - Double_(5.0), Double_(2.0), Double_(9.0), @@ -362,11 +359,11 @@ TEST( TrajectoryModel , MeshModel ) { for(const auto& path : mesh){ TrajectoryModel row(basis_function, path); rows.push_back(row); - auto row_sample = row.sample_trajectory(x_expr); + auto row_sample = row.sampleTrajectory(x_expr); col_path.push_back(row_sample); } TrajectoryModel col(basis_function, col_path); - auto sample = col.sample_trajectory(y_expr); + auto sample = col.sampleTrajectory(y_expr); for(double x=0; x<7; x+=0.1) From 7cab6c8da078f4ab28fd6ed2892837f259ad3f94 Mon Sep 17 00:00:00 2001 From: Brett Downing Date: Wed, 13 Aug 2025 13:44:59 +1000 Subject: [PATCH 12/16] adds AsVectorSpace CRTP manifold to vectorspace adaptor --- gtsam/geometry/AsVectorSpace.h | 89 +++++++++++++++++++ gtsam/geometry/tests/testAsVectorSpace.cpp | 99 ++++++++++++++++++++++ 2 files changed, 188 insertions(+) create mode 100644 gtsam/geometry/AsVectorSpace.h create mode 100644 gtsam/geometry/tests/testAsVectorSpace.cpp diff --git a/gtsam/geometry/AsVectorSpace.h b/gtsam/geometry/AsVectorSpace.h new file mode 100644 index 0000000000..dbf1b951e8 --- /dev/null +++ b/gtsam/geometry/AsVectorSpace.h @@ -0,0 +1,89 @@ +#pragma once + +#include + +/** + * Adapter to 'hack' VectorSpace traits onto a class that only satisfies Manifold traits + * This class assumes that a there is no "most sensible" choice of embedding, + * so composition will always be meaningless regardless of embedding. + * This class provides rough and ready implementations for expmap and logmap + * based on extrapolations of ManifoldTraits::Local and ManifoldTraits::Retract + * The Identity vector is chosen as the default constructor for the base class. + */ + +namespace gtsam { + +using namespace gtsam::internal; + +template +struct AsVectorSpace : public Class +{ + typedef typename traits::TangentVector TangentVector; + + /** + * default and copy constructor allows up-cast from Class + */ + AsVectorSpace(const Class& c = Identity()):Class(c){} + + /** + * Identity can be anywhere, as long as it's consistent + * Choose the default constructor for simplicity. + */ + static Class Identity() { return Class();} + + /** + * satisfy vector constructor requirement imposed by + * static Class VectorSpaceImpl::Expmap(const TangentVector&, ChartJacobian Hv) + */ + AsVectorSpace(const TangentVector& v): + Class(Identity().retract(v)) {} + //Class(traits::Retract(Identity(), v)) {} + + /** + * Local and Retract are provided by ManifoldTraits + * VectorSpace Compose, Between, Inverse require binary +- and unary - operators. + */ + Class operator+ (const Class& rhs) const { + auto v_rhs = traits::Local(Identity(), rhs); + return traits::Retract(*this, v_rhs); + } + Class operator- (const Class& rhs) const { + auto v_rhs = traits::Local(Identity(), rhs); + return traits::Retract(*this, -v_rhs); + } + Class operator- () const { + auto v_rhs = traits::Local(Identity(), *this); + return traits::Retract(Identity(), -v_rhs); + } + Class operator+ (const TangentVector& rhs) const { + return traits::Retract(*this, rhs); + } + Class operator- (const TangentVector& rhs) const { + return traits::Retract(*this, -rhs); + } + + /** + * return a vector describing the difference between this and Identity + * required by VectorSpaceImpl::Logmap + */ + TangentVector vector() const { + return traits::Local(Identity(), *this); + } + + +}; // class AsVectorSpace + + +template +struct traits> : + public internal::VectorSpaceTraits> , + public Testable> +{}; + +template +struct traits> : + public internal::VectorSpaceTraits>, + public Testable> +{}; + +} // namespace gtsam diff --git a/gtsam/geometry/tests/testAsVectorSpace.cpp b/gtsam/geometry/tests/testAsVectorSpace.cpp new file mode 100644 index 0000000000..6cdff6ca7b --- /dev/null +++ b/gtsam/geometry/tests/testAsVectorSpace.cpp @@ -0,0 +1,99 @@ +/** + * @file testCartesinProduct.cpp + * @brief Test vector space properties of extrapolated manifolds + * @author Brett Downing + * @date April 2025 + */ + + + +#include +#include +#include +#include +#include +#include +#include + + +using namespace gtsam; + + +//GTSAM_CONCEPT_TESTABLE_INST(AsVectorSpace) +GTSAM_CONCEPT_TESTABLE_INST(AsVectorSpace) +//GTSAM_CONCEPT_LIE_INST(StampedPoint3) + +//****************************************************************************** +//TEST( AsVectorSpace , Cal3Constructor) { +// AsVectorSpace p; +//} +//****************************************************************************** +TEST( AsVectorSpace , Cal3FisheyeConstructor) { + AsVectorSpace p; +} + +//****************************************************************************** +//TEST( AsVectorSpace , Cal3UpCast) { +// Cal3 q(); +// AsVectorSpace p = q; +//} +//****************************************************************************** +TEST( AsVectorSpace , Cal3FisheyeUpCast) { + Cal3Fisheye q = Cal3Fisheye(); + AsVectorSpace p(q);// = q; +} + +//****************************************************************************** +//TEST( AsVectorSpace , Cal3DownCast) { +// AsVectorSpace p(); +// const Cal3& q = p; +//} +//****************************************************************************** +TEST( AsVectorSpace , Cal3FisheyeDownCast) { + AsVectorSpace p = AsVectorSpace(); + const Cal3Fisheye& q = p; +} + + +//****************************************************************************** +//TEST( AsVectorSpace , Cal3Concept) { +// GTSAM_CONCEPT_ASSERT(IsManifold>); +// GTSAM_CONCEPT_ASSERT(IsGroup>); +// GTSAM_CONCEPT_ASSERT(IsVectorSpace>); +//} + +//****************************************************************************** +TEST( AsVectorSpace , Cal3FisheyeConcept) { + GTSAM_CONCEPT_ASSERT(IsManifold>); + GTSAM_CONCEPT_ASSERT(IsGroup>); + GTSAM_CONCEPT_ASSERT(IsVectorSpace>); +} + + +//****************************************************************************** +//TEST( AsVectorSpace , Cal3Invariants) { +// traits::TangentVector va, vb; +// va << 3,4,5,6,7; +// vb << 5,6,7,8,9; +// AsVectorSpace ca(va), cb(vb); +// EXPECT(check_group_invariants(ca, cb)); +// EXPECT(check_manifold_invariants(ca, cb)); +//} + + +//****************************************************************************** +TEST( AsVectorSpace , Cal3FisheyeInvariants) { + traits::TangentVector va, vb; + va << 3,4,5,6,7,8,9,10,11; + vb << 5,6,7,8,9,10,11,12,13; + AsVectorSpace ca(va), cb(vb); + EXPECT(check_group_invariants(ca, cb)); + EXPECT(check_manifold_invariants(ca, cb)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From e3911258d0fd605a1c4206a135005755534acfd2 Mon Sep 17 00:00:00 2001 From: Brett Downing Date: Wed, 13 Aug 2025 15:05:29 +1000 Subject: [PATCH 13/16] extends AsVectorSpace tests over common Cal3 types, adds test for CartesianProduct between multiplicative and additive groups. --- gtsam/geometry/tests/testAsVectorSpace.cpp | 120 +++++++++++++----- gtsam/geometry/tests/testCartesianProduct.cpp | 50 ++++++-- 2 files changed, 128 insertions(+), 42 deletions(-) diff --git a/gtsam/geometry/tests/testAsVectorSpace.cpp b/gtsam/geometry/tests/testAsVectorSpace.cpp index 6cdff6ca7b..759b849bfc 100644 --- a/gtsam/geometry/tests/testAsVectorSpace.cpp +++ b/gtsam/geometry/tests/testAsVectorSpace.cpp @@ -11,7 +11,9 @@ #include #include #include -#include +#include +#include +#include #include #include @@ -19,35 +21,64 @@ using namespace gtsam; -//GTSAM_CONCEPT_TESTABLE_INST(AsVectorSpace) +GTSAM_CONCEPT_TESTABLE_INST(AsVectorSpace) +GTSAM_CONCEPT_TESTABLE_INST(AsVectorSpace) +GTSAM_CONCEPT_TESTABLE_INST(AsVectorSpace) GTSAM_CONCEPT_TESTABLE_INST(AsVectorSpace) -//GTSAM_CONCEPT_LIE_INST(StampedPoint3) //****************************************************************************** -//TEST( AsVectorSpace , Cal3Constructor) { -// AsVectorSpace p; -//} +TEST( AsVectorSpace , Cal3fConstructor) { + AsVectorSpace p; +} +//****************************************************************************** +TEST( AsVectorSpace , Cal3DS2Constructor) { + AsVectorSpace p; +} +//****************************************************************************** +TEST( AsVectorSpace , Cal3_S2Constructor) { + AsVectorSpace p; +} //****************************************************************************** TEST( AsVectorSpace , Cal3FisheyeConstructor) { AsVectorSpace p; } //****************************************************************************** -//TEST( AsVectorSpace , Cal3UpCast) { -// Cal3 q(); -// AsVectorSpace p = q; -//} +TEST( AsVectorSpace , Cal3fUpCast) { + Cal3f q = Cal3f(); + AsVectorSpace p(q); +} +//****************************************************************************** +TEST( AsVectorSpace , Cal3_S2UpCast) { + Cal3_S2 q = Cal3_S2(); + AsVectorSpace p(q); +} +//****************************************************************************** +TEST( AsVectorSpace , Cal3DS2UpCast) { + Cal3DS2 q = Cal3DS2(); + AsVectorSpace p(q); +} //****************************************************************************** TEST( AsVectorSpace , Cal3FisheyeUpCast) { Cal3Fisheye q = Cal3Fisheye(); - AsVectorSpace p(q);// = q; + AsVectorSpace p(q); } //****************************************************************************** -//TEST( AsVectorSpace , Cal3DownCast) { -// AsVectorSpace p(); -// const Cal3& q = p; -//} +TEST( AsVectorSpace , Cal3fDownCast) { + AsVectorSpace p = AsVectorSpace(); + const Cal3f& q = p; +} +//****************************************************************************** +TEST( AsVectorSpace , Cal3_S2DownCast) { + AsVectorSpace p = AsVectorSpace(); + const Cal3_S2& q = p; +} +//****************************************************************************** +TEST( AsVectorSpace , Cal3DS2DownCast) { + AsVectorSpace p = AsVectorSpace(); + const Cal3DS2& q = p; +} //****************************************************************************** TEST( AsVectorSpace , Cal3FisheyeDownCast) { AsVectorSpace p = AsVectorSpace(); @@ -56,12 +87,23 @@ TEST( AsVectorSpace , Cal3FisheyeDownCast) { //****************************************************************************** -//TEST( AsVectorSpace , Cal3Concept) { -// GTSAM_CONCEPT_ASSERT(IsManifold>); -// GTSAM_CONCEPT_ASSERT(IsGroup>); -// GTSAM_CONCEPT_ASSERT(IsVectorSpace>); -//} - +TEST( AsVectorSpace , Cal3fConcept) { + GTSAM_CONCEPT_ASSERT(IsManifold>); + GTSAM_CONCEPT_ASSERT(IsGroup>); + GTSAM_CONCEPT_ASSERT(IsVectorSpace>); +} +//****************************************************************************** +TEST( AsVectorSpace , Cal3_S2Concept) { + GTSAM_CONCEPT_ASSERT(IsManifold>); + GTSAM_CONCEPT_ASSERT(IsGroup>); + GTSAM_CONCEPT_ASSERT(IsVectorSpace>); +} +//****************************************************************************** +TEST( AsVectorSpace , Cal3DS2Concept) { + GTSAM_CONCEPT_ASSERT(IsManifold>); + GTSAM_CONCEPT_ASSERT(IsGroup>); + GTSAM_CONCEPT_ASSERT(IsVectorSpace>); +} //****************************************************************************** TEST( AsVectorSpace , Cal3FisheyeConcept) { GTSAM_CONCEPT_ASSERT(IsManifold>); @@ -71,16 +113,32 @@ TEST( AsVectorSpace , Cal3FisheyeConcept) { //****************************************************************************** -//TEST( AsVectorSpace , Cal3Invariants) { -// traits::TangentVector va, vb; -// va << 3,4,5,6,7; -// vb << 5,6,7,8,9; -// AsVectorSpace ca(va), cb(vb); -// EXPECT(check_group_invariants(ca, cb)); -// EXPECT(check_manifold_invariants(ca, cb)); -//} - - +TEST( AsVectorSpace , Cal3fInvariants) { + traits::TangentVector va, vb; + va << 3; + vb << 5; + AsVectorSpace ca(va), cb(vb); + EXPECT(check_group_invariants(ca, cb)); + EXPECT(check_manifold_invariants(ca, cb)); +} +//****************************************************************************** +TEST( AsVectorSpace , Cal3_S2Invariants) { + traits::TangentVector va, vb; + va << 3,4,5,6,7; + vb << 5,6,7,8,9; + AsVectorSpace ca(va), cb(vb); + EXPECT(check_group_invariants(ca, cb)); + EXPECT(check_manifold_invariants(ca, cb)); +} +//****************************************************************************** +TEST( AsVectorSpace , Cal3DS2Invariants) { + traits::TangentVector va, vb; + va << 3,4,5,6,7,8,9,10,11; + vb << 5,6,7,8,9,10,11,12,13; + AsVectorSpace ca(va), cb(vb); + EXPECT(check_group_invariants(ca, cb)); + EXPECT(check_manifold_invariants(ca, cb)); +} //****************************************************************************** TEST( AsVectorSpace , Cal3FisheyeInvariants) { traits::TangentVector va, vb; diff --git a/gtsam/geometry/tests/testCartesianProduct.cpp b/gtsam/geometry/tests/testCartesianProduct.cpp index ed396c9df0..04a7f88ad2 100644 --- a/gtsam/geometry/tests/testCartesianProduct.cpp +++ b/gtsam/geometry/tests/testCartesianProduct.cpp @@ -15,6 +15,9 @@ #include #include +#include +#include + using namespace gtsam; @@ -25,35 +28,60 @@ typedef CartesianProduct StampedPose3; // split variant of pose3 allows comparing interpolate to interpolateRT typedef CartesianProduct SplitPose3; typedef CartesianProduct StampedSplitPose3; +// bind a multiplicative group to an additive group +typedef CartesianProduct> CameraPose; GTSAM_CONCEPT_TESTABLE_INST(StampedPoint3) -//GTSAM_CONCEPT_LIE_INST(StampedPoint3) +GTSAM_CONCEPT_LIE_INST(StampedPoint3) + +GTSAM_CONCEPT_TESTABLE_INST(CameraPose) +GTSAM_CONCEPT_LIE_INST(CameraPose) + + static Point3 P(0.2, 0.7, -2); static StampedPoint3 S(P, 0.1); //****************************************************************************** -TEST( CartesianProduct , Constructor) { +TEST( CartesianProduct , StampedPoint3Constructor) { StampedPoint3 p; } +//****************************************************************************** +TEST( CartesianProduct , CameraPoseConstructor) { + CameraPose p; +} //****************************************************************************** -TEST( CartesianProduct , StampedPoint3_Concept) { +TEST( CartesianProduct , StampedPoint3Concept) { GTSAM_CONCEPT_ASSERT(IsGroup); GTSAM_CONCEPT_ASSERT(IsManifold); //GTSAM_CONCEPT_ASSERT(IsVectorSpace); } +//****************************************************************************** +TEST( CartesianProduct , CameraPoseConcept) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + //GTSAM_CONCEPT_ASSERT(IsVectorSpace); +} + //****************************************************************************** -TEST( CartesianProduct , StampedPoint3_Invariants) { +TEST( CartesianProduct , StampedPoint3Invariants) { StampedPoint3 p1(Point3(1, 2, 3),7), p2(Point3(4, 5, 6),8); EXPECT(check_group_invariants(p1, p2)); EXPECT(check_manifold_invariants(p1, p2)); } //****************************************************************************** +TEST( CartesianProduct , CameraPoseInvariants) { + CameraPose p1(Pose3(Rot3::Rodrigues(0.3,0.2,0.1), Point3(1, 2, 3)), AsVectorSpace(Cal3DS2(9,8,7,6,5,4,3,2,1)) ); + CameraPose p2(Pose3(Rot3::Rodrigues(0.1,0.2,0.3), Point3(4, 5, 6)), AsVectorSpace(Cal3DS2(6,7,8,9,1,2,3,4,5)) ); + EXPECT(check_group_invariants(p1, p2)); + EXPECT(check_manifold_invariants(p1, p2)); +} +//****************************************************************************** -TEST( CartesianProduct, StampedPoint3_interpolate) { +TEST( CartesianProduct, StampedPoint3interpolate) { EXPECT(StampedPoint3(Point3(2,2,2), 2).equals(interpolate( StampedPoint3(Point3(1,2,3),0), StampedPoint3(Point3(3,2,1),4), 0.5), 1e-9)); @@ -62,14 +90,14 @@ TEST( CartesianProduct, StampedPoint3_interpolate) { //****************************************************************************** -TEST( CartesianProduct , StampedPose3_Concept) { +TEST( CartesianProduct , StampedPose3Concept) { GTSAM_CONCEPT_ASSERT(IsGroup); GTSAM_CONCEPT_ASSERT(IsManifold); //GTSAM_CONCEPT_ASSERT(IsVectorSpace); } //****************************************************************************** -TEST( CartesianProduct , StampedPose3_Invariants) { +TEST( CartesianProduct , StampedPose3Invariants) { StampedPose3 p1(Pose3(Rot3::Rodrigues(0.3,0.2,0.1), Point3(1, 2, 3)),7), p2(Pose3(Rot3::Rodrigues(0.1,0.2,0.3), Point3(4, 5, 6)),8); EXPECT(check_group_invariants(p1, p2)); @@ -77,7 +105,7 @@ TEST( CartesianProduct , StampedPose3_Invariants) { } //****************************************************************************** -TEST( CartesianProduct, StampedPose3_interpolate) { +TEST( CartesianProduct, StampedPose3interpolate) { EXPECT(StampedPose3(Pose3(Rot3::Rx(0), Point3(2,2,2)), 2).equals(interpolate( StampedPose3(Pose3(Rot3::Rx(0), Point3(1,2,3)),0), StampedPose3(Pose3(Rot3::Rx(0), Point3(3,2,1)),4), 0.5), 1e-7)); @@ -89,14 +117,14 @@ TEST( CartesianProduct, StampedPose3_interpolate) { //****************************************************************************** -TEST( CartesianProduct , StampedSplitPose3_Concept) { +TEST( CartesianProduct , StampedSplitPose3Concept) { GTSAM_CONCEPT_ASSERT(IsGroup); GTSAM_CONCEPT_ASSERT(IsManifold); //GTSAM_CONCEPT_ASSERT(IsVectorSpace); } //****************************************************************************** -TEST( CartesianProduct , StampedSplitPose3_Invariants) { +TEST( CartesianProduct , StampedSplitPose3Invariants) { StampedSplitPose3 p1(SplitPose3(Rot3::Rodrigues(0.3,0.2,0.1), Point3(1, 2, 3)),7), p2(SplitPose3(Rot3::Rodrigues(0.1,0.2,0.3), Point3(4, 5, 6)),8); EXPECT(check_group_invariants(p1, p2)); @@ -104,7 +132,7 @@ TEST( CartesianProduct , StampedSplitPose3_Invariants) { } //****************************************************************************** -TEST( CartesianProduct, StampedSplitPose3_interpolate) { +TEST( CartesianProduct, StampedSplitPose3interpolate) { EXPECT(StampedSplitPose3(SplitPose3(Rot3::Rx(0.2), Point3(2,2,2)), 2).equals(interpolate( StampedSplitPose3(SplitPose3(Rot3::Rx(0), Point3(1,2,3)),0), StampedSplitPose3(SplitPose3(Rot3::Rx(0.4), Point3(3,2,1)),4), 0.5), 1e-9)); From 5c2bc154a0b23449765bf78d94db026f5d024dd8 Mon Sep 17 00:00:00 2001 From: Brett Downing Date: Thu, 14 Aug 2025 14:46:35 +1000 Subject: [PATCH 14/16] adds a rought draft implementing Furgale2013 with extra auto-calibration --- .../ClockRecoveryContinuousTrajectory.cpp | 227 ++++++++++++++++++ 1 file changed, 227 insertions(+) create mode 100644 examples/ClockRecoveryContinuousTrajectory.cpp diff --git a/examples/ClockRecoveryContinuousTrajectory.cpp b/examples/ClockRecoveryContinuousTrajectory.cpp new file mode 100644 index 0000000000..77d19ed669 --- /dev/null +++ b/examples/ClockRecoveryContinuousTrajectory.cpp @@ -0,0 +1,227 @@ + +#include +#include +#include +#include + +/** +* In this example, we estimate pose from pictures and landmarks using bundle adjustment in the usual way +* we also capture acceleration and rotation rates with a mems IMU on the camera. +* Notably, we don't assume that the camera, accelerometer, or gryo clocks are related to each other. +* We pick one sensor to be the clock datum, the other sensors' clocks are modelled as drifting in +* relation to that datum. +* we'll choose the camera's frame rate to be the datum clock for this example. +*/ + + +// structs to collect and name the important measurements from each sensor +struct { + gtsam::Key camera_pose; // allow bundle adjustment to associate a Pose3 with a Key + double timestamp; // the timestamp (in seconds) for the shutter based on the camera's frame rate. +} shutter_event_t; + +struct { + Vector3 acceleration; // raw acceleration measurement + double timestamp; // the timestamp (in seconds) extrapolated from the accelerometer sample rate +} acceleration_t; + +struct { + Vector3 angular_velocity; // raw angular velocity measurement + double timestamp; // the timestamp (in seconds) extrapolated from the gyro sample rate +} angular_rate_t; + + + +// alias the time derivatives of pose so we can express intent through types +using Vector3_ = Expression; +using Vector6_ = Expression; + +using Pose3Deriv_ = Expression::TangentVector>; // Vector6_ +using Pose3Vel_ = Pose3Deriv_; +using Pose3Accel_ = Pose3Deriv_; + +using Point3Deriv_ = Vector3_; +using Point3Accel_ = Point3Deriv_; + +using Rot3Deriv_ = Vector3_; +using Rot3Vel_ = Rot3Deriv_; + + + +// We need to extract rotation and translation components from time derivatives of Pose3 +Rot3Deriv_ pose3DerivativeRotation(Pose3Deriv_ pose_derivative, OptionalJacobian<3, 6> H = {}){ + if (H) *H << I_3x3, Z_3x3; + return pose_derivative.seq(0,2); +} +Point3Deriv_ pose3DerivativeTranslation(Pose3Deriv_ pose_derivative, OptionalJacobian<3, 6> H = {}){ + if (H) *H << Z_3x3, I_3x3; + return pose_derivative.seq(3,5); +} +inline Rot3Deriv_ pose3DerivativeRotation(const Pose3Deriv_& p) { + return Vector3_(&pose3DerivativeRotation, p); +} +inline Point3Deriv_ pose3DerivativeTranslation(const Pose3Deriv_& p) { + return Point3Deriv_(&pose3DerivativeTranslation, p); +} + +// we also need to compose Pose3 TangentVectors with Pose3s +Pose3Deriv_ adjoint(const Pos3_& p, const Pose3Deriv_& v) +{ + return Point3_(p, &Pose3::Adjoint, v); +} + +// enum to make the sampleTrajectoryDerivative arguemnts obvious +struct TimeDerivatives{ + enum TimeDerivatives{ + velocity = 1; + acceleration = 2; + jerk = 3; + snap = 4; + crackle = 5; + pop = 6; + } +}; + + + +void simulate_run() +{ + + gtsam::Values initial_values; + + // model the trajectory with cubic splines (makes accelerations piecewise linear) + TrajectoryModel trajectory_model(kernels::IrwinHallCDF2); + // model the clock drift with a quadratic spline, (makes drift rate piecewise linear) + TrajectoryModel accel_clock_model(kernels::IrwinHallCDF1); + TrajectoryModel gyro_clock_model(kernels::IrwinHallCDF1); + + // consider the bandwidth you need to represent the dynamics you want to model + // any sensor bandwidth above this frequency contributes to graph noise. + double trajectory_sample_rate = 20; // control points per second + double clock_drift_sample_rate = 0.1; // control points per second + + + // Account for typical raw-sensor weirdness: + + // we know the IMU is not located at the camera's optical centre, we can compensate that. + // If these were Keys, we could estimate the location of the IMU relative to the camera. + Pose3_ accel_pose = Pose3_(Pose3()); + Pose3_ gyro_pose = Pose3_(Pose3()); + + // Gravity vector gives us a massive hint about orientations, so keep it in our model + Point3Accel_ gravity = Point3Accel_(Vector3(0,0,9.81)); // North East Down coords + + // the raw sensor will have a small bias to it, constrain the magnitude with a prior. + // for extremely long runs with thermal drift, these can also be a trajectory + Key accel_bias_key = Key('b', 0); + Key gyro_bias_key = Key('b', 1); + Point3Accel_ accel_bias = Point3Accel_(accel_bias_key); + Rot3Vel_ gyro_bias = Rot3Vel_(gyro_bias_key); + initial_values.insert(accel_bias_key, Vector3(0,0,0)); + initial_values.insert(gyro_bias_key, Vector3(0,0,0)); + + + + // we'll build a bunch of trajectory up front, this can be done incrementally the same way + // just make sure that the trajectory is longer than your new timestamp + the kernel length before generating expressions. + double run_length = 10.0; //seconds + // add some control points for poses + for(int i=0;i< ceil(run_length * trajectory_sample_rate); i++) + { + Key pose_key = Key('p', i); + trajectory_model.add_control_point(Pose3_(pose_key)); + initial_values.insert(pose_key, Pose3()); + } + + // add control points for clock drift + for(int i=0;i< ceil(run_length * clock_drift_sample_rate); i++) + { + Key accel_clock_drift_key = Key('a', i); + Key gyro_clock_drift_key = Key('g', i); + initial_values.insert(accel_drift_key, 0.0); + initial_values.insert(gyro_clock_drift_key, 0.0); + accel_clock_model.add_control_point(Pose3_(accel_clock_drift_key)); + gyro_clock_model.add_control_point(Pose3_(gyro_clock_drift_key)); + } + + + + + // sample each sensor + shutter_event_t shutter_event = sample_camera_add_slam_factors(); + acceleration_t acceleration = sample_accelerometer(); + angular_rate_t angular_rate = sample_gyro(); + + // use the clock models to estimate the sensor clock's offset from the datum for a given timestamp + // being sure to scale the timestamp from seconds to units of control-points before sampling the trajectory + Double_ gyro_clock_offset = gyro_clock_model.sampleTrajectory(Double_(angular_rate.timestamp * clock_drift_sample_rate)); + Double_ accel_clock_offset = accel_clock_model.sampleTrajectory(Double_(acceleration.timestamp * clock_drift_sample_rate)); + + // scale the timestamps from seconds into units of control_points and apply clock offsets + Double_ camera_time = Double_(shutter_event.timestamp * trajectory_sample_rate); + Double_ gyro_time = (Double_(angular_rate.timestamp) - gyro_clock_offset) * trajectory_sample_rate; + Double_ accel_time = (Double_(acceleration.timestamp) - accel_clock_offset) * trajectory_sample_rate; + + + + // start evaluating the pose trajectory model + + // we can prune control points out of the Expression by setting the window around the timestamp + double window_start = 0; // timestamp - temporal_uncertainty + double window_end = -1; // timestamp + temporal_uncertainty + + + // construct an expression that estimates the pose at the shutter time + Pose3_ camera_pose = trajectory_model.sampleTrajectory(camera_time, window_start, window_end); + // construct an expression that estimates the velocity at the gyro sample time + Pose3Vel_ camera_velocity = trajectory_model.sampleTrajectoryDerivative(gyro_time, window_start, window_end, TimeDerivatives::velocity); + // construct an expression that estimates the acceleration at the accelerometer sample time + Pose3Accel_ camera_acceleration = trajectory_model.sampleTrajectoryDerivative(accel_time, window_start, window_end, TimeDerivatives::acceleration); + + + // transform the trajectory velocity and acceleration predictions into the sensor's local coords + // XXX I'm not 100% sure that this is the right way of transforming the TangentVector, + // this might need accel_pose.inverse().adjoint() + Pose3Vel_ gyro_vel = gyro_pose.adjoint(camera_velocity); + Pose3Accel_ accel_accel = accel_pose.adjoint(camera_acceleration); + + + // construct the pose factors connecting the trajectory_model to the slam bundle-adjustment graph + auto camera_factor = ExpressionFactor( + /*noise model*/ + Pose3(), // identity transform + between(camera_pose, Pose3_(shutter_event.camera_pose)), // discrepancy between bundle adjustment and trajectory model + ); + + // construct the velocity factor + auto gyro_factor = ExpressionFactor( + /*raw sensor noise model*/ + angular_rate.angular_velocity, + Pose3DerivativeRotation(gyro_vel) + gyro_bias + ); + + // construct the acceleration factor + auto accel_factor = ExpressionFactor( + /*raw sensor noise model*/ + acceleration.acceleration, + Pose3DerivativeTranslation(accel_accel) + gravity + accel_bias + ); + + + + + // We can also constrain the smoothness of the trajectory by adding factors to high derivatives + // Factors are only needed at the control point rate. + // This is equivalent to certain forms of Penalty Spline methods + for(int i=0; i( + /* any robust noise model */ + Vector6()::Zero(), + trajectory_model.sampleTrajectoryDerivative(i, window_start, window_end, TimeDerivatives::crackle); + ); + } + + +} + + From 7c47248c09b3551df31a8cade1a257ef3ba527b9 Mon Sep 17 00:00:00 2001 From: Brett Downing Date: Thu, 14 Aug 2025 16:43:18 +1000 Subject: [PATCH 15/16] automates non-trivial control-point density compensation --- .../ClockRecoveryContinuousTrajectory.cpp | 47 +++++----- gtsam/basis/polynomial/TrajectoryModel.h | 22 +++-- gtsam/basis/tests/testTrajectoryModel.cpp | 90 +++++++++++++++++-- 3 files changed, 121 insertions(+), 38 deletions(-) diff --git a/examples/ClockRecoveryContinuousTrajectory.cpp b/examples/ClockRecoveryContinuousTrajectory.cpp index 77d19ed669..fb878daf0a 100644 --- a/examples/ClockRecoveryContinuousTrajectory.cpp +++ b/examples/ClockRecoveryContinuousTrajectory.cpp @@ -65,12 +65,12 @@ inline Point3Deriv_ pose3DerivativeTranslation(const Pose3Deriv_& p) { } // we also need to compose Pose3 TangentVectors with Pose3s -Pose3Deriv_ adjoint(const Pos3_& p, const Pose3Deriv_& v) +Pose3Deriv_ adjoint(const Pose3_& p, const Pose3Deriv_& v) { return Point3_(p, &Pose3::Adjoint, v); } -// enum to make the sampleTrajectoryDerivative arguemnts obvious +// enum to make the sampleTrajectoryDerivative arguments obvious struct TimeDerivatives{ enum TimeDerivatives{ velocity = 1; @@ -84,21 +84,26 @@ struct TimeDerivatives{ + + void simulate_run() { gtsam::Values initial_values; + // First, consider the bandwidth you need to represent the dynamics you want to model. + // any valid sensor data above this frequency contributes to graph noise. + double trajectory_sample_rate = 20; // control points per unit time + double clock_drift_sample_rate = 0.1; // control points per unit time + + + // model the trajectory with cubic splines (makes accelerations piecewise linear) - TrajectoryModel trajectory_model(kernels::IrwinHallCDF2); + TrajectoryModel trajectory_model(trajectory_sample_rate, kernels::IrwinHallCDF2); // model the clock drift with a quadratic spline, (makes drift rate piecewise linear) - TrajectoryModel accel_clock_model(kernels::IrwinHallCDF1); - TrajectoryModel gyro_clock_model(kernels::IrwinHallCDF1); + TrajectoryModel accel_clock_model(clock_drift_sample_rate, kernels::IrwinHallCDF1); + TrajectoryModel gyro_clock_model(clock_drift_sample_rate, kernels::IrwinHallCDF1); - // consider the bandwidth you need to represent the dynamics you want to model - // any sensor bandwidth above this frequency contributes to graph noise. - double trajectory_sample_rate = 20; // control points per second - double clock_drift_sample_rate = 0.1; // control points per second // Account for typical raw-sensor weirdness: @@ -126,7 +131,7 @@ void simulate_run() // just make sure that the trajectory is longer than your new timestamp + the kernel length before generating expressions. double run_length = 10.0; //seconds // add some control points for poses - for(int i=0;i< ceil(run_length * trajectory_sample_rate); i++) + for(int i=0;i< ceil(run_length * trajectory_sample_rate) + 5; i++) { Key pose_key = Key('p', i); trajectory_model.add_control_point(Pose3_(pose_key)); @@ -134,7 +139,7 @@ void simulate_run() } // add control points for clock drift - for(int i=0;i< ceil(run_length * clock_drift_sample_rate); i++) + for(int i=0;i< ceil(run_length * clock_drift_sample_rate) + 3; i++) { Key accel_clock_drift_key = Key('a', i); Key gyro_clock_drift_key = Key('g', i); @@ -153,14 +158,13 @@ void simulate_run() angular_rate_t angular_rate = sample_gyro(); // use the clock models to estimate the sensor clock's offset from the datum for a given timestamp - // being sure to scale the timestamp from seconds to units of control-points before sampling the trajectory - Double_ gyro_clock_offset = gyro_clock_model.sampleTrajectory(Double_(angular_rate.timestamp * clock_drift_sample_rate)); - Double_ accel_clock_offset = accel_clock_model.sampleTrajectory(Double_(acceleration.timestamp * clock_drift_sample_rate)); + Double_ gyro_clock_offset = gyro_clock_model.sampleTrajectory(Double_(angular_rate.timestamp)); + Double_ accel_clock_offset = accel_clock_model.sampleTrajectory(Double_(acceleration.timestamp)); // scale the timestamps from seconds into units of control_points and apply clock offsets - Double_ camera_time = Double_(shutter_event.timestamp * trajectory_sample_rate); - Double_ gyro_time = (Double_(angular_rate.timestamp) - gyro_clock_offset) * trajectory_sample_rate; - Double_ accel_time = (Double_(acceleration.timestamp) - accel_clock_offset) * trajectory_sample_rate; + Double_ camera_time = Double_(shutter_event.timestamp); + Double_ gyro_time = Double_(angular_rate.timestamp) - gyro_clock_offset; + Double_ accel_time = Double_(acceleration.timestamp) - accel_clock_offset; @@ -170,7 +174,6 @@ void simulate_run() double window_start = 0; // timestamp - temporal_uncertainty double window_end = -1; // timestamp + temporal_uncertainty - // construct an expression that estimates the pose at the shutter time Pose3_ camera_pose = trajectory_model.sampleTrajectory(camera_time, window_start, window_end); // construct an expression that estimates the velocity at the gyro sample time @@ -179,11 +182,11 @@ void simulate_run() Pose3Accel_ camera_acceleration = trajectory_model.sampleTrajectoryDerivative(accel_time, window_start, window_end, TimeDerivatives::acceleration); - // transform the trajectory velocity and acceleration predictions into the sensor's local coords + // transform the trajectory velocity and acceleration predictions into the IMU sensor's local coords // XXX I'm not 100% sure that this is the right way of transforming the TangentVector, - // this might need accel_pose.inverse().adjoint() - Pose3Vel_ gyro_vel = gyro_pose.adjoint(camera_velocity); - Pose3Accel_ accel_accel = accel_pose.adjoint(camera_acceleration); + // this might need to invert gyro_pose + Pose3Vel_ gyro_vel = adjoint(gyro_pose, camera_velocity); + Pose3Accel_ accel_accel = adjoint(accel_pose, camera_acceleration); // construct the pose factors connecting the trajectory_model to the slam bundle-adjustment graph diff --git a/gtsam/basis/polynomial/TrajectoryModel.h b/gtsam/basis/polynomial/TrajectoryModel.h index 4aa1fe1265..35be862c12 100644 --- a/gtsam/basis/polynomial/TrajectoryModel.h +++ b/gtsam/basis/polynomial/TrajectoryModel.h @@ -59,6 +59,7 @@ class TrajectoryModel /** * Constructor + * @param density the number of control points per unit (1.0) step in sample location * @param kernel the interpolating kernel to use, See IrwinHallCDF.cpp for splines * default is IrwinHallCDF2 for cubic splines * @param points Expressions representing control points for the curve, defaults to empty @@ -67,10 +68,12 @@ class TrajectoryModel * true places padding at the front, pre-empting response to the control points */ TrajectoryModel( + double density = 1.0, const KernelBase& kernel = kernels::IrwinHallCDF2, const std::vector>& points = {}, bool padFront = false ): + density_(density), kernel_(kernel), points_(points), padFront_(padFront), @@ -85,9 +88,12 @@ class TrajectoryModel 0 ) {} -private: +public: + const double density_; const KernelBase& kernel_; +private: std::vector> points_; +public: const bool padFront_; const double kernelOffset_; const int windowPre_; @@ -260,14 +266,14 @@ Expression TrajectoryModel::sampleTrajectory( double windowEnd) const { // apply padding to the window - int start = floor(windowStart) - windowPre_; - int end = ceil(windowEnd) + windowPost_; + int start = floor(windowStart * density_) - windowPre_; + int end = ceil(windowEnd * density_) + windowPost_; // sanitise inputs if(start < 0) start = 0; if(windowEnd < 0 || end > points_.size()) end = points_.size(); // apply filter delay correction - Double_ kernelTime = timestamp + Double_(kernelOffset_); + Double_ kernelTime = (density_ * timestamp) + Double_(kernelOffset_); // pass to internal methods return kernelInterpolate(kernel_, kernelTime, points_, start, end); @@ -287,17 +293,17 @@ Expression::TangentVector> TrajectoryModel::sampleTrajecto size_t derivative) const { // apply padding to the window - int start = floor(windowStart) - windowPre_; - int end = ceil(windowEnd) + windowPost_; + int start = floor(windowStart * density_) - windowPre_; + int end = ceil(windowEnd * density_) + windowPost_; // sanitise inputs if(start < 0) start = 0; if(windowEnd < 0 || end > points_.size()) end = points_.size(); // apply filter delay correction - Double_ kernelTime = timestamp + Double_(kernelOffset_); + Double_ kernelTime = (density_ * timestamp) + Double_(kernelOffset_); // pass to internal methods - return kernelInterpolateDerivative(kernel_, kernelTime, points_, start, end, derivative); + return pow(density_, derivative) * kernelInterpolateDerivative(kernel_, kernelTime, points_, start, end, derivative); } diff --git a/gtsam/basis/tests/testTrajectoryModel.cpp b/gtsam/basis/tests/testTrajectoryModel.cpp index f2e489b455..1997d8b091 100644 --- a/gtsam/basis/tests/testTrajectoryModel.cpp +++ b/gtsam/basis/tests/testTrajectoryModel.cpp @@ -26,6 +26,9 @@ double tolerance = epsilon/4.0; TEST( TrajectoryModel , BasicBounds ) { gtsam::Values v; // needed for evaluating Expressions + + //set control point density + double density = 1.0; // choose a cubic spline const KernelBase& basis_function = kernels::IrwinHallCDF2; @@ -43,7 +46,7 @@ TEST( TrajectoryModel , BasicBounds ) { // with padding at the back, // non-constant timestamps will be in // [0 : path.size()-1 + basis_function.getLength()] - TrajectoryModel model(basis_function, path/*, pad_front*/); + TrajectoryModel model(density, basis_function, path/*, pad_front*/); Key t_key = Key(0); // choose a key to carry the timestamp v.insert(t_key, 0.0); // assign a value to the timestamp @@ -71,6 +74,9 @@ TEST( TrajectoryModel , BasicBounds ) { TEST( TrajectoryModel , FrontPadding ) { gtsam::Values v; // needed for evaluating Expressions + //set control point density + double density = 1.0; + // choose a cubic spline const KernelBase& basis_function = kernels::IrwinHallCDF2; @@ -85,7 +91,7 @@ TEST( TrajectoryModel , FrontPadding ) { // with padding at the front, // non-constant timestamps will be in // [-basis_function.getLength() : path.size()-1] - TrajectoryModel model(basis_function, path, pad_front); + TrajectoryModel model(density, basis_function, path, pad_front); Key t_key = Key(0); // choose a key to carry the timestamp v.insert(t_key, 0.0); // assign a value to the timestamp @@ -121,6 +127,9 @@ TEST( TrajectoryModel , WindowTruncation ) { gtsam::Values v; // needed for evaluating Expressions + //set control point density + double density = 1.0; + // choose a cubic spline const KernelBase& basis_function = kernels::IrwinHallCDF2; @@ -148,7 +157,7 @@ TEST( TrajectoryModel , WindowTruncation ) { }); //rear padding by default - TrajectoryModel model(basis_function, path); + TrajectoryModel model(density, basis_function, path); Key t_key = Key(0); // choose a key to carry the timestamp v.insert(t_key, 0.0); // assign a value to the timestamp @@ -180,6 +189,9 @@ TEST( TrajectoryModel , TypesPose3 ) { // tests TrajectoryModel can be constructed for Pose3 gtsam::Values v; // needed for evaluating Expressions + //set control point density + double density = 1.0; + // choose a cubic spline const KernelBase& basis_function = kernels::IrwinHallCDF2; @@ -192,7 +204,7 @@ TEST( TrajectoryModel , TypesPose3 ) { }); //rear padding by default - TrajectoryModel model(basis_function, path); + TrajectoryModel model(density, basis_function, path); Key t_key = Key(0); // choose a key to carry the timestamp v.insert(t_key, 0.0); // assign a value to the timestamp @@ -211,6 +223,9 @@ TEST( TrajectoryModel , TypesPose3 ) { TEST( TrajectoryModel , DerivativePose3 ) { gtsam::Values v; // needed for evaluating Expressions + //set control point density + double density = 1.0; + // choose a cubic spline const KernelBase& basis_function = kernels::IrwinHallCDF2; @@ -222,7 +237,7 @@ TEST( TrajectoryModel , DerivativePose3 ) { Pose3_(Pose3(Rot3::Rodrigues(0.2,2.1,0.4), Point3(0,2,0))), }); - TrajectoryModel model(basis_function, path); + TrajectoryModel model(density, basis_function, path); Key t_ref_key = Key(0); // key to carry the timestamp Key t_eps_key = Key(1); // key to carry the timestamp + epsilon @@ -260,6 +275,9 @@ TEST( TrajectoryModel , DerivativePose3 ) { TEST( TrajectoryModel , NthDerivativePose3 ) { gtsam::Values v; // needed for evaluating Expressions + //set control point density + double density = 1.0; + // choose a cubic spline const KernelBase& basis_function = kernels::IrwinHallCDF2; @@ -271,7 +289,7 @@ TEST( TrajectoryModel , NthDerivativePose3 ) { Pose3_(Pose3(Rot3::Rodrigues(0.2,2.1,0.4), Point3(0,2,0))), }); - TrajectoryModel model(basis_function, path); + TrajectoryModel model(density, basis_function, path); Key t_ref_key = Key(0); // key to carry the timestamp Key t_eps_key = Key(1); // key to carry the timestamp + epsilon @@ -302,11 +320,64 @@ TEST( TrajectoryModel , NthDerivativePose3 ) { )); } } +} + + + +TEST( TrajectoryModel , NthDerivativeDensityPose3 ) { + gtsam::Values v; // needed for evaluating Expressions + + //set control point density + double density = 10.0; + + // choose a cubic spline + const KernelBase& basis_function = kernels::IrwinHallCDF2; + + // specify some control points + std::vector path({ + Pose3_(Pose3(Rot3::Rodrigues(0.3,2.2,0.1), Point3(0,0,0))), // XXX use non-trival rotations + Pose3_(Pose3(Rot3::Rodrigues(0.2,2.5,0.1), Point3(0,0,0))), + Pose3_(Pose3(Rot3::Rodrigues(0.0,2.2,0.1), Point3(0,2,0))), + Pose3_(Pose3(Rot3::Rodrigues(0.2,2.1,0.4), Point3(0,2,0))), + }); + + TrajectoryModel model(density, basis_function, path); + + Key t_ref_key = Key(0); // key to carry the timestamp + Key t_eps_key = Key(1); // key to carry the timestamp + epsilon + + // assign types to Keys + v.insert(t_ref_key, 0.0); + v.insert(t_eps_key, 0.0); + + // cast the Keys to Expressions + Double_ t_ref = Double_(t_ref_key); + Double_ t_eps = Double_(t_eps_key); + for(size_t n=0; n(t_ref_key, sample_time); + v.update(t_eps_key, sample_time + epsilon); + CHECK(assert_equal( + sample_eps.value(v), + expmap(sample_ref, epsilon * sample_d).value(v), + tolerance + )); + } + } } + + TEST( TrajectoryModel , MeshModel ) { // TrajectoryModel accepts Expressions as control points, // allowing arbitrarily complex control point definitions, @@ -314,6 +385,9 @@ TEST( TrajectoryModel , MeshModel ) { gtsam::Values v; // needed for evaluating Expressions + //set control point density + double density = 1.0; + // choose a cubic spline const KernelBase& basis_function = kernels::IrwinHallCDF2; @@ -357,12 +431,12 @@ TEST( TrajectoryModel , MeshModel ) { std::vector> rows; for(const auto& path : mesh){ - TrajectoryModel row(basis_function, path); + TrajectoryModel row(density, basis_function, path); rows.push_back(row); auto row_sample = row.sampleTrajectory(x_expr); col_path.push_back(row_sample); } - TrajectoryModel col(basis_function, col_path); + TrajectoryModel col(density, basis_function, col_path); auto sample = col.sampleTrajectory(y_expr); From 6b060a7caa332c865570cf277a283d2bced4f8e4 Mon Sep 17 00:00:00 2001 From: Brett Downing Date: Thu, 21 Aug 2025 14:10:11 +1000 Subject: [PATCH 16/16] tidy up and correct types in example --- examples/ClockRecoveryContinuousTrajectory.cpp | 12 ++++++------ gtsam/basis/polynomial/TrajectoryModel.h | 6 +++--- gtsam/basis/tests/testTrajectoryModel.cpp | 2 +- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/examples/ClockRecoveryContinuousTrajectory.cpp b/examples/ClockRecoveryContinuousTrajectory.cpp index fb878daf0a..59732fb800 100644 --- a/examples/ClockRecoveryContinuousTrajectory.cpp +++ b/examples/ClockRecoveryContinuousTrajectory.cpp @@ -129,9 +129,9 @@ void simulate_run() // we'll build a bunch of trajectory up front, this can be done incrementally the same way // just make sure that the trajectory is longer than your new timestamp + the kernel length before generating expressions. - double run_length = 10.0; //seconds + double max_timestamp = 10.0; //seconds // add some control points for poses - for(int i=0;i< ceil(run_length * trajectory_sample_rate) + 5; i++) + for(int i=0; i(accel_drift_key, 0.0); initial_values.insert(gyro_clock_drift_key, 0.0); - accel_clock_model.add_control_point(Pose3_(accel_clock_drift_key)); - gyro_clock_model.add_control_point(Pose3_(gyro_clock_drift_key)); + accel_clock_model.add_control_point(Double_(accel_clock_drift_key)); + gyro_clock_model.add_control_point(Double_(gyro_clock_drift_key)); } @@ -161,7 +161,7 @@ void simulate_run() Double_ gyro_clock_offset = gyro_clock_model.sampleTrajectory(Double_(angular_rate.timestamp)); Double_ accel_clock_offset = accel_clock_model.sampleTrajectory(Double_(acceleration.timestamp)); - // scale the timestamps from seconds into units of control_points and apply clock offsets + // apply clock offsets Double_ camera_time = Double_(shutter_event.timestamp); Double_ gyro_time = Double_(angular_rate.timestamp) - gyro_clock_offset; Double_ accel_time = Double_(acceleration.timestamp) - accel_clock_offset; diff --git a/gtsam/basis/polynomial/TrajectoryModel.h b/gtsam/basis/polynomial/TrajectoryModel.h index 35be862c12..df89ea8bdf 100644 --- a/gtsam/basis/polynomial/TrajectoryModel.h +++ b/gtsam/basis/polynomial/TrajectoryModel.h @@ -38,9 +38,9 @@ using an Irwin Hall probability density function as the impulse, replicates cardinal splines. -There is possibly a more numerically way of doing this where reference point - is chosen as close to the control point as possible, and the logspace - accumulation is performed both ways out from the reference point. +There is possibly a more numerically accurate way of doing this where the + reference point is chosen as close to the control point as possible, and the + logspace accumulation is performed both ways out from the reference point. The CDF would need to be evaluated and split at the reference point */ diff --git a/gtsam/basis/tests/testTrajectoryModel.cpp b/gtsam/basis/tests/testTrajectoryModel.cpp index 1997d8b091..c973577b5e 100644 --- a/gtsam/basis/tests/testTrajectoryModel.cpp +++ b/gtsam/basis/tests/testTrajectoryModel.cpp @@ -361,7 +361,7 @@ TEST( TrajectoryModel , NthDerivativeDensityPose3 ) { auto sample_eps = model.sampleTrajectoryDerivative(t_eps, 0, -1, n); // construct an Expression that captures the (N+1)th derivative of the trajectory auto sample_d = model.sampleTrajectoryDerivative(t_ref,0,-1, n+1); - for(double sample_time = 0; sample_time<7; sample_time+=0.1) + for(double sample_time = 0; sample_time<(7.0/density); sample_time+=0.1) { v.update(t_ref_key, sample_time); v.update(t_eps_key, sample_time + epsilon);