diff --git a/examples/ClockRecoveryContinuousTrajectory.cpp b/examples/ClockRecoveryContinuousTrajectory.cpp new file mode 100644 index 0000000000..59732fb800 --- /dev/null +++ b/examples/ClockRecoveryContinuousTrajectory.cpp @@ -0,0 +1,230 @@ + +#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 Pose3_& p, const Pose3Deriv_& v) +{ + return Point3_(p, &Pose3::Adjoint, v); +} + +// enum to make the sampleTrajectoryDerivative arguments obvious +struct TimeDerivatives{ + enum TimeDerivatives{ + velocity = 1; + acceleration = 2; + jerk = 3; + snap = 4; + crackle = 5; + pop = 6; + } +}; + + + + + +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(trajectory_sample_rate, kernels::IrwinHallCDF2); + // model the clock drift with a quadratic spline, (makes drift rate piecewise linear) + TrajectoryModel accel_clock_model(clock_drift_sample_rate, kernels::IrwinHallCDF1); + TrajectoryModel gyro_clock_model(clock_drift_sample_rate, kernels::IrwinHallCDF1); + + + + // 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 max_timestamp = 10.0; //seconds + // add some control points for poses + for(int i=0; i(pose_key, Pose3()); + } + + // add control points for clock drift + 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(Double_(accel_clock_drift_key)); + gyro_clock_model.add_control_point(Double_(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 + Double_ gyro_clock_offset = gyro_clock_model.sampleTrajectory(Double_(angular_rate.timestamp)); + Double_ accel_clock_offset = accel_clock_model.sampleTrajectory(Double_(acceleration.timestamp)); + + // 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; + + + + // 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 IMU sensor's local coords + // XXX I'm not 100% sure that this is the right way of transforming the TangentVector, + // 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 + 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); + ); + } + + +} + + diff --git a/gtsam/basis/polynomial/IrwinHall.cpp b/gtsam/basis/polynomial/IrwinHall.cpp new file mode 100644 index 0000000000..cc68fa6b67 --- /dev/null +++ b/gtsam/basis/polynomial/IrwinHall.cpp @@ -0,0 +1,69 @@ + +#include "IrwinHall.h" +namespace gtsam { +namespace kernels { + +/** + * coefficients for the Irwin Hall Probability Density Functions + * https://oeis.org/A188816 + */ + +const PiecewisePolynomial<0,1> IrwinHall0({ + {1.}, + {0.,1.}, + 1./2. +}); +const PiecewisePolynomial<1,2> IrwinHall1({ + { 0., 1., + 2., -1.}, + {0.,1.,2.}, + 1. +}); +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 PiecewisePolynomial<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 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. , + -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 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. , + 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 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. , + -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..752ac22f75 --- /dev/null +++ b/gtsam/basis/polynomial/IrwinHall.h @@ -0,0 +1,39 @@ +#pragma once + +#include "PiecewisePolynomial.h" + +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 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 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 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 new file mode 100644 index 0000000000..1462f6fad1 --- /dev/null +++ b/gtsam/basis/polynomial/IrwinHallCDF.cpp @@ -0,0 +1,70 @@ + +#include "IrwinHall.h" +namespace gtsam { +namespace kernels { + +/* + * Coefficients for the Irwin Hall Cumulative Distribution Function + * https://oeis.org/A188668 + */ + +const PiecewisePolynomial<1,1> IrwinHallCDF0({ + {0., 1.}, + {0., 1.}, + 1./2. +}); +const PiecewisePolynomial<2,2> IrwinHallCDF1({ + { 0. , 0. , 1./2., + -2./2., 4./2., -1./2.}, + {0.,1.,2.}, + 1. +}); +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.}, + {0.,1.,2.,3.}, + 3./2. +}); + +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., + -232./24., 256./24., -96./24., 16./24., -1./24.}, + {0.,1.,2.,3.,4.}, + 2. +}); +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., + 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 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, + -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 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., + 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. +}); + +} // 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..df3af2e464 --- /dev/null +++ b/gtsam/basis/polynomial/PiecewisePolynomial.h @@ -0,0 +1,121 @@ +#pragma once + +#include + +namespace gtsam { + + +/** + * A Piecewise-defined Polynomial in 1d + * mostly used for low-order approximations to probability distributions + * automatically computes derivatives + * + */ +template +class PiecewisePolynomial : public KernelBase +{ +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: + PiecewisePolynomial(param_s init_list):params(init_list){} + /** returns the centre of the distribution */ + double getCenter() const override + { + return params.center; + } + /** returns the lowest value of t where this polynomial has non-constant return */ + double getBeginning() const override + { + return params.intervals[0]; + } + /** returns the highest value of t where this polynomial has non-constant return */ + double getEnd() const override + { + return params.intervals[pieces]; + } + /** returns the number of times this function can be differentiated and still return something useful */ + size_t getValidDerivatives() const override + { + return order; + } + + /** returns the number of pieces this polynomial is defined by, mostly use for testing. */ + std::array getIntervals() const { + return params.intervals; + } + + /** + * 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 evaluateDerivative(0, t, H); + } + + /** + * 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]) + { + 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 // 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_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 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 PDF. + +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 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. + + +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 + +*/ + +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 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 + * @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( + double density = 1.0, + const KernelBase& kernel = kernels::IrwinHallCDF2, + const std::vector>& points = {}, + bool padFront = false + ): + density_(density), + kernel_(kernel), + points_(points), + padFront_(padFront), + kernelOffset_(padFront ? + kernel.getEnd() : + kernel.getBeginning()), + windowPre_(padFront ? + 0 : + ceil(kernel.getLength())), + windowPost_(padFront ? + ceil(kernel.getLength()) : + 0 ) + {} + +public: + const double density_; + const KernelBase& kernel_; +private: + std::vector> points_; +public: + 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. + * @param point the control point to insert + */ + void addControlPoint(Expression point) + { + points_.push_back(point); + } + + /** + get a read-only reference to the current 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 + * 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 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. + * + * @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 windowStart = 0, + double windowEnd = -1, + size_t derivative = 1) const; + + + +private: + + /** + * 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 + ); + + + /** + * 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 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); + + + /** + * 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' + */ + static Expression cumulativePathSum( + const std::vector>& points, + const std::vector& cdfWeights); + + /** + * 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. + */ + 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::sampleTrajectory( + Double_ timestamp, + double windowStart, + double windowEnd) const +{ + // apply padding to the window + 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 = (density_ * timestamp) + Double_(kernelOffset_); + + // 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::sampleTrajectoryDerivative( + Double_ timestamp, + double windowStart, + double windowEnd, + size_t derivative) const +{ + // apply padding to the window + 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 = (density_ * timestamp) + Double_(kernelOffset_); + + // pass to internal methods + return pow(density_, derivative) * kernelInterpolateDerivative(kernel_, kernelTime, points_, start, end, derivative); +} + + +/** + * compute a weighted sum of logmap differences between adjacent points applied to the first point + */ +template +Expression TrajectoryModel::cumulativePathSum( + const std::vector>& points, + const std::vector& cdfWeights) +{ + return expmap(points[0], cumulativePathSumDerivative(points, cdfWeights)); +} + + +template +Expression::TangentVector> TrajectoryModel::cumulativePathSumDerivative( + 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 weight for the relevant point + // accumulate the logspace vector + // 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]); + } + return v; +} + + + +/** +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::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> pointsRange(points.begin()+start, points.begin()+end); + // generate the basis functions as expressions depending on the timestamp + std::vector weights_range = sampleKernel(kernel, timestamp, start, end, 0); + // apply the weighted sum + return cumulativePathSum(pointsRange, weights_range); +} + +/** +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::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> pointsRange(points.begin()+start, points.begin()+end); + // generate the basis functions as expressions depending on the timestamp + std::vector weights_range = sampleKernel(kernel, timestamp, start, end, derivative); + // apply the weighted sum + return cumulativePathSumDerivative(pointsRange, 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::sampleKernel( + const KernelBase& kernel, + const Double_& timestamp, + size_t start, size_t end, + size_t derivative) +{ + 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.evaluateDerivative(derivative, x, j); + }; + + for(size_t i = start; i < end; i++) + { + Double_ kernelTime = timestamp - Double_(double(i)); + kernelSamples.push_back(Double_(keval, kernelTime)); + } + + return kernelSamples; +} + + + + + + +} // namespace gtsam + + diff --git a/gtsam/basis/polynomial/kernels.h b/gtsam/basis/polynomial/kernels.h new file mode 100644 index 0000000000..a3f587e483 --- /dev/null +++ b/gtsam/basis/polynomial/kernels.h @@ -0,0 +1,48 @@ +/** + * Kernels for continuous interpolating Finite Impulse Response filters + * This can be used to define continuous-time trajectory models + */ + +#pragma once + +#include + +namespace gtsam { + + +/** + * A base class for convolutional kernel functions + */ +class KernelBase +{ +public: + /** 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 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 getEnd() - getBeginning(); + }; + + /** default destructor */ + virtual ~KernelBase() = default; + +}; + + + + +} // 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..6292e12a20 --- /dev/null +++ b/gtsam/basis/tests/testIrwinHall.cpp @@ -0,0 +1,182 @@ +/** + * @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; + +// 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; + + + +// test boundary conditions at the limits of the domain +TEST( poly , Boundaries1 ) { + auto poly = IrwinHall1; + 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.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.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.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.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.getIntervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.getIntervals().back()), tolerance); +} + + + +// test continuity where the polynomial pieces join +TEST( IrwinHall , Continuity1 ) { + auto poly = IrwinHall1; + 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.getIntervals()) { + EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), + poly.evaluate(t-epsilon), tolerance ); + } +} +TEST( IrwinHall , Continuity3 ) { + auto poly = IrwinHall3; + 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.getIntervals()) { + EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), + poly.evaluate(t-epsilon), tolerance ); + } +} +TEST( IrwinHall , Continuity5 ) { + auto poly = IrwinHall5; + 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.getIntervals()) { + 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.getIntervals()) { + for(int d=0; d +#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.getIntervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 1.0, poly.evaluate(poly.getIntervals().back()), tolerance); +} +TEST( IrwinHallCDF , Boundaries2 ) { + auto poly = IrwinHallCDF2; + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.getIntervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 1.0, poly.evaluate(poly.getIntervals().back()), tolerance); +} +TEST( IrwinHallCDF , Boundaries3 ) { + auto poly = IrwinHallCDF3; + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.getIntervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 1.0, poly.evaluate(poly.getIntervals().back()), tolerance); +} +TEST( IrwinHallCDF , Boundaries4 ) { + auto poly = IrwinHallCDF4; + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.getIntervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 1.0, poly.evaluate(poly.getIntervals().back()), tolerance); +} +TEST( IrwinHallCDF , Boundaries5 ) { + auto poly = IrwinHallCDF5; + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.getIntervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 1.0, poly.evaluate(poly.getIntervals().back()), tolerance); +} +TEST( IrwinHallCDF , Boundaries6 ) { + auto poly = IrwinHallCDF6; + EXPECT_DOUBLES_EQUAL( 0.0, poly.evaluate(poly.getIntervals().front()), tolerance); + EXPECT_DOUBLES_EQUAL( 1.0, poly.evaluate(poly.getIntervals().back()), tolerance); +} + + + + +// test continuity where the polynomial pieces join +TEST( IrwinHallCDF , Continuity1 ) { + auto poly = IrwinHallCDF1; + for(const double &t : poly.getIntervals()) { + EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), + poly.evaluate(t-epsilon), tolerance ); + } +} +TEST( IrwinHallCDF , Continuity2 ) { + auto poly = IrwinHallCDF2; + for(const double &t : poly.getIntervals()) { + EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), + poly.evaluate(t-epsilon), tolerance ); + } +} +TEST( IrwinHallCDF , Continuity3 ) { + auto poly = IrwinHallCDF3; + for(const double &t : poly.getIntervals()) { + EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), + poly.evaluate(t-epsilon), tolerance ); + } +} +TEST( IrwinHallCDF , Continuity4 ) { + auto poly = IrwinHallCDF4; + for(const double &t : poly.getIntervals()) { + EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), + poly.evaluate(t-epsilon), tolerance ); + } +} +TEST( IrwinHallCDF , Continuity5 ) { + auto poly = IrwinHallCDF5; + for(const double &t : poly.getIntervals()) { + EXPECT_DOUBLES_EQUAL( poly.evaluate(t+epsilon), + poly.evaluate(t-epsilon), tolerance ); + } +} +TEST( IrwinHallCDF , Continuity6 ) { + auto poly = IrwinHallCDF6; + for(const double &t : poly.getIntervals()) { + 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.getIntervals()) { + 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.getIntervals()) { + for(int d=0; d jacobian; + poly.evaluateDerivative(d, t, &jacobian); + double derivative = poly.evaluateDerivative(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.getIntervals()) { + for(int d=0; d jacobian; + poly.evaluateDerivative(d, t, &jacobian); + double derivative = poly.evaluateDerivative(d+1, t); + EXPECT_DOUBLES_EQUAL(jacobian[0], derivative, tolerance ); + } + } +} + + + +/* ************************************************************************* */ +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..c973577b5e --- /dev/null +++ b/gtsam/basis/tests/testTrajectoryModel.cpp @@ -0,0 +1,462 @@ +/** + * @file TestTrajectoryModel.cpp + * @brief unit tests for cardinal spline interpolation + * @author Brett Downing + * @date August 2025 + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace gtsam; + + + +// 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 ) { + 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; + + // 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.getLength()] + 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 + Double_ timestamp = Double_(t_key); // associate the timestamp with the key + + // construct an Expression that samples the trajectory + 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.getLength()); + CHECK(assert_equal(path.back().value(v), sample.value(v), tolerance)); + + // mid value + 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)); + +} + + + +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; + + // 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.getLength() : path.size()-1] + 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 + Double_ timestamp = Double_(t_key); // associate the timestamp with the key + + // construct an Expression that samples the trajectory + Double_ sample = model.sampleTrajectory(timestamp); + + + // bounds check + 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.getLength() + (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 + // 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. + // padding is automatically applied to the window arguments + + 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; + + // 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(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 + Double_ timestamp = Double_(t_key); // associate the timestamp with the key + + // construct an Expression that samples from the entire trajectory + 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.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(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)); + + } +} + + + +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; + + // 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,2,0))), + Pose3_(Pose3(Rot3(), Point3(0,2,0))), + }); + + //rear padding by default + 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 + Double_ timestamp = Double_(t_key); // associate the timestamp with the key + + // construct an Expression that samples from the entire trajectory + Pose3_ sample = model.sampleTrajectory(timestamp); + + v.update(t_key, 3.5); + CHECK(assert_equal(Pose3(Rot3(), Point3(0,1,0)), sample.value(v), tolerance)); + +} + + + +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; + + // specify some control points + std::vector path({ + 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(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); + + // construct Expressions that sample the trajectory + 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.sampleTrajectoryDerivative(t_ref,0,-1,1); + + for(double sample_time = 0; sample_time<7; sample_time+=0.1) + { + 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 + )); + } + +} + + + +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; + + // 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 , 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, + // such as entire other TrajectoryModel samples + + 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; + + // 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){ + 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(density, basis_function, col_path); + auto sample = col.sampleTrajectory(y_expr); + + + for(double x=0; x<7; x+=0.1) + { + for(double y=0; y<7; y+=0.1) + { + v.update(x_key, x); + v.update(y_key, y); + CHECK(sample.value(v)<10); + CHECK(sample.value(v)>-10); + } + } +} + + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + 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/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/testAsVectorSpace.cpp b/gtsam/geometry/tests/testAsVectorSpace.cpp new file mode 100644 index 0000000000..759b849bfc --- /dev/null +++ b/gtsam/geometry/tests/testAsVectorSpace.cpp @@ -0,0 +1,157 @@ +/** + * @file testCartesinProduct.cpp + * @brief Test vector space properties of extrapolated manifolds + * @author Brett Downing + * @date April 2025 + */ + + + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +using namespace gtsam; + + +GTSAM_CONCEPT_TESTABLE_INST(AsVectorSpace) +GTSAM_CONCEPT_TESTABLE_INST(AsVectorSpace) +GTSAM_CONCEPT_TESTABLE_INST(AsVectorSpace) +GTSAM_CONCEPT_TESTABLE_INST(AsVectorSpace) + +//****************************************************************************** +TEST( AsVectorSpace , Cal3fConstructor) { + AsVectorSpace p; +} +//****************************************************************************** +TEST( AsVectorSpace , Cal3DS2Constructor) { + AsVectorSpace p; +} +//****************************************************************************** +TEST( AsVectorSpace , Cal3_S2Constructor) { + AsVectorSpace p; +} +//****************************************************************************** +TEST( AsVectorSpace , Cal3FisheyeConstructor) { + AsVectorSpace p; +} + +//****************************************************************************** +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); +} + +//****************************************************************************** +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(); + const Cal3Fisheye& q = p; +} + + +//****************************************************************************** +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>); + GTSAM_CONCEPT_ASSERT(IsGroup>); + GTSAM_CONCEPT_ASSERT(IsVectorSpace>); +} + + +//****************************************************************************** +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; + 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); +} +/* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testCartesianProduct.cpp b/gtsam/geometry/tests/testCartesianProduct.cpp new file mode 100644 index 0000000000..04a7f88ad2 --- /dev/null +++ b/gtsam/geometry/tests/testCartesianProduct.cpp @@ -0,0 +1,148 @@ +/** + * @file testCartesinProduct.cpp + * @brief Test manifold properties of combined manifolds + * @author Brett Downing + * @date April 2025 + */ + + + +#include +#include +#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; +// bind a multiplicative group to an additive group +typedef CartesianProduct> CameraPose; + + + +GTSAM_CONCEPT_TESTABLE_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 , StampedPoint3Constructor) { + StampedPoint3 p; +} +//****************************************************************************** +TEST( CartesianProduct , CameraPoseConstructor) { + CameraPose p; +} + +//****************************************************************************** +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 , 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, 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)); +} + + + +//****************************************************************************** +TEST( CartesianProduct , StampedPose3Concept) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + //GTSAM_CONCEPT_ASSERT(IsVectorSpace); +} + +//****************************************************************************** +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)); + EXPECT(check_manifold_invariants(p1, p2)); +} +//****************************************************************************** + +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)); + 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 , StampedSplitPose3Concept) { + GTSAM_CONCEPT_ASSERT(IsGroup); + GTSAM_CONCEPT_ASSERT(IsManifold); + //GTSAM_CONCEPT_ASSERT(IsVectorSpace); +} + +//****************************************************************************** +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)); + EXPECT(check_manifold_invariants(p1, p2)); +} +//****************************************************************************** + +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)); +} + + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + 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