Detailed Description

Aligns Pose3 trajectories from multiple child coordinate frames to a parent reference frame using Sim3 (similarity) transformations.

This class solves an optimization problem to find the best Sim3 transforms (rotation, translation, and scale) that align poses from one or more child coordinate frames to a parent reference frame. The optimization jointly refines both the parent frame poses and the child-to-parent transformations.

The class takes as input:

Parent frame poses (aTi): Pose3 measurements in the parent coordinate frame
Child frame poses (bTi_all): Pose3 measurements in one or more child frames
(Optional) Initial Sim3 estimates (bSa_all): Initial transforms from parent to each child frame

The output is a Values object containing:

Optimized parent frame poses (with keys from the input aTi measurements)
Optimized Sim3 transforms (with Symbol keys 'S' and index for each child)

Public Member Functions
	TrajectoryAlignerSim3 (const std::vector< UnaryMeasurement< Pose3 > > &aTi, const std::vector< std::vector< UnaryMeasurement< Pose3 > > > &bTi_all, const std::vector< Similarity3 > &bSa_all={}, const bool use_gnc_optimizer=false, const std::vector< std::vector< std::pair< Point3, Point3 > > > &overlapping_points={}, const double point3_factor_sigma=1e-2)
	Constructs a trajectory aligner with the given measurements.
Values	solve () const
	Optimizes the graph and returns optimized poses and Sim3 transforms.
Marginals	marginalize (const Values &solution, const Ordering::OrderingType ordering_type=Ordering::COLAMD) const
	Computes the marginals of the solution.

Constructor & Destructor Documentation

◆ TrajectoryAlignerSim3()

gtsam::TrajectoryAlignerSim3::TrajectoryAlignerSim3	(	const std::vector< UnaryMeasurement< Pose3 > > &	aTi,
		const std::vector< std::vector< UnaryMeasurement< Pose3 > > > &	bTi_all,
		const std::vector< Similarity3 > &	bSa_all = {},
		const bool	use_gnc_optimizer = false,
		const std::vector< std::vector< std::pair< Point3, Point3 > > > &	overlapping_points = {},
		const double	point3_factor_sigma = 1e-2 )

Constructs a trajectory aligner with the given measurements.

Parameters

aTi	Parent frame pose measurements (key-value pairs with noise)
bTi_all	Vector of child frame pose measurements, one vector per child
bSa_all	Initial Sim3 estimates transforming from parent to each child. If empty, initial estimates are computed automatically.
use_gnc_optimizer	Whether to use GncOptimizer for optimization.
overlapping_points	Vector of overlapping point3-point3 constraints, one vector per child.
point3_factor_sigma	Sigma for point3-point3 constraints.

Member Function Documentation

◆ marginalize()

Marginals gtsam::TrajectoryAlignerSim3::marginalize	(	const Values &	solution,
		const Ordering::OrderingType	ordering_type = Ordering::COLAMD ) const

Computes the marginals of the solution.

Parameters

solution	The solution to marginalize. Obtained from solve() or should contain the same keys as variables in graph_.
ordering_type	The ordering type to use for the marginalization.

Returns: The Marginals object.

◆ solve()

Values gtsam::TrajectoryAlignerSim3::solve ( ) const

Optimizes the graph and returns optimized poses and Sim3 transforms.

Returns

The optimized poses and transforms. Contains:

Parent frame poses (with keys the same as those in input aTi measurements)
Sim3 transforms (with keys Symbol('S', i), where i is the child index)

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

/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/sfm/TrajectoryAlignerSim3.h
/tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/sfm/TrajectoryAlignerSim3.cpp