Detailed Description

template<typename PoseType>
class gtsam::Interpolator< PoseType >

Interpolator for poses and velocities under a motion prior.

The Interpolator class provides routines to interpolate pose and velocity at arbitrary times between (or outside) estimated states. It encapsulates the motion model (WNOA by default) via function objects for transition, covariance and Jacobian computations. The class also computes conditional covariances for interpolated states when requested.

Template Parameters

PoseType Pose group/type (e.g., Pose2, Pose3, or a vector-space pose)

Public Member Functions
	Interpolator (const VectorN &q_psd_diag, std::function< Matrix(double dt)> transitionFunction, std::function< Matrix(double dt, const VectorN &q_psd_diag)> covarianceFunction, std::function< Matrix(double dt, const VectorN &q_psd_diag)> inverseCovarianceFunction, std::function< Matrix(const std::pair< PoseType, VelocityType > &, const std::pair< PoseType, VelocityType > &, double)> computeJacobianPrev, std::function< Matrix(const std::pair< PoseType, VelocityType > &, const std::pair< PoseType, VelocityType > &, double)> computeJacobianNext)
	Construct an Interpolator with custom motion-model functions.
	Interpolator (const VectorN &q_psd_diag)
	Default constructor using the WNOA motion model.
PoseVel	interpolatePoseAndVelocity (const std::optional< TimestampedPoseVel > &Tvarpi_k, const std::optional< TimestampedPoseVel > &Tvarpi_kp1, double t_tau, OptionalMatrixVecType H=nullptr, const std::shared_ptr< Matrix > &mainSolveMarginalMatrix=nullptr, Matrix *covarianceOut=nullptr, const std::shared_ptr< const LambdaPsiMats > &LambdaPsiPreComp=nullptr, const std::shared_ptr< const LocalStateVecs > &localStateVecsPreComp=nullptr, const std::shared_ptr< const StateJacobians > &stateJacobiansPreComp=nullptr) const
	Interpolate the pose and velocity at time t_tau.
Values	interpolatePosesAndVelocities (const NonlinearFactorGraph &mainSolveGraph, const Values &mainSolveSolution, const StateDataSet &mainSolveStates, const StateDataSet &interpolatedStates, std::shared_ptr< InterpCovarianceMap > covarianceMapOut=nullptr) const
	Interpolate multiple poses and velocities given a solved factor graph.
Matrix2N	computeConditionalCov (const TimestampedPoseVel &pvk, const TimestampedPoseVel &pvkp1, const TimestampedPoseVel &pvtau, OptionalMatrixType Lambda=nullptr, OptionalMatrixType Psi=nullptr) const
	Compute the conditional covariance of the interpolated state.
std::pair< Matrix, Matrix >	getLambdaPsi (double t_k, double t_kp1, double t_tau) const
	Compute interpolation matrices Lambda and Psi (WNOA-optimized).
std::pair< Matrix, Matrix >	getLambdaPsiGeneral (double t_k, double t_kp1, double t_tau) const
	Compute interpolation matrices Lambda and Psi for a general motion model.
LocalStateVecs	computeLocalStateVecs (const TimestampedPoseVel &pvk, const TimestampedPoseVel &pvkp1, StateJacobians *jacs=nullptr) const
	Compute the local state vectors (xi, xi_dot) for a pair of bordering states.

Public Types
using	StateDataSet = std::set<StateData>
using	PoseVel = PoseVelocity<PoseType>
using	TimestampedPoseVel = TimestampedPoseVelocity<PoseType>
using	LambdaPsiMats = std::pair<Matrix2N, Matrix2N>
using	LocalStateVecs = std::pair<VectorN, VectorN>

Classes
struct	StateJacobians

Protected Types
using	VelocityType = typename traits<PoseType>::TangentVector
using	MatrixN = Eigen::Matrix<double, dim, dim>
using	VectorN = Eigen::Matrix<double, dim, 1>
using	Matrix2N = Eigen::Matrix<double, 2 * dim, 2 * dim>
using	Vector2N = Eigen::Matrix<double, 2 * dim, 1>
using	MatrixNx2N = Eigen::Matrix<double, dim, 2 * dim>

Protected Member Functions
PoseVel	interpolateBoundaryLeft (const PoseVelocity< PoseType > &poseVel_k, OptionalMatrixVecType H=nullptr, const std::shared_ptr< Matrix > &mainSolveMarginalMatrix=nullptr, Matrix *covarianceOut=nullptr) const
	Interpolate pose and velocity at the left boundary.
PoseVel	interpolateBoundaryRight (const PoseVelocity< PoseType > &poseVel_kp1, OptionalMatrixVecType H=nullptr, const std::shared_ptr< Matrix > &mainSolveMarginalMatrix=nullptr, Matrix *covarianceOut=nullptr) const
	Interpolate pose and velocity at the right boundary.
PoseVel	extrapolatePoseAndVelocity (const PoseVelocity< PoseType > &poseVel, double t_diff, OptionalMatrixVecType H=nullptr, const std::shared_ptr< Matrix > &mainSolveMarginalMatrix=nullptr, Matrix *covarianceOut=nullptr) const
	Extrapolate pose and velocity from a border state.
PoseVel	interpolatePoseAndVelocity_ (const TimestampedPoseVel &tPoseVel_k, const TimestampedPoseVel &tPoseVel_kp1, double t_tau, OptionalMatrixVecType H=nullptr, const std::shared_ptr< Matrix > &mainSolveMarginalMatrix=nullptr, Matrix *covarianceOut=nullptr, const std::shared_ptr< const LambdaPsiMats > &LambdaPsiPreComp=nullptr, const std::shared_ptr< const LocalStateVecs > &localStateVecsPreComp=nullptr, const std::shared_ptr< const StateJacobians > &stateJacobiansPreComp=nullptr) const
	Internal implementation that performs interpolation between two bordering states.
void	formLocalStateAndJacobians_ (const TimestampedPoseVel &tPoseVel_k, const TimestampedPoseVel &tPoseVel_kp1, const std::shared_ptr< const LocalStateVecs > &localStateVecsPreComp, const std::shared_ptr< const StateJacobians > &stateJacobiansPreComp, VectorN xi_dot_k, VectorN xi_kp1, VectorN xi_dot_kp1, StateJacobians jacs=nullptr) const
	Step 1 of interpolatePoseAndVelocity: form local state vectors and optional local Jacobians.
void	interpolateLocalState_ (double t_k, double t_kp1, double t_tau, const VectorN &xi_dot_k, const VectorN &xi_kp1, const VectorN &xi_dot_kp1, const std::shared_ptr< const LambdaPsiMats > &LambdaPsiPreComp, Matrix2N Lambda, Matrix2N Psi, VectorN xi_tau, VectorN xidot_tau) const
	Step 2 of interpolatePoseAndVelocity: interpolate local state using Lambda/Psi matrices.
PoseVel	mapLocalStateToManifold_ (const PoseType &T_k, const VectorN &xi_tau, const VectorN &xidot_tau, MatrixN right_jac_tau, MatrixN dTtau_dTk=nullptr, MatrixN *dTtau_dxitau=nullptr) const
	Step 3 of interpolatePoseAndVelocity: map interpolated local state back to manifold.
void	computeCompleteJacobians_ (const Matrix2N &Lambda, const Matrix2N &Psi, const VectorN &xidot_tau, const MatrixN &right_jac_tau, const MatrixN &dTtau_dTk, const MatrixN &dTtau_dxitau, const StateJacobians &jacs, OptionalMatrixVecType H) const
	Step 4 of interpolatePoseAndVelocity: compose full Jacobians with chain rule.
void	computeInterpolationCovariance_ (const TimestampedPoseVel &tPoseVel_k, const TimestampedPoseVel &tPoseVel_kp1, const PoseVel &poseVel_tau, double t_tau, const Matrix2N &Lambda, const Matrix2N &Psi, const std::shared_ptr< Matrix > &mainSolveMarginalMatrix, Matrix *covarianceOut) const
	Step 5 of interpolatePoseAndVelocity: compute interpolated covariance when requested.

Static Protected Member Functions
static std::map< StateDataInterval, std::shared_ptr< Matrix > >	ComputeJointMarginals (const std::map< StateDataInterval, std::vector< StateData > > &queryBuckets, const std::unique_ptr< Marginals > &marginals)
	Compute joint marginal covariances for requested state intervals.
static Matrix	ConstructMatrixFromJointMarginal (const JointMarginal &blockMatrix, const KeyVector &keyVector, size_t blockSize)
	Construct a full covariance matrix from a joint marginal block matrix.

Protected Attributes
VectorN	q_psd_diag_
std::function< Matrix(double dt)>	transitionFunction_
std::function< Matrix(double dt, const VectorN &q_psd_diag)>	covarianceFunction_
std::function< Matrix(double dt, const VectorN &q_psd_diag)>	inverseCovarianceFunction_
std::function< Matrix(const std::pair< PoseType, VelocityType > &, const std::pair< PoseType, VelocityType > &, double)>	computeJacobianPrev_
std::function< Matrix(const std::pair< PoseType, VelocityType > &, const std::pair< PoseType, VelocityType > &, double)>	computeJacobianNext_

Static Protected Attributes
static constexpr int	dim = traits<PoseType>::dimension

Constructor & Destructor Documentation

◆ Interpolator() [1/2]

template<typename PoseType>

gtsam::Interpolator< PoseType >::Interpolator	(	const VectorN &	q_psd_diag,
		std::function< Matrix(double dt)>	transitionFunction,
		std::function< Matrix(double dt, const VectorN &q_psd_diag)>	covarianceFunction,
		std::function< Matrix(double dt, const VectorN &q_psd_diag)>	inverseCovarianceFunction,
		std::function< Matrix(const std::pair< PoseType, VelocityType > &, const std::pair< PoseType, VelocityType > &, double)>	computeJacobianPrev,
		std::function< Matrix(const std::pair< PoseType, VelocityType > &, const std::pair< PoseType, VelocityType > &, double)>	computeJacobianNext )

Construct an Interpolator with custom motion-model functions.

Parameters

q_psd_diag	Diagonal power spectral density vector for the motion prior.
transitionFunction	Function returning the transition matrix for dt.
covarianceFunction	Function returning process noise covariance for dt and q_psd_diag.
inverseCovarianceFunction	Function returning inverse covariance for dt and q_psd_diag.
computeJacobianPrev	Function that computes the Jacobian of the interpolated state with respect to the previous bordering state.
computeJacobianNext	Function that computes the Jacobian of the interpolated state with respect to the next bordering state.

◆ Interpolator() [2/2]

template<typename PoseType>

gtsam::Interpolator< PoseType >::Interpolator ( const VectorN & q_psd_diag )

Default constructor using the WNOA motion model.

Parameters

q_psd_diag Diagonal power spectral density vector for the motion prior.

Member Function Documentation

◆ computeCompleteJacobians_()

template<typename PoseType>

void gtsam::Interpolator< PoseType >::computeCompleteJacobians_	(	const Matrix2N &	Lambda,
		const Matrix2N &	Psi,
		const VectorN &	xidot_tau,
		const MatrixN &	right_jac_tau,
		const MatrixN &	dTtau_dTk,
		const MatrixN &	dTtau_dxitau,
		const StateJacobians &	jacs,
		OptionalMatrixVecType	H ) const

protected

Step 4 of interpolatePoseAndVelocity: compose full Jacobians with chain rule.

Parameters

Lambda	Interpolation matrix \(\Lambda\).
Psi	Interpolation matrix \(\Psi\).
xidot_tau	Interpolated local velocity vector.
right_jac_tau	Right Jacobian of Expmap(xi_tau).
dTtau_dTk	Jacobian of \(T_\tau\) wrt \(T_k\) from composition.
dTtau_dxitau	Jacobian of \(T_\tau\) wrt \(\xi_\tau\).
jacs	Jacobians for local state wrt bordering states.
H	Output vector of Jacobian blocks to populate.

Returns: void

◆ computeConditionalCov()

template<typename PoseType>

Interpolator< PoseType >::Matrix2N gtsam::Interpolator< PoseType >::computeConditionalCov	(	const TimestampedPoseVel &	pvk,
		const TimestampedPoseVel &	pvkp1,
		const TimestampedPoseVel &	pvtau,
		OptionalMatrixType	Lambda = nullptr,
		OptionalMatrixType	Psi = nullptr ) const

Compute the conditional covariance of the interpolated state.

Given timestamped left and right bordering states and the interpolated state's timestamped value, compute the 2N x 2N conditional covariance matrix Sigma_tau. Optionally return the interpolation matrices Lambda and Psi.

Parameters

pvk	Timestamped pose/velocity at left border.
pvkp1	Timestamped pose/velocity at right border.
pvtau	Timestamped pose/velocity at interpolation time.
Lambda	Optional output pointer to receive Lambda matrix.
Psi	Optional output pointer to receive Psi matrix.

Returns: Matrix2N Conditional covariance matrix of the interpolated state.

◆ computeInterpolationCovariance_()

template<typename PoseType>

void gtsam::Interpolator< PoseType >::computeInterpolationCovariance_	(	const TimestampedPoseVel &	tPoseVel_k,
		const TimestampedPoseVel &	tPoseVel_kp1,
		const PoseVel &	poseVel_tau,
		double	t_tau,
		const Matrix2N &	Lambda,
		const Matrix2N &	Psi,
		const std::shared_ptr< Matrix > &	mainSolveMarginalMatrix,
		Matrix *	covarianceOut ) const

protected

Step 5 of interpolatePoseAndVelocity: compute interpolated covariance when requested.

Parameters

tPoseVel_k	Timestamped left border state.
tPoseVel_kp1	Timestamped right border state.
poseVel_tau	Interpolated pose/velocity at query time.
t_tau	Query timestamp.
Lambda	Interpolation matrix \(\Lambda\).
Psi	Interpolation matrix \(\Psi\).
mainSolveMarginalMatrix	Optional covariance of bordering states.
covarianceOut	Optional output covariance for interpolated state.

Returns: void

◆ ComputeJointMarginals()

template<typename PoseType>

std::map< StateDataInterval, std::shared_ptr< Matrix > > gtsam::Interpolator< PoseType >::ComputeJointMarginals	(	const std::map< StateDataInterval, std::vector< StateData > > &	queryBuckets,
		const std::unique_ptr< Marginals > &	marginals )

staticprotected

Compute joint marginal covariances for requested state intervals.

Given a set of query buckets mapping intervals to the contained StateData entries, extract and assemble the joint marginal covariance matrices from the provided Marginals object. Returns a map from the interval to the joint marginal matrix (as a shared pointer).

Parameters

queryBuckets	Mapping of StateDataInterval to a vector of StateData keys.
marginals	Unique pointer to a Marginals object computed from the main solve.

Returns: std::map<StateDataInterval, std::shared_ptr<Matrix>> Map of joint marginal matrices.

◆ computeLocalStateVecs()

template<typename PoseType>

Interpolator< PoseType >::LocalStateVecs gtsam::Interpolator< PoseType >::computeLocalStateVecs	(	const TimestampedPoseVel &	pvk,
		const TimestampedPoseVel &	pvkp1,
		StateJacobians *	jacs = nullptr ) const

Compute the local state vectors (xi, xi_dot) for a pair of bordering states.

The local state vectors are used as intermediate quantities in interpolation and in Jacobian computations. Optionally returns local-to-global Jacobians.

Parameters

pvk	Timestamped pose/velocity at left border state.
pvkp1	Timestamped pose/velocity at right border state.
jacs	Optional output pointer to receive local-to-global Jacobians.

Returns: LocalStateVecs Pair of local state vectors (xi, xi_dot).

◆ ConstructMatrixFromJointMarginal()

template<typename PoseType>

Matrix gtsam::Interpolator< PoseType >::ConstructMatrixFromJointMarginal	(	const JointMarginal &	blockMatrix,
		const KeyVector &	keyVector,
		size_t	blockSize )

staticprotected

Construct a full covariance matrix from a joint marginal block matrix.

The blockMatrix argument contains the joint marginal arranged in a block structure. This helper reorders and flattens it into a full dense matrix according to keyVector and the specified blockSize.

Parameters

blockMatrix	The joint marginal in block structure.
keyVector	Ordering of keys to determine row/column ordering.
blockSize	Size of each block (e.g., dimension of a single state block).

Returns: Matrix Full covariance matrix assembled from the joint marginal.

◆ extrapolatePoseAndVelocity()

template<typename PoseType>

Interpolator< PoseType >::PoseVel gtsam::Interpolator< PoseType >::extrapolatePoseAndVelocity	(	const PoseVelocity< PoseType > &	poseVel,
		double	t_diff,
		OptionalMatrixVecType	H = nullptr,
		const std::shared_ptr< Matrix > &	mainSolveMarginalMatrix = nullptr,
		Matrix *	covarianceOut = nullptr ) const

protected

Extrapolate pose and velocity from a border state.

Performs extrapolation when the interpolation time lies outside the interval between bordering states. The input t_diff is the time difference between the target time and the provided state's timestamp. Optionally provides Jacobians and covariance output when requested.

Parameters

poseVel	Pose and velocity at the source border.
t_diff	Time difference from source timestamp to interpolation time.
H	Optional output pointer to a vector of Jacobian blocks.
mainSolveMarginalMatrix	Optional shared pointer to the main-solve marginal covariance.
covarianceOut	Optional output pointer for the resulting covariance matrix.

Returns: PoseVel Extrapolated pose and velocity at the requested time.

◆ formLocalStateAndJacobians_()

template<typename PoseType>

void gtsam::Interpolator< PoseType >::formLocalStateAndJacobians_	(	const TimestampedPoseVel &	tPoseVel_k,
		const TimestampedPoseVel &	tPoseVel_kp1,
		const std::shared_ptr< const LocalStateVecs > &	localStateVecsPreComp,
		const std::shared_ptr< const StateJacobians > &	stateJacobiansPreComp,
		VectorN *	xi_dot_k,
		VectorN *	xi_kp1,
		VectorN *	xi_dot_kp1,
		StateJacobians *	jacs = nullptr ) const

protected

Step 1 of interpolatePoseAndVelocity: form local state vectors and optional local Jacobians.

Computes \((\xi_k, \dot\xi_k, \xi_{k+1}, \dot\xi_{k+1})\) used by interpolation. Jacobian outputs are populated only when all Jacobian pointers are non-null.

Parameters

tPoseVel_k	Timestamped left border state.
tPoseVel_kp1	Timestamped right border state.
localStateVecsPreComp	Optional precomputed local state vectors at the right border.
stateJacobiansPreComp	Optional precomputed local-to-global Jacobians.
xi_dot_k	Output local velocity vector at the left border.
xi_kp1	Output local position vector at the right border.
xi_dot_kp1	Output local velocity vector at the right border.
jacs	Optional output Jacobians for local state wrt bordering states.

Returns: void

◆ getLambdaPsi()

template<typename PoseType>

std::pair< Matrix, Matrix > gtsam::Interpolator< PoseType >::getLambdaPsi	(	double	t_k,
		double	t_kp1,
		double	t_tau ) const

Compute interpolation matrices Lambda and Psi (WNOA-optimized).

Lambda and Psi relate the bordering states to the interpolated state and are used for both calculating interpolated values and propagating covariance. This implementation is optimized for the WNOA motion prior.

Parameters

t_k	Time of the left border state.
t_kp1	Time of the right border state.
t_tau	Interpolation time.

Returns: std::pair<Matrix,Matrix> Pair (Lambda, Psi) interpolation matrices.

◆ getLambdaPsiGeneral()

template<typename PoseType>

std::pair< Matrix, Matrix > gtsam::Interpolator< PoseType >::getLambdaPsiGeneral	(	double	t_k,
		double	t_kp1,
		double	t_tau ) const

Compute interpolation matrices Lambda and Psi for a general motion model.

This general implementation does not rely on WNOA-specific simplifications and can be used for other motion priors provided the appropriate transition/covariance functions are supplied.

Parameters

t_k	Time of the left border state.
t_kp1	Time of the right border state.
t_tau	Interpolation time.

Returns: std::pair<Matrix,Matrix> Pair (Lambda, Psi) interpolation matrices.

◆ interpolateBoundaryLeft()

template<typename PoseType>

Interpolator< PoseType >::PoseVel gtsam::Interpolator< PoseType >::interpolateBoundaryLeft	(	const PoseVelocity< PoseType > &	poseVel_k,
		OptionalMatrixVecType	H = nullptr,
		const std::shared_ptr< Matrix > &	mainSolveMarginalMatrix = nullptr,
		Matrix *	covarianceOut = nullptr ) const

protected

Interpolate pose and velocity at the left boundary.

When the requested interpolation time lies at the left border state's timestamp, this routine computes the interpolated pose and velocity using boundary logic. Optionally returns Jacobian blocks and the interpolated covariance when the main-solve marginal is provided.

Parameters

poseVel_k	Pose and velocity at the left border (local representation).
H	Optional output pointer to a vector of Jacobian blocks.
mainSolveMarginalMatrix	Optional shared pointer to the main-solve marginal covariance.
covarianceOut	Optional output pointer for the resulting covariance matrix.

Returns: PoseVel Interpolated pose and velocity at the left boundary.

◆ interpolateBoundaryRight()

template<typename PoseType>

Interpolator< PoseType >::PoseVel gtsam::Interpolator< PoseType >::interpolateBoundaryRight	(	const PoseVelocity< PoseType > &	poseVel_kp1,
		OptionalMatrixVecType	H = nullptr,
		const std::shared_ptr< Matrix > &	mainSolveMarginalMatrix = nullptr,
		Matrix *	covarianceOut = nullptr ) const

protected

Interpolate pose and velocity at the right boundary.

When the requested interpolation time lies at the right border state's timestamp, this routine computes the interpolated pose and velocity using boundary logic. Optionally returns Jacobian blocks and the interpolated covariance when the main-solve marginal is provided.

Parameters

poseVel_kp1	Pose and velocity at the right border (local representation).
H	Optional output pointer to a vector of Jacobian blocks.
mainSolveMarginalMatrix	Optional shared pointer to the main-solve marginal covariance.
covarianceOut	Optional output pointer for the resulting covariance matrix.

Returns: PoseVel Interpolated pose and velocity at the right boundary.

◆ interpolateLocalState_()

template<typename PoseType>

void gtsam::Interpolator< PoseType >::interpolateLocalState_	(	double	t_k,
		double	t_kp1,
		double	t_tau,
		const VectorN &	xi_dot_k,
		const VectorN &	xi_kp1,
		const VectorN &	xi_dot_kp1,
		const std::shared_ptr< const LambdaPsiMats > &	LambdaPsiPreComp,
		Matrix2N *	Lambda,
		Matrix2N *	Psi,
		VectorN *	xi_tau,
		VectorN *	xidot_tau ) const

protected

Step 2 of interpolatePoseAndVelocity: interpolate local state using Lambda/Psi matrices.

Retrieves (or reuses precomputed) Lambda/Psi and computes \((\xi_\tau, \dot\xi_\tau)\).

Parameters

t_k	Left border timestamp.
t_kp1	Right border timestamp.
t_tau	Query timestamp.
xi_dot_k	Local velocity vector at the left border.
xi_kp1	Local position vector at the right border.
xi_dot_kp1	Local velocity vector at the right border.
LambdaPsiPreComp	Optional precomputed \((\Lambda, \Psi)\) pair.
Lambda	Output interpolation matrix \(\Lambda\).
Psi	Output interpolation matrix \(\Psi\).
xi_tau	Output interpolated local position vector.
xidot_tau	Output interpolated local velocity vector.

Returns: void

◆ interpolatePoseAndVelocity()

template<typename PoseType>

Interpolator< PoseType >::PoseVel gtsam::Interpolator< PoseType >::interpolatePoseAndVelocity	(	const std::optional< TimestampedPoseVel > &	Tvarpi_k,
		const std::optional< TimestampedPoseVel > &	Tvarpi_kp1,
		double	t_tau,
		OptionalMatrixVecType	H = nullptr,
		const std::shared_ptr< Matrix > &	mainSolveMarginalMatrix = nullptr,
		Matrix *	covarianceOut = nullptr,
		const std::shared_ptr< const LambdaPsiMats > &	LambdaPsiPreComp = nullptr,
		const std::shared_ptr< const LocalStateVecs > &	localStateVecsPreComp = nullptr,
		const std::shared_ptr< const StateJacobians > &	stateJacobiansPreComp = nullptr ) const

Interpolate the pose and velocity at time t_tau.

The function computes the interpolated pose and velocity at the requested time using the provided bordering (optional) timestamped states. When a border is missing, boundary or extrapolation logic is used.

This is the main workhorse function for interpolated factors, but is also used after the fact to compute interpolated values and covariances given a solved factor graph.

Optionally returns Jacobians (8 matrices) and the interpolated covariance when the main-solve marginals are provided.

Parameters

Tvarpi_k	Optional timestamped pose/velocity at left border t_k.
Tvarpi_kp1	Optional timestamped pose/velocity at right border t_kp1.
t_tau	Interpolation time.
H	Optional output vector of Jacobian blocks (8 matrices).
mainSolveMarginalMatrix	Optional marginal covariance of bordering states from the main solve.
covarianceOut	Optional output pointer for the interpolated state's covariance (requires mainSolveMarginalMatrix).
LambdaPsiPreComp	Optional precomputed Lambda/Psi matrices to reuse work.
localStateVecsPreComp	Optional precomputed local state vectors (xi, xi_dot).
stateJacobiansPreComp	Optional precomputed local state->global state Jacobians.

Returns: PoseVel Interpolated pose and velocity pair at t_tau.

◆ interpolatePoseAndVelocity_()

template<typename PoseType>

Interpolator< PoseType >::PoseVel gtsam::Interpolator< PoseType >::interpolatePoseAndVelocity_	(	const TimestampedPoseVel &	tPoseVel_k,
		const TimestampedPoseVel &	tPoseVel_kp1,
		double	t_tau,
		OptionalMatrixVecType	H = nullptr,
		const std::shared_ptr< Matrix > &	mainSolveMarginalMatrix = nullptr,
		Matrix *	covarianceOut = nullptr,
		const std::shared_ptr< const LambdaPsiMats > &	LambdaPsiPreComp = nullptr,
		const std::shared_ptr< const LocalStateVecs > &	localStateVecsPreComp = nullptr,
		const std::shared_ptr< const StateJacobians > &	stateJacobiansPreComp = nullptr ) const

protected

Internal implementation that performs interpolation between two bordering states.

This function follows the interpolation logic outlined in Section 11.3.1 of (Barfoot 2024) and is used by the public interpolatePoseAndVelocity method after handling boundary/extrapolation cases.

This internal overload implements the core interpolation algorithm for a pair of timestamped bordering states and a target interpolation time. It supports optional precomputed helpers (Lambda/Psi matrices and local state vectors/Jacobians) to avoid redundant work when called in tight loops.

Parameters

tPoseVel_k	Timestamped pose/velocity at the left border.
tPoseVel_kp1	Timestamped pose/velocity at the right border.
t_tau	Interpolation time.
H	Optional output pointer to a vector of Jacobian blocks.
mainSolveMarginalMatrix	Optional shared pointer to the main-solve marginal covariance.
covarianceOut	Optional output pointer for the resulting covariance matrix.
LambdaPsiPreComp	Optional precomputed Lambda/Psi matrices to reuse work.
localStateVecsPreComp	Optional precomputed local state vectors (xi, xi_dot).
stateJacobiansPreComp	Optional precomputed local->global Jacobians.

Returns: PoseVel Interpolated pose and velocity at t_tau.

◆ interpolatePosesAndVelocities()

template<typename PoseType>

Values gtsam::Interpolator< PoseType >::interpolatePosesAndVelocities	(	const NonlinearFactorGraph &	mainSolveGraph,
		const Values &	mainSolveSolution,
		const StateDataSet &	mainSolveStates,
		const StateDataSet &	interpolatedStates,
		std::shared_ptr< InterpCovarianceMap >	covarianceMapOut = nullptr ) const

Interpolate multiple poses and velocities given a solved factor graph.

This routine takes a factor graph and the solution for the estimated states and returns a Values object containing interpolated poses and velocities for the requested set of interpolatedStates.

Optionally fills covarianceMapOut with the conditional covariances for each interpolated state.

Parameters

mainSolveGraph	Factor graph used in the main solve (estimated states only).
mainSolveSolution	Solution Values from the main solve.
mainSolveStates	Ordered set of estimated StateData used in the main solve.
interpolatedStates	Set of StateData to interpolate.
covarianceMapOut	Optional output map to receive per-state covariances.

Returns: Values A Values container with interpolated pose and velocity entries.

◆ mapLocalStateToManifold_()

template<typename PoseType>

Interpolator< PoseType >::PoseVel gtsam::Interpolator< PoseType >::mapLocalStateToManifold_	(	const PoseType &	T_k,
		const VectorN &	xi_tau,
		const VectorN &	xidot_tau,
		MatrixN *	right_jac_tau,
		MatrixN *	dTtau_dTk = nullptr,
		MatrixN *	dTtau_dxitau = nullptr ) const

protected

Step 3 of interpolatePoseAndVelocity: map interpolated local state back to manifold.

Computes interpolated pose/velocity pair. Jacobian outputs are optional and populated only when dTtau_dTk and dTtau_dxitau are non-null.

Parameters

T_k	Left border pose.
xi_tau	Interpolated local position vector.
xidot_tau	Interpolated local velocity vector.
right_jac_tau	Output right Jacobian of Expmap(xi_tau).
dTtau_dTk	Optional output Jacobian of \(T_\tau\) wrt \(T_k\).
dTtau_dxitau	Optional output Jacobian of \(T_\tau\) wrt \(\xi_\tau\).

Returns: PoseVel Interpolated pose and velocity pair at \(t_\tau\).

The documentation for this class was generated from the following files:

/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/nonlinear/WnoaInterpolator.h
/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/nonlinear/WnoaInterpolator.cpp

Detailed Description

Public Member Functions

Public Types

Classes

Protected Types

Protected Member Functions

Static Protected Member Functions

Protected Attributes

Static Protected Attributes

Constructor & Destructor Documentation

◆ Interpolator() [1/2]

◆ Interpolator() [2/2]

Member Function Documentation

◆ computeCompleteJacobians_()

◆ computeConditionalCov()

◆ computeInterpolationCovariance_()

◆ ComputeJointMarginals()

◆ computeLocalStateVecs()

◆ ConstructMatrixFromJointMarginal()

◆ extrapolatePoseAndVelocity()

◆ formLocalStateAndJacobians_()

◆ getLambdaPsi()

◆ getLambdaPsiGeneral()

◆ interpolateBoundaryLeft()

◆ interpolateBoundaryRight()

◆ interpolateLocalState_()

◆ interpolatePoseAndVelocity()

◆ interpolatePoseAndVelocity_()

◆ interpolatePosesAndVelocities()

◆ mapLocalStateToManifold_()