gtsam Namespace Reference

Global functions in a separate testing namespace. More...

Namespaces
namespace	treeTraversal
	Internal functions used for traversing trees.
namespace	noiseModel
	All noise models live in the noiseModel namespace.
namespace	imuBias
	All bias models live in the imuBias namespace.

Classes
class	ConcurrentMap
	FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the default STL allocator. More...
class	DSFMap
	Disjoint set forest using an STL map data structure underneath Uses rank compression and union by rank, iterator version. More...
class	IndexPair
	Small utility class for representing a wrappable pairs of ints. More...
class	DSFBase
	A fast implementation of disjoint set forests that uses vector as underly data structure. More...
class	DSFVector
	DSFVector additionally keeps a vector of keys to support more expensive operations. More...
class	FastList
	FastList is a thin wrapper around std::list that uses the boost fast_pool_allocator instead of the default STL allocator. More...
class	FastMap
	FastMap is a thin wrapper around std::map that uses the boost fast_pool_allocator instead of the default STL allocator. More...
class	FastSet
	FastSet is a thin wrapper around std::set that uses the boost fast_pool_allocator instead of the default STL allocator. More...
class	ForestTraversal
	Mixin that provides depth-based top-down or bottom-up traversal. More...
class	GenericValue
	Wraps any type T so it can play as a Value. More...
struct	traits< GenericValue< ValueType > >
struct	group_tag
	tag to assert a type is a group More...
struct	multiplicative_group_tag
	Group operator syntax flavors. More...
struct	additive_group_tag
struct	traits
	A manifold defines a space in which there is a notion of a linear tangent space that can be centered around a given point on the manifold. More...
class	IsGroup
	Group Concept. More...
class	DirectProduct
	Template to construct the direct product of two arbitrary groups Assumes nothing except group structure and Testable from G and H. More...
struct	traits< DirectProduct< G, H > >
class	DirectSum
	Template to construct the direct sum of two additive groups Assumes existence of three additive operators for both groups. More...
struct	traits< DirectSum< G, H > >
struct	GroupAction
	GroupAction CRTP base class. More...
struct	LieGroup
	A CRTP helper class that implements Lie group methods Prerequisites: methods operator*, inverse, and AdjointMap, as well as a ChartAtOrigin struct that will be used to define the manifold Chart To use, simply derive, but also say "using LieGroup<Class,N>::inverse" For derivative math, see doc/math.pdf. More...
struct	lie_group_tag
	tag to assert a type is a Lie group More...
class	IsLieGroup
	Lie Group Concept. More...
class	TransformCovariance
	Functor for transforming covariance of T. More...
struct	manifold_tag
	tag to assert a type is a manifold More...
class	IsManifold
	Manifold concept. More...
struct	FixedDimension
	Give fixed size dimension of a type, fails at compile time if dynamic. More...
struct	MultiplyWithInverse
	Functor that implements multiplication of a vector b with the inverse of a matrix A. More...
struct	MultiplyWithInverseFunction
	Functor that implements multiplication with the inverse of a matrix, itself the result of a function f. More...
struct	MatrixLieGroup
	A CRTP helper class that implements matrix Lie group methods. More...
class	IsMatrixLieGroup
	Matrix Lie Group Concept. More...
class	OptionalJacobian
	OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size or dynamic Eigen matrix. More...
class	OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic >
struct	MakeJacobian
	: meta-function to generate Jacobian More...
struct	MakeOptionalJacobian
	: meta-function to generate JacobianTA optional reference Used mainly by Expressions More...
class	ProductLieGroup
	Direct product Lie group G × H. More...
struct	PowerLieGroupJacobianStorage
	Shared implementation for fixed-size and dynamic-count PowerLieGroup. More...
struct	PowerLieGroupJacobianStorage< T, Eigen::Dynamic >
class	PowerLieGroupBase
class	PowerLieGroup
	Template to construct the N-fold power of a Lie group Represents the group G^N = G x G x ... x G (N times) Assumes Lie group structure for fixed-size G and fixed N >= 1. More...
class	PowerLieGroup< G, Eigen::Dynamic >
	Dynamic-count specialization of PowerLieGroup Represents G^N for runtime-sized N while keeping G fixed-size. More...
struct	traits< ProductLieGroup< G, H > >
	Traits specialization for ProductLieGroup. More...
struct	traits< PowerLieGroup< G, N > >
	Traits specialization for PowerLieGroup. More...
class	Scheduler
	Thread pool scheduler parameterized by a queue Policy. More...
class	SemidirectLieGroup
	Left semidirect product G ⋉ H induced by Action. More...
struct	traits< SemidirectLieGroup< G, H, Action > >
class	SymmetricBlockMatrix
	This class stores a dense matrix and allows it to be accessed as a collection of blocks. More...
struct	AdjointAction
	The adjoint action phi(g,v) = Ad_g v, also useful independently. More...
class	TangentLieGroup
	Tangent Lie group TG = G ⋉ 𝔤, with dimension 2 dim(G). More...
struct	traits< TangentLieGroup< G > >
class	IsTestable
	A testable concept check that should be placed in applicable unit tests and in generic algorithms. More...
struct	equals
	Template to create a binary predicate. More...
struct	equals_star
	Binary predicate on shared pointers. More...
struct	HasTestablePrereqs
	Requirements on type to pass it to Testable template below. More...
struct	Testable
	A helper that implements the traits interface for GTSAM types. More...
class	ThreadsafeException
	Base exception type that uses tbb_allocator if GTSAM is compiled with TBB. More...
class	RuntimeErrorThreadsafe
	Thread-safe runtime error exception. More...
class	OutOfRangeThreadsafe
	Thread-safe out of range exception. More...
class	InvalidArgumentThreadsafe
	Thread-safe invalid argument exception. More...
class	CholeskyFailed
	Indicate Cholesky factorization failure. More...
struct	ValueWithDefault
	Helper struct that encapsulates a value with a default, this is just used as a member object so you don't have to specify defaults in the class constructor. More...
class	TbbOpenMPMixedScope
	An object whose scope defines a block where TBB and OpenMP parallelism are mixed. More...
struct	RedirectCout
	For Python str(). More...
struct	index_sequence
struct	make_index_sequence
struct	make_index_sequence< 0 >
struct	make_index_sequence< 1 >
class	Value
	This is the base class for any type to be stored in Values. More...
struct	vector_space_tag
	tag to assert a type is a vector space More...
struct	traits< double >
	double More...
struct	traits< float >
	float More...
struct	traits< Eigen::Matrix< double, M, N, Options, MaxRows, MaxCols > >
struct	traits< Eigen::Matrix< double, -1, -1, Options, MaxRows, MaxCols > >
struct	traits< Eigen::Matrix< double, -1, 1, Options, MaxRows, MaxCols > >
struct	traits< Eigen::Matrix< double, 1, -1, Options, MaxRows, MaxCols > >
class	IsVectorSpace
	Vector Space concept. More...
class	VerticalBlockMatrix
	This class stores a dense matrix and allows it to be accessed as a collection of vertical blocks. More...
class	WeightedSampler
class	Basis
	CRTP Base class for function bases. More...
class	EvaluationFactor
	Factor for enforcing the scalar value of the polynomial BASIS representation at x is the same as the measurement z when using a pseudo-spectral parameterization. More...
class	VectorEvaluationFactor
	Unary factor for enforcing BASIS polynomial evaluation on a parameter Matrix of size (M, N) is equal to a vector-valued measurement at the same point, when using a pseudo-spectral parameterization. More...
class	VectorComponentFactor
	Unary factor for enforcing BASIS polynomial evaluation on a parameter Matrix of size (P, N) is equal to specified measurement at the same point, when using a pseudo-spectral parameterization. More...
class	ManifoldEvaluationFactor
	For a measurement value of type T i.e. More...
class	DerivativeFactor
	A unary factor which enforces the evaluation of the derivative of a BASIS polynomial at a specified pointx is equal to the scalar measurement z. More...
class	VectorDerivativeFactor
	A unary factor which enforces the evaluation of the derivative of a BASIS polynomial at a specified point x is equal to the vector value z. More...
class	ComponentDerivativeFactor
	A unary factor which enforces the evaluation of the derivative of a BASIS polynomial is equal to the scalar value at a specific index i of a vector-valued measurement z. More...
class	CardinalSplineBasis
	Cubic cardinal-spline basis for scalar, vector, and manifold Basis functors. More...
struct	Chebyshev1Basis
	Basis of Chebyshev polynomials of the first kind https://en.wikipedia.org/wiki/Chebyshev_polynomials#First_kind These are typically denoted with the symbol T_n, where n is the degree. More...
struct	Chebyshev2Basis
	Basis of Chebyshev polynomials of the second kind. More...
class	Chebyshev2
	Chebyshev Interpolation on Chebyshev points of the second kind Note that N here, the number of points, is one less than N from 'Approximation Theory and Approximation Practice by L. More...
class	CumulativeSplineTrajectory
	Factory for expressions that sample a cumulative spline on a Lie group. More...
class	FitBasis
	Class that does regression via least squares Example usage: size_t N = 3; auto fit = FitBasis<Chebyshev2>(data_points, noise_model, N); Vector coefficients = fit.parameters();. More...
class	FourierBasis
	Fourier basis. More...
class	KernelBase
	Interface for a compactly supported continuous convolution kernel. More...
class	PiecewisePolynomial
	A one-dimensional piecewise polynomial kernel. More...
class	AlgebraicDecisionTree
	An algebraic decision tree fixes the range of a DecisionTree to double. More...
struct	traits< AlgebraicDecisionTree< T > >
class	AllDiff
	General AllDiff constraint. More...
class	Assignment
	An assignment from labels to value index (size_t). More...
class	BinaryAllDiff
	Binary AllDiff constraint. More...
class	Constraint
	Base class for constraint factors. More...
struct	Visit
	Functor performing depth-first visit to each leaf with the leaf value as the argument. More...
struct	VisitLeaf
	Functor performing depth-first visit to each leaf with the Leaf object passed as an argument. More...
struct	VisitWith
	Functor performing depth-first visit to each leaf with the leaf's Assignment<L> and value passed as arguments. More...
class	DecisionTree
	a decision tree is a function from assignments to values. More...
struct	traits< DecisionTree< L, Y > >
class	DecisionTreeFactor
	A discrete probabilistic factor. More...
struct	traits< DecisionTreeFactor >
class	DiscreteBayesNet
	A Bayes net made from discrete conditional distributions. More...
struct	traits< DiscreteBayesNet >
class	DiscreteBayesTreeClique
	A clique in a DiscreteBayesTree. More...
class	DiscreteBayesTree
	A Bayes tree representing a Discrete distribution. More...
struct	traits< DiscreteBayesTreeClique >
	traits More...
struct	traits< DiscreteBayesTree >
class	DiscreteConditional
	Discrete Conditional Density Derives from DecisionTreeFactor. More...
struct	traits< DiscreteConditional >
class	DiscreteDistribution
	A prior probability on a set of discrete variables. More...
struct	traits< DiscreteDistribution >
class	DiscreteEliminationTree
	Elimination tree for discrete factors. More...
class	DiscreteFactor
	Base class for discrete probabilistic factors The most general one is the derived DecisionTreeFactor. More...
struct	traits< DiscreteFactor >
struct	EliminationTraits< DiscreteFactorGraph >
class	DiscreteFactorGraph
	A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e. More...
struct	traits< DiscreteFactorGraph >
class	DiscreteJunctionTree
	An EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with the additional property that it represents the clique tree associated with a Bayes net. More...
struct	DiscreteKeys
	DiscreteKeys is a set of keys that can be assembled using the & operator. More...
struct	traits< DiscreteKeys >
class	DiscreteLookupTable
	DiscreteLookupTable table for max-product. More...
class	DiscreteLookupDAG
	A DAG made from lookup tables, as defined above. More...
struct	traits< DiscreteLookupDAG >
class	DiscreteMarginals
	A class for computing marginals of variables in a DiscreteFactorGraph. More...
class	DiscreteSearch
	DiscreteSearch: Search for the K best solutions. More...
class	DiscreteValues
	A map from keys to values. More...
struct	traits< DiscreteValues >
class	Domain
	The Domain class represents a constraint that restricts the possible values a particular variable, with given key, can take on. More...
class	Signature
	Signature for a discrete conditional distribution, used to construct conditionals. More...
struct	SignatureParser
	A simple parser that replaces the boost spirit parser. More...
class	SingleValue
	SingleValue constraint: ensures a variable takes on a certain value. More...
class	TableDistribution
	Distribution which uses a SparseVector as the internal representation, similar to the TableFactor. More...
struct	traits< TableDistribution >
class	TableFactor
	A discrete probabilistic factor optimized for sparsity. More...
struct	traits< TableFactor >
class	AsVectorSpace
	Opt-in adapter that equips a manifold class with additive vector-space operations. More...
struct	traits< AsVectorSpace< Class > >
	Traits that expose the adapter as a testable additive vector space. More...
struct	traits< const AsVectorSpace< Class > >
	Const-qualified traits forward to the mutable adapter traits. More...
struct	Bearing
struct	Range
struct	BearingRange
	Bearing-Range product for a particular A1,A2 combination will use the functors above to create a similar functor of type A1*A2 -> pair<Bearing::return_type,Range::return_type> For example BearingRange<Pose2,Point2>(pose,point) will return pair<Rot2,double> and BearingRange<Pose3,Point3>(pose,point) will return pair<Unit3,double>. More...
struct	traits< BearingRange< A1, A2 > >
struct	HasBearing
struct	HasRange
class	Cal3
	Common base class for all calibration models. More...
class	Cal3_S2
	The most common 5DOF 3D->2D calibration. More...
struct	traits< Cal3_S2 >
struct	traits< const Cal3_S2 >
class	Cal3_S2Stereo
	The most common 5DOF 3D->2D calibration, stereo version. More...
struct	traits< Cal3_S2Stereo >
struct	traits< const Cal3_S2Stereo >
class	Cal3Bundler
	Calibration used by Bundler. More...
struct	traits< Cal3Bundler >
struct	traits< const Cal3Bundler >
class	Cal3DS2
	Calibration of a camera with radial distortion that also supports Lie-group behaviors for optimization. More...
struct	traits< Cal3DS2 >
struct	traits< const Cal3DS2 >
class	Cal3DS2_Base
	Calibration of a camera with radial distortion. More...
class	Cal3f
	Calibration model with a single focal length and zero skew. More...
struct	traits< Cal3f >
struct	traits< const Cal3f >
class	Cal3Fisheye
	Calibration of a fisheye camera. More...
struct	traits< Cal3Fisheye >
struct	traits< const Cal3Fisheye >
class	Cal3Unified
	Calibration of a omni-directional camera with mirror + lens radial distortion. More...
struct	traits< Cal3Unified >
struct	traits< const Cal3Unified >
class	CheiralityException
class	PinholeBase
	A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras. More...
class	CalibratedCamera
	A Calibrated camera class [R|-R't], calibration K=I. More...
struct	traits< CalibratedCamera >
struct	traits< const CalibratedCamera >
struct	Range< CalibratedCamera, T >
class	CameraSet
	A set of cameras, all with their own calibration. More...
struct	traits< CameraSet< CAMERA > >
struct	traits< const CameraSet< CAMERA > >
class	PoseConcept
	Pose Concept A must contain a translation and a rotation, with each structure accessable directly and a type provided for each. More...
class	Cyclic
	Cyclic group of order N. More...
struct	traits< Cyclic< N > >
	Define cyclic group to be a model of the Additive Group concept. More...
class	EssentialMatrix
	An essential matrix is like a Pose3, except with translation up to scale It is named after the 3*3 matrix aEb = [aTb]x aRb from computer vision, but here we choose instead to parameterize it as a (Rot3,Unit3) pair. More...
struct	traits< EssentialMatrix >
struct	traits< const EssentialMatrix >
class	Event
	A space-time event models an event that happens at a certain 3D location, at a certain time. More...
struct	traits< Event >
class	TimeOfArrival
	Time of arrival to given sensor. More...
class	ExtendedPose3
	Lie group SE_k(3): semidirect product of SO(3) with k copies of R^3. More...
struct	traits< ExtendedPose3< K, Derived > >
struct	traits< const ExtendedPose3< K, Derived > >
class	FundamentalMatrix
	Represents a fundamental matrix in computer vision, which encodes the epipolar geometry between two views. More...
class	SimpleFundamentalMatrix
	Class for representing a simple fundamental matrix. More...
struct	TripleF
	Represents a set of three fundamental matrices for transferring points between three cameras. More...
struct	traits< FundamentalMatrix >
struct	traits< SimpleFundamentalMatrix >
class	Gal3
	Represents an element of the 3D Galilean group SGal(3). More...
struct	traits< Gal3 >
	Traits specialization for Gal3. More...
struct	traits< const Gal3 >
struct	Bearing< Gal3, Point3 >
struct	Range< Gal3, Point3 >
class	Line3
	A 3D line (R,a,b) : (Rot3,Scalar,Scalar). More...
struct	traits< Line3 >
struct	traits< const Line3 >
class	OrientedPlane3
	Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distance to the origin. More...
struct	traits< OrientedPlane3 >
struct	traits< const OrientedPlane3 >
class	PinholeCamera
	A pinhole camera class that has a Pose3 and a Calibration. More...
struct	traits< PinholeCamera< Calibration > >
struct	traits< const PinholeCamera< Calibration > >
struct	Range< PinholeCamera< Calibration >, T >
class	PinholeBaseK
	A pinhole camera class that has a Pose3 and a fixed Calibration. More...
class	PinholePose
	A pinhole camera class that has a Pose3 and a fixed Calibration. More...
struct	traits< PinholePose< CALIBRATION > >
struct	traits< const PinholePose< CALIBRATION > >
class	PinholeSet
	PinholeSet: triangulates point and keeps an estimate of it around. More...
struct	traits< PinholeSet< CAMERA > >
struct	traits< const PinholeSet< CAMERA > >
struct	Range< Point1, Point1 >
	Range functor between two Point1 values, using L1 distance. More...
struct	Range< Point2, Point2 >
struct	Range< Point3, Point3 >
class	Pose2
	A 2D pose (Point2,Rot2). More...
struct	traits< Pose2 >
	Add the Pose2-specific operations used by the generic QCQP conversion code. More...
struct	traits< const Pose2 >
struct	Bearing< Pose2, T >
struct	Range< Pose2, T >
class	Pose3
	A 3D pose (R,t) : (Rot3,Point3). More...
struct	traits< Pose3 >
	Add the Pose3-specific operations used by the generic QCQP conversion code. More...
struct	traits< const Pose3 >
struct	Bearing< Pose3, Point3 >
struct	Bearing< Pose3, Pose3 >
struct	Range< Pose3, T >
struct	traits< QUATERNION_TYPE >
class	Rot2
	Rotation matrix NOTE: the angle theta is in radians unless explicitly stated. More...
struct	traits< Rot2 >
struct	traits< const Rot2 >
class	Rot3
	Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIONS is not defined, or as a quaternion if it is defined. More...
struct	traits< Rot3 >
struct	traits< const Rot3 >
class	Similarity2
	2D similarity transform More...
struct	traits< Similarity2 >
struct	traits< const Similarity2 >
class	Similarity3
	3D similarity transform More...
struct	traits< Similarity3 >
struct	traits< const Similarity3 >
class	SL4
struct	traits< SL4 >
struct	traits< const SL4 >
struct	traits< SO3 >
struct	traits< const SO3 >
struct	traits< SO4 >
struct	traits< const SO4 >
class	SO
	Manifold of special orthogonal rotation matrices SO<N>. More...
struct	traits< SO< N > >
struct	traits< const SO< N > >
class	EmptyCal
	Empty calibration. More...
class	SphericalCamera
	A spherical camera class that has a Pose3 and measures bearing vectors. More...
struct	traits< SphericalCamera >
struct	traits< const SphericalCamera >
class	StereoCheiralityException
class	StereoCamera
	A stereo camera class, parameterize by left camera pose and stereo calibration. More...
struct	traits< StereoCamera >
struct	traits< const StereoCamera >
class	StereoPoint2
	A 2D stereo point, v will be same for rectified images. More...
struct	traits< StereoPoint2 >
struct	traits< const StereoPoint2 >
class	TriangulationUnderconstrainedException
	Exception thrown by triangulateDLT when SVD returns rank < 3. More...
class	TriangulationCheiralityException
	Exception thrown by triangulateDLT when landmark is behind one or more of the cameras. More...
struct	TriangulationParameters
class	TriangulationResult
	TriangulationResult is an optional point, along with the reasons why it is invalid. More...
class	Unit3
	Represents a 3D point on a unit sphere. More...
struct	traits< Unit3 >
	Define GTSAM traits. More...
struct	traits< const Unit3 >
class	DCSAM
	Class which implements Discrete-Continuous Smoothing And Mapping, as detailed in Doherty22ral (https://arxiv.org/abs/2204.11936). More...
class	DiscreteBoundaryFactor
	A discrete probabilistic factor which computes its values from the continuous factors connected to it and a set of values. More...
struct	traits< DiscreteBoundaryFactor >
class	HybridBayesNet
	A hybrid Bayes net is a collection of HybridConditionals, which can have discrete conditionals, hybrid Gaussian conditionals, or pure Gaussian conditionals. More...
struct	traits< HybridBayesNet >
	traits More...
class	HybridBayesTreeClique
	A clique in a HybridBayesTree which is a HybridConditional internally. More...
class	HybridBayesTree
	A Bayes tree representing a Hybrid density. More...
struct	traits< HybridBayesTreeClique >
	traits More...
struct	traits< HybridBayesTree >
class	BayesTreeOrphanWrapper< HybridBayesTreeClique >
	Class for Hybrid Bayes tree orphan subtrees. More...
class	HybridConditional
	Hybrid Conditional Density. More...
struct	traits< HybridConditional >
class	HybridEliminationTree
	Elimination Tree type for Hybrid Factor Graphs. More...
class	HybridFactor
	Base class for truly hybrid probabilistic factors. More...
struct	traits< HybridFactor >
class	HybridFactorGraph
	Hybrid Factor Graph Factor graph with utilities for hybrid factors. More...
class	HybridGaussianConditional
	A conditional of gaussian conditionals indexed by discrete variables, as part of a Bayes Network. More...
struct	traits< HybridGaussianConditional >
class	HybridGaussianFactor
	Implementation of a discrete-conditioned hybrid factor. More...
struct	traits< HybridGaussianFactor >
struct	EliminationTraits< HybridGaussianFactorGraph >
class	HybridGaussianFactorGraph
struct	traits< HybridGaussianFactorGraph >
class	HybridGaussianISAM
	Incremental Smoothing and Mapping (ISAM) algorithm for hybrid factor graphs. More...
struct	traits< HybridGaussianISAM >
	traits More...
class	HybridGaussianProductFactor
	Alias for DecisionTree of GaussianFactorGraphs and their scalar sums. More...
struct	traits< HybridGaussianProductFactor >
class	HybridJunctionTree
	An EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with the additional property that it represents the clique tree associated with a Bayes net. More...
class	HybridNonlinearFactor
	Implementation of a discrete-conditioned hybrid factor. More...
struct	traits< HybridNonlinearFactor >
class	HybridNonlinearFactorGraph
struct	traits< HybridNonlinearFactorGraph >
class	HybridNonlinearISAM
	Wrapper class to manage ISAM in a nonlinear context. More...
class	HybridSmoother
class	HybridValues
	HybridValues represents a collection of DiscreteValues and VectorValues. More...
struct	traits< HybridValues >
class	BayesNet
	A BayesNet is a tree of conditionals, stored in elimination order. More...
class	FactorGraph
	A factor graph is a bipartite graph with factor nodes connected to variable nodes. More...
class	EliminatableClusterTree
	A cluster-tree that eliminates to a Bayes tree. More...
struct	BayesTreeCliqueStats
	clique statistics More...
struct	BayesTreeCliqueData
	store all the sizes More...
class	BayesTree
	Bayes tree. More...
class	BayesTreeOrphanWrapper
struct	EliminationTraits
	Traits class for eliminateable factor graphs, specifies the types that result from elimination, etc. More...
class	BayesTreeCliqueBase
	This is the base class for BayesTree cliques. More...
struct	EliminationData
class	ClusterTree
	A cluster-tree is associated with a factor graph and is defined as in Koller-Friedman: each node k represents a subset \( C_k \sub X \), and the tree is family preserving, in that each factor \( f_i \) is associated with a single cluster and \( scope(f_i) \sub C_k \). More...
class	Conditional
struct	DotWriter
	DotWriter is a helper class for writing graphviz .dot files. More...
class	EdgeKey
struct	traits< EdgeKey >
	traits More...
class	EliminateableFactorGraph
	EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms. More...
class	EliminationTree
	An elimination tree is a data structure used intermediately during elimination. More...
class	Factor
class	ordering_key_visitor
class	compose_key_visitor
class	SDGraph
	SDGraph is undirected graph with variable keys and double edge weights. More...
class	SGraph
class	PredecessorMap
	Map from variable key to parent key. More...
class	InconsistentEliminationRequested
	An inference algorithm was called with inconsistent arguments. More...
class	ISAM
	A Bayes tree with an update methods that implements the iSAM algorithm. More...
struct	ConstructorTraversalData
class	JunctionTree
	A JunctionTree is a cluster tree, a set of variable clusters with factors, arranged in a tree, with the additional property that it represents the clique tree associated with a Bayes Net. More...
struct	StreamedKey
	To use the key_formatter on Keys, they must be wrapped in a StreamedKey. More...
class	key_formatter
	Output stream manipulator that will format gtsam::Keys according to the given KeyFormatter, as long as Key values are wrapped in a gtsam::StreamedKey. More...
struct	traits< Key >
class	LabeledSymbol
	Customized version of gtsam::Symbol for multi-robot use. More...
struct	traits< LabeledSymbol >
	traits More...
class	MetisIndex
	The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in METIS algorithms. More...
class	Ordering
struct	traits< Ordering >
	traits More...
class	Symbol
	Character and index key used to refer to variables. More...
class	SymbolGenerator
	Generates symbol shorthands with alternative names different than the one-letter predefined ones. More...
struct	traits< Symbol >
	traits More...
class	VariableIndex
	The VariableIndex class computes and stores the block column structure of a factor graph. More...
struct	traits< VariableIndex >
	traits More...
class	VariableSlots
	A combined factor is assembled as one block of rows for each component factor. More...
struct	traits< VariableSlots >
	traits More...
class	AcceleratedPowerMethod
	Compute maximum Eigenpair with accelerated power method. More...
class	BatchJacobianFactorBase
	Common interface for compact batch Jacobian factors. More...
class	BatchJacobianFactor
	Fixed-dimension row-sparse batch Jacobian factor. More...
struct	ConjugateGradientParameters
	Parameters for the Conjugate Gradient method. More...
struct	ConjugateGradientStats
	Diagnostics collected during a conjugate-gradient solve. More...
struct	ConjugateGradientResult
	Solution and diagnostics returned by the detailed CG interface. More...
struct	traits< Errors >
	traits More...
struct	FixedJacobianFactor
	A JacobianFactor whose residual and variable dimensions are known at compile time. More...
struct	traits< FixedJacobianFactor< M, Ns... > >
class	FlatGaussianFactor
	Optional preindexed kernels for matrix-free Gaussian factors. More...
class	GaussianBayesNet
	GaussianBayesNet is a Bayes net made from linear-Gaussian conditionals. More...
struct	traits< GaussianBayesNet >
	traits More...
class	GaussianBayesTreeClique
	A clique in a GaussianBayesTree. More...
class	GaussianBayesTree
	A Bayes tree representing a Gaussian density. More...
struct	traits< GaussianBayesTree >
	traits More...
class	GaussianConditional
	A GaussianConditional functions as the node in a Bayes network. More...
struct	traits< GaussianConditional >
	traits More...
class	GaussianDensity
	A GaussianDensity is a GaussianConditional without parents. More...
class	GaussianEliminationTree
class	GaussianFactor
	An abstract virtual base class for JacobianFactor and HessianFactor. More...
struct	traits< GaussianFactor >
	traits More...
struct	EliminationTraits< GaussianFactorGraph >
class	GaussianFactorGraph
	A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. More...
struct	traits< GaussianFactorGraph >
	traits More...
class	GaussianISAM
struct	traits< GaussianISAM >
	traits More...
class	GaussianJunctionTree
	A junction tree specialized to Gaussian factors, i.e., it is a cluster tree with Gaussian factors stored in each cluster. More...
class	HessianFactor
	A Gaussian factor using the canonical parameters (information form). More...
struct	traits< HessianFactor >
	traits More...
struct	CGState
class	System
	Helper class encapsulating the combined system |Ax-b_|^2 Needed to run Conjugate Gradients on matrices. More...
class	IterativeOptimizationParameters
	parameters for iterative linear solvers More...
class	IterativeSolver
	Base class for Iterative Solvers like SubgraphSolver. More...
class	JacobianFactor
	A Gaussian factor in the squared-error form. More...
struct	traits< JacobianFactor >
	traits More...
class	JointMarginal
	A class to store and access a joint marginal, returned from Gaussian and nonlinear covariance query APIs. More...
class	KalmanFilter
	Kalman Filter class. More...
struct	KeyInfoEntry
	Position, dimension, and scalar offset of one key in an ordered vector. More...
class	KeyInfo
	Ordered key dimensions and offsets in one flattened variable vector. More...
class	IndeterminateSystemException
	Thrown when a linear system is ill-posed. More...
class	InvalidNoiseModel
	An exception indicating that the noise model dimension passed into a JacobianFactor has a different dimensionality than the factor. More...
class	InvalidMatrixBlock
	An exception indicating that a matrix block passed into a JacobianFactor has a different dimensionality than the factor. More...
class	InvalidDenseElimination
class	MultifrontalClique
	Imperative multifrontal clique structure used by MultifrontalSolver. More...
struct	MultifrontalParameters
	Parameters for gtsam::MultifrontalSolver. More...
class	MultifrontalSolverNotSupported
	Exception for unsupported use of the new multifrontal solver. More...
class	MultifrontalSolver
	Imperative-style multifrontal solver for Gaussian factor graphs. More...
struct	traits< noiseModel::Gaussian >
	traits More...
struct	traits< noiseModel::Diagonal >
struct	traits< noiseModel::Constrained >
struct	traits< noiseModel::Isotropic >
struct	traits< noiseModel::Unit >
struct	PCGSolverParameters
	Parameters for Preconditioned Conjugate Gradient solver. More...
struct	PCGSolverResult
	Solution, convergence diagnostics, and phase timings from PCGSolver. More...
class	PCGSolver
	A virtual base class for the preconditioned conjugate gradient solver. More...
class	GaussianFactorGraphSystem
	Compiled flat-vector system used by preconditioned conjugate gradients. More...
class	PowerMethod
	Compute maximum Eigenpair with power method. More...
struct	PreconditionerParameters
class	Preconditioner
struct	DummyPreconditionerParameters
class	DummyPreconditioner
struct	BlockJacobiPreconditionerParameters
class	BlockJacobiPreconditioner
class	RegularHessianFactor
	A HessianFactor where all variables have the same dimension D. More...
struct	traits< RegularHessianFactor< D > >
class	RegularJacobianFactor
	JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator. More...
class	Sampler
	Sampling structure that keeps internal random number generators for diagonal distributions specified by NoiseModel. More...
struct	SlotEntry
	One SlotEntry stores the slot index for a variable, as well its dim. More...
class	Scatter
	Scatter is an intermediate data structure used when building a HessianFactor incrementally, to get the keys in the right order. More...
class	Subgraph
struct	SubgraphBuilderParameters
class	SubgraphBuilder
struct	SubgraphPreconditionerParameters
class	SubgraphPreconditioner
	Subgraph conditioner class, as explained in the RSS 2010 submission. More...
struct	SubgraphSolverParameters
class	SubgraphSolver
	This class implements the linear SPCG solver presented in Dellaert et al in IROS'10. More...
class	VectorValues
	VectorValues represents a collection of vector-valued variables associated each with a unique integer index. More...
struct	traits< VectorValues >
	traits More...
class	PreintegratedAhrsMeasurements
	PreintegratedAHRSMeasurements accumulates (integrates) the gyroscope measurements (rotation rates) and the corresponding covariance matrix. More...
class	AHRSFactorT
	An AHRSFactor is a three-way factor that is based on the preintegrated gyroscope measurements. More...
struct	traits< PreintegratedAhrsMeasurements >
struct	traits< AHRSFactorT< PIM > >
class	AttitudeFactor
	Unary factor that constrains the rotation component of a value. More...
struct	traits< AttitudeFactor< VALUE > >
class	BarometricFactor
	Prior on height in a cartesian frame. More...
class	CarrierPhaseFactor
	Undifferenced GNSS carrier phase factor for point positioning. More...
struct	traits< CarrierPhaseFactor >
	traits More...
class	UndifferencedCarrierPhaseFactor
	Undifferenced (raw) PPP carrier phase factor. More...
struct	traits< UndifferencedCarrierPhaseFactor >
	traits More...
class	UndifferencedCarrierPhaseFactorArm
	Undifferenced (raw) PPP carrier phase factor with lever-arm correction. More...
struct	traits< UndifferencedCarrierPhaseFactorArm >
	traits More...
class	CarrierPhaseFactorArm
	Carrier phase factor with lever arm correction. More...
struct	traits< CarrierPhaseFactorArm >
	traits More...
class	DoubleDifferenceCarrierPhaseFactor
	Double-difference carrier phase factor. More...
struct	traits< DoubleDifferenceCarrierPhaseFactor >
class	DoubleDifferenceCarrierPhaseFactorArm
	Double-difference carrier phase factor with lever arm correction. More...
struct	traits< DoubleDifferenceCarrierPhaseFactorArm >
class	PreintegratedCombinedMeasurementsT
	PreintegratedCombinedMeasurements integrates the IMU measurements (rotation rates and accelerations) and the corresponding covariance matrix. More...
class	CombinedImuFactorT
	CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle, as well as bias at previous time step), and current state (pose, velocity, bias at current time step). More...
struct	traits< PreintegrationCombinedParams >
struct	traits< PreintegratedCombinedMeasurementsT< PreintegrationType > >
struct	traits< CombinedImuFactorT< PIM > >
class	CombinedImuFactorWithGravityT
	CombinedImuFactorWithGravityT is CombinedImuFactorT with an additional GRAVITY variable, so that gravity can be optimized instead of being fixed by the preintegration parameters. More...
struct	traits< CombinedImuFactorWithGravityT< PIM, GRAVITY > >
class	ConstantVelocityFactor
	Binary factor for applying a constant velocity model to a moving body represented as a NavState. More...
class	DopplerFactor
	GNSS Doppler (range-rate) factor. More...
struct	traits< DopplerFactor >
	traits More...
class	DopplerFactorArm
	DopplerFactor with a kinematic lever-arm correction, keyed on a body Pose3. More...
struct	traits< DopplerFactorArm >
	traits More...
class	EquivariantFilter
	Equivariant Filter (EqF) for state estimation on Lie groups. More...
class	Gal3ImuEKF
	Specialized EKF for IMU-driven on Gal3. More...
class	GalileanPreintegration
	IMU preintegration on Gal(3) using GTSAM's left-invariant local error. More...
struct	GnssMeasurementBase
	Base class storing common members for GNSS measurement factors. More...
class	GPSFactor
	Prior on position in a Cartesian frame. More...
class	GPSFactorArm
	Version of GPSFactor (for Pose3) with lever arm between GPS and Body frame. More...
struct	traits< GPSFactorArm >
	traits More...
class	GPSFactorArmCalib
	Version of GPSFactorArm (for Pose3) with unknown lever arm between GPS and Body frame. More...
struct	traits< GPSFactorArmCalib >
	traits More...
class	GPSFactor2
	Version of GPSFactor for NavState, assuming zero lever arm between body frame and GPS. More...
class	GPSFactor2Arm
	Version of GPSFactor2 with lever arm between GPS and Body frame. More...
struct	traits< GPSFactor2Arm >
	traits More...
class	GPSFactor2ArmCalib
	Version of GPSFactor2Arm for an unknown lever arm between GPS and Body frame. More...
struct	traits< GPSFactor2ArmCalib >
	traits More...
struct	traits< imuBias::ConstantBias >
class	PreintegratedImuMeasurementsT
	PreintegratedImuMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations) and the corresponding covariance matrix. More...
class	ImuFactorT
	ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous time step), current state (pose and velocity at current time step), and the bias estimate. More...
class	ImuFactor2T
	ImuFactor2 is a ternary factor that uses NavStates rather than Pose/Velocity. More...
struct	traits< PreintegratedImuMeasurementsT< PreintegrationType > >
struct	traits< ImuFactorT< PIM > >
struct	traits< ImuFactor2T< PIM > >
class	ImuFactorWithGravityT
	ImuFactorWithGravityT is a 6-ways factor: in addition to the previous and current states (pose and velocity) and the bias estimate of ImuFactorT, it involves a GRAVITY variable so that gravity can be optimized instead of being fixed by the preintegration parameters. More...
struct	traits< ImuFactorWithGravityT< PIM, GRAVITY > >
class	ImuFactor2WithGravityT
	ImuFactor2WithGravityT is the NavState version of ImuFactorWithGravityT, just as ImuFactor2T is the NavState version of ImuFactorT: a 4-ways factor involving the previous and current NavStates, the bias estimate, and a GRAVITY variable so that gravity can be optimized instead of being fixed by the preintegration parameters. More...
struct	traits< ImuFactor2WithGravityT< PIM, GRAVITY > >
class	InvariantEKF
	Left-Invariant Extended Kalman Filter on a Lie group G. More...
class	LeftLinearEKF
	EKF on a Lie group with a general left–linear prediction model. More...
struct	ContactMeasurement
	Body-frame contact measurement for one foot. More...
struct	LeggedEstimatorParams
	Common estimator parameters shared by all four variants. More...
class	LeggedEstimator
	Common runtime interface shared by all four legged estimator variants. More...
class	LeggedInvariantEKF
	Invariant filter: LeftLinearEKF on ExtendedPose3(2+k). More...
class	LeggedInvariantIEKF
	Invariant filter with local graph fragment. More...
class	LeggedFixedLagSmoother
	Fixed-lag smoother over NavState and contact-episode footholds. More...
class	LeggedCombinedFixedLagSmoother
	Fixed-lag smoother with CombinedImuFactor bias evolution. More...
class	ExtendedPoseContactFactor
	Contact factor for the ExtendedPose3(2+k) graph-update variant. More...
class	ExtendedPoseHeightFactor
	Height factor for the ExtendedPose3(2+k) graph-update variant. More...
class	NavStatePointContactFactor
	Contact factor between a NavState and a foothold point variable. More...
class	Pose3PointContactFactor
	Contact factor between a Pose3 and a foothold point variable. More...
class	PointHeightFactor
	Height factor on a standalone foothold point variable. More...
class	LieGroupEKF
	Extended Kalman Filter on a Lie group G, derived from ManifoldEKF. More...
class	LieGroupPreintegration
	IMU preintegration using the SE_2(3) Lie-group structure of NavState. More...
class	MagFactor
	Factor to estimate rotation given magnetometer reading This version uses model measured bM = scale * bRn * direction + bias and assumes scale, direction, and the bias are given. More...
class	MagFactor1
	Factor to estimate rotation given magnetometer reading This version uses model measured bM = scale * bRn * direction + bias and assumes scale, direction, and the bias are given. More...
class	MagFactor2
	Factor to calibrate local Earth magnetic field as well as magnetometer bias This version uses model measured bM = bRn * nM + bias and optimizes for both nM and the bias, where nM is in units defined by magnetometer. More...
class	MagFactor3
	Factor to calibrate local Earth magnetic field as well as magnetometer bias This version uses model measured bM = scale * bRn * direction + bias and optimizes for both scale, direction, and the bias. More...
class	MagPoseFactor
	Factor to estimate rotation of a Pose2 or Pose3 given a magnetometer reading. More...
class	ManifoldEKF
	Extended Kalman Filter on a generic manifold M. More...
class	ManifoldPreintegration
	IMU pre-integration on NavState manifold. More...
class	NavState
	Navigation state: Pose (rotation, translation) + velocity Following Barrau20icra, this class belongs to the Lie group SE_2(3). More...
struct	traits< NavState >
struct	traits< const NavState >
struct	Bearing< NavState, Point3 >
struct	Range< NavState, Point3 >
class	NavStateImuEKF
	Specialized EKF for IMU-driven NavState on SE_2(3). More...
class	PlanarGyroParams
	Modeled parameters of the gyro. More...
class	PlanarGyroBiasFactor
	Models the evolution of the bias itself as a random walk. More...
class	PlanarGyroFactor
	A "between" factor for Pose2 rotation, with variable bias. More...
struct	PreintegratedRotationParams
	Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the constructor. More...
class	PreintegratedRotation
	PreintegratedRotation is the base class for all PreintegratedMeasurements classes (in AHRSFactor, ImuFactor, and CombinedImuFactor). More...
struct	traits< PreintegratedRotation >
class	PreintegrationBase
	PreintegrationBase is the base class for PreintegratedMeasurements (in ImuFactor) and CombinedPreintegratedMeasurements (in CombinedImuFactor). More...
struct	PreintegrationCombinedParams
	Parameters for pre-integration using PreintegratedCombinedMeasurements: Usage: Create just a single Params and pass a shared pointer to the constructor. More...
struct	PreintegrationParams
	Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the constructor. More...
class	PseudorangeFactor
	Simplified GNSS pseudorange model for basic positioning problems. More...
struct	traits< PseudorangeFactor >
	traits More...
class	UndifferencedPseudorangeFactor
	Undifferenced (raw) PPP pseudorange factor. More...
struct	traits< UndifferencedPseudorangeFactor >
	traits More...
class	UndifferencedPseudorangeFactorArm
	Undifferenced (raw) PPP pseudorange factor with lever-arm correction. More...
struct	traits< UndifferencedPseudorangeFactorArm >
	traits More...
class	DifferentialPseudorangeFactor
struct	traits< DifferentialPseudorangeFactor >
	traits More...
class	PseudorangeFactorArm
	GNSS pseudorange factor with lever arm correction. More...
struct	traits< PseudorangeFactorArm >
	traits More...
class	DifferentialPseudorangeFactorArm
	Differentially-corrected pseudorange factor with lever arm correction. More...
struct	traits< DifferentialPseudorangeFactorArm >
	traits More...
class	DoubleDifferencePseudorangeFactor
	Double-difference pseudorange factor. More...
struct	traits< DoubleDifferencePseudorangeFactor >
class	DoubleDifferencePseudorangeFactorArm
	Double-difference pseudorange factor with lever arm correction. More...
struct	traits< DoubleDifferencePseudorangeFactorArm >
class	Scenario
	Simple trajectory simulator. More...
class	ConstantTwistScenario
	Scenario with constant twist 3D trajectory. More...
class	AcceleratingScenario
	Accelerating from an arbitrary initial state, with optional rotation. More...
class	DiscreteScenario
	A scenario defined by discrete ground-truth measurements over time. More...
class	ScenarioRunner
class	CombinedScenarioRunner
class	AhrsScenarioRunner
class	TangentPreintegration
	Integrate on the 9D tangent space of the NavState manifold. More...
class	AdaptAutoDiff
	The AdaptAutoDiff class uses ceres-style autodiff to adapt a ceres-style Function evaluation, i.e., a function FUNCTOR that defines an operator template<typename T> bool operator()(const T* const, const T* const, T* predicted) const; For now only binary operators are supported. More...
class	AntiFactor
	A class for downdating an existing factor from a graph. More...
class	BatchFactor
	BatchFactor is a NonlinearFactor that wraps a collection of identical factors. More...
class	BatchFixedLagSmoother
class	BayesTreeMarginalizationHelper
	This class provides helper functions for marginalizing variables from a Bayes Tree. More...
class	ConcentratedGaussian
	A nonlinear density, inherits from ExtendedPriorFactor. More...
class	CustomFactor
class	DoglegParams
	Parameters for Levenberg-Marquardt optimization. More...
class	DoglegOptimizer
	This class performs Dogleg nonlinear optimization. More...
struct	DoglegOptimizerImpl
	This class contains the implementation of the Dogleg algorithm. More...
struct	DoglegLineSearchImpl
	This class contains an extension of the Dogleg Algorithm where a line search is performed across the Dogleg arc (interpolation of gradient and Gauss-Newton directions). More...
class	ExpressionFactor
	Factor that supports arbitrary expressions via AD. More...
class	ExpressionEqualityConstraint
class	Expression
	Expression class that supports automatic differentiation. More...
class	ScalarMultiplyExpression
	A ScalarMultiplyExpression is a specialization of Expression that multiplies with a scalar It optimizes the Jacobian calculation for this specific case. More...
class	BinarySumExpression
	A BinarySumExpression is a specialization of Expression that adds two expressions together It optimizes the Jacobian calculation for this specific case. More...
struct	traits< ExpressionFactor< T > >
	traits More...
class	ExpressionFactorN
	N-ary variadic template for ExpressionFactor meant as a base class for N-ary factors. More...
struct	traits< ExpressionFactorN< T, Args... > >
	traits More...
class	ExpressionFactorGraph
	Factor graph that supports adding ExpressionFactors directly. More...
class	ExtendedKalmanFilter
	This is a generic Extended Kalman Filter class implemented using nonlinear factors. More...
class	ExtendedPriorFactor
	A class for a soft prior on any Value type, but with a non-zero mean in the tangent space. More...
struct	traits< ExtendedPriorFactor< VALUE > >
	traits More...
class	FixedLagSmoother
class	FunctorizedFactor
	Factor which evaluates provided unary functor and uses the result to compute error with respect to the provided measurement. More...
struct	traits< FunctorizedFactor< R, T > >
	traits More...
class	FunctorizedFactor2
	Factor which evaluates provided binary functor and uses the result to compute error with respect to the provided measurement. More...
struct	traits< FunctorizedFactor2< R, T1, T2 > >
	traits More...
class	GaussNewtonParams
	Parameters for Gauss-Newton optimization, inherits from NonlinearOptimizationParams. More...
class	GaussNewtonOptimizer
	This class performs Gauss-Newton nonlinear optimization. More...
struct	GncIterationTiming
	Timing of one GNC outer iteration (all in seconds). More...
struct	GncTiming
	Timing of a full GncOptimizer::optimize() call. More...
class	GncOptimizer
class	GncParams
struct	GraphvizFormatting
	Formatting options and functions for saving a NonlinearFactorGraph instance in GraphViz format. More...
class	IncrementalFixedLagSmoother
	This is a base class for the various HMF2 implementations. More...
class	ISAM2BayesTree
class	ISAM2JunctionTree
struct	DeltaImpl
struct	UpdateImpl
	Implementation functions for update method All of the methods below have clear inputs and outputs, even if not functional: iSAM2 is inherintly imperative. More...
class	ISAM2
	Implementation of the full ISAM2 algorithm for incremental nonlinear optimization. More...
struct	traits< ISAM2 >
	traits More...
class	ISAM2Clique
	Specialized Clique structure for ISAM2, incorporating caching and gradient contribution TODO: more documentation. More...
struct	ISAM2GaussNewtonParams
	Parameters for ISAM2 using Gauss-Newton optimization. More...
struct	ISAM2DoglegParams
	Parameters for ISAM2 using Dogleg optimization. More...
struct	ISAM2DoglegLineSearchParams
	Parameters for ISAM2 using Dogleg Line Search optimization. More...
struct	ISAM2Params
struct	ISAM2Result
	This struct is returned from ISAM2::update() and contains information about the update that is useful for determining whether the solution is converging, and about how much work was required for the update. More...
struct	ISAM2UpdateParams
	This struct is used by ISAM2::update() to pass additional parameters to give the user a fine-grained control on how factors and relinearized, etc. More...
class	LevenbergMarquardtOptimizer
	This class performs Levenberg-Marquardt nonlinear optimization. More...
class	LevenbergMarquardtParams
	Parameters for Levenberg-Marquardt optimization. More...
class	LinearContainerFactor
	Dummy version of a generic linear factor to be injected into a nonlinear factor graph. More...
struct	traits< LinearContainerFactor >
struct	LMDampingParams
	Parameters controlling LM-style damping as applied by gtsam::NonlinearMultifrontalSolver. More...
class	Marginals
	A class for computing Gaussian marginals of variables in a NonlinearFactorGraph. More...
class	NoiseModelFactorT
	A convenient base class for creating your own NoiseModelFactor with n variables. More...
class	NonlinearConjugateGradientOptimizer
	An implementation of the nonlinear CG method using the template below. More...
class	MarginalizeNonleafException
	Thrown when requesting to marginalize out variables from ISAM2 that are not leaves. More...
class	NonlinearFactor
	Nonlinear factor base class. More...
struct	traits< NonlinearFactor >
	traits More...
class	NoiseModelFactor
	A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density \( P(z|x) \propto exp -0.5*|z-h(x)|^2_C \) Templated on the parameter type X and the values structure Values There is no return type specified for h(x). More...
class	NonlinearFactorGraph
struct	traits< NonlinearFactorGraph >
	traits More...
class	NonlinearISAM
	Wrapper class to manage ISAM in a nonlinear context. More...
class	NonlinearMultifrontalSolver
	Multifrontal solver for nonlinear factor graphs. More...
class	NonlinearOptimizer
	This is the abstract interface for classes that can optimize for the maximum-likelihood estimate of a NonlinearFactorGraph. More...
class	NonlinearOptimizerParams
	The common parameters for Nonlinear optimizers. More...
class	PriorFactor
	A class for a soft prior on any Value type. More...
struct	traits< PriorFactor< VALUE > >
	traits More...
struct	_ValuesKeyValuePair
struct	_ValuesConstKeyValuePair
struct	ValuesCastHelper
struct	ValuesCastHelper< Value, CastedKeyValuePairType, KeyValuePairType >
struct	ValuesCastHelper< const Value, CastedKeyValuePairType, KeyValuePairType >
class	ValueCloneAllocator
class	Values
	A non-templated config holding any types of Manifold-group elements. More...
class	ValuesKeyAlreadyExists
class	ValuesKeyDoesNotExist
class	ValuesIncorrectType
class	DynamicValuesMismatched
class	NoMatchFoundForFixed
struct	traits< Values >
	traits More...
class	VectorNormFactor
	Unary factor constraining the norm of an N-dimensional vector-space variable to a given value, with error = ||v|| - norm. More...
struct	traits< VectorNormFactor< N > >
	traits More...
class	WhiteNoiseFactor
	Binary factor to estimate parameters of zero-mean Gaussian white noise. More...
class	WnoaMotionFactor
	WNOA (White Noise on Acceleration) motion prior factor. More...
struct	traits< WnoaMotionFactor< Pose > >
class	WnoaFactorGraph
	Factor graph specialized for WNOA interpolation-aware computation. More...
class	WnoaInterpFactor
	Wrapper factor that evaluates an inner NoiseModelFactor on states interpolated from neighboring estimated states using a WNOA (white-noise-on-acceleration) Gaussian process interpolator. More...
struct	traits< WnoaInterpFactor< POSE > >
	traits More...
struct	PoseVelocity
	Simple container for a pose and its corresponding velocity. More...
struct	TimestampedPoseVelocity
	Timestamped pose and velocity container. More...
class	Interpolator
	Interpolator for poses and velocities under a motion prior. More...
struct	StateData
	Lightweight container for states used for continuous-time estimation and interpolation. More...
struct	BearingFactor
	Binary factor for a bearing measurement Works for any two types A1,A2 for which the functor Bearing<A1,A2>() is defined. More...
struct	traits< BearingFactor< A1, A2, T > >
	traits More...
class	BearingRangeFactor
	Binary factor for a bearing/range measurement. More...
struct	traits< BearingRangeFactor< A1, A2, B, R > >
	traits More...
class	QuadraticRangeFactor
class	RangeFactor
	Binary factor for a range measurement Works for any two types A1,A2 for which the functor Range<A1,A2>() is defined. More...
struct	traits< RangeFactor< A1, A2, T > >
	traits More...
class	RangeFactorWithTransform
	Binary factor for a range measurement, with a transform applied. More...
struct	traits< RangeFactorWithTransform< A1, A2, T > >
	traits More...
class	RangeFactorWithTransformBias
	Ternary factor for a range measurement with a fixed sensor transform and an additive bias term. More...
struct	traits< RangeFactorWithTransformBias< A1, A2, T > >
	traits More...
struct	RISAMParams
	Struct Containing all configuration parameters for riSAM. More...
struct	RISAMUpdateResult
	Struct containing information about the riSAM update. More...
class	RISAM
	Robust Incremental Smoothing and Mapping (riSAM) is a robust variant of iSAM2 for incremental factor-graph optimization. More...
class	GraduatedFactor
	Graduated Factor for riSAM base class. More...
class	GenericGraduatedFactor
	Instantiation of Graduated Factor wrapping any Nonlinear Factor. More...
class	GraduationScheduler
	Class for graduation scheduling for riSAM. More...
class	BinaryMeasurement
class	EssentialMatrixConstraint
	Binary factor between two Pose3 variables induced by an EssentialMatrix measurement. More...
class	EssentialMatrixFactor
	Factor that evaluates epipolar error p'Ep for given essential matrix. More...
class	EssentialMatrixFactor2
	Binary factor that optimizes for E and inverse depth d: assumes measurement in image 2 is perfect, and returns re-projection error in image 1. More...
class	EssentialMatrixFactor3
	Binary factor that optimizes for E and inverse depth d: assumes measurement in image 2 is perfect, and returns re-projection error in image 1 This version takes an extrinsic rotation to allow for omni-directional rigs. More...
class	EssentialMatrixFactor4
	Binary factor that optimizes for E and calibration K using the algebraic epipolar error (K^-1 pA)'E (K^-1 pB). More...
class	EssentialMatrixFactor5
	Binary factor that optimizes for E and two calibrations Ka and Kb using the algebraic epipolar error (Ka^-1 pA)'E (Kb^-1 pB). More...
class	GlobalPositioner
class	LocationRecovery
class	MFAS
	The MFAS class to solve a Minimum feedback arc set (MFAS) problem. More...
class	SelfCalibrationFactor
	Binary factor measuring how close a fundamental matrix is to admitting a valid essential matrix, as a function of the two cameras' focal lengths. More...
struct	SfmData
	SfmData stores a bunch of SfmTracks. More...
struct	traits< SfmData >
	traits More...
class	SfmLevenbergMarquardtParams
	Parameters for CPU SFM Levenberg-Marquardt optimization. More...
class	SfmLevenbergMarquardtOptimizer
	CPU SFM optimizer supporting either a joint solve or Schur elimination of Point3 and Unit3 variables. More...
struct	SfmTrack2d
	Track containing 2D measurements associated with a single 3D point. More...
struct	SfmTrack
struct	traits< SfmTrack >
struct	ShonanAveragingParameters
	Parameters governing optimization etc. More...
class	ShonanAveraging
	Class that implements Shonan Averaging from our ECCV'20 paper. More...
class	ShonanAveraging2
class	ShonanAveraging3
class	ShonanFactor
	ShonanFactor is a BetweenFactor that moves in SO(p), but will land on the SO(d) sub-manifold of SO(p) at the global minimum. More...
class	ShonanGaugeFactor
	The ShonanGaugeFactor creates a constraint on a single SO(n) to avoid moving in the stabilizer. More...
class	TrajectoryAlignerSim3
	Aligns Pose3 trajectories from multiple child coordinate frames to a parent reference frame using Sim3 (similarity) transformations. More...
class	TransferEdges
	Base class that holds the EdgeKeys and provides the getMatrices method. More...
class	TransferFactor
	Binary factor in the context of Structure from Motion (SfM). More...
class	EssentialTransferFactor
	Transfers points between views using essential matrices with a shared calibration. More...
class	EssentialTransferFactorK
	Transfers points between views using essential matrices, optimizes for calibrations of the views, as well. More...
class	TranslationFactor
	Binary factor for a relative translation direction measurement w_aZb. More...
class	BilinearAngleTranslationFactor
	Binary factor for a relative translation direction measurement w_aZb. More...
class	TranslationRecovery
class	UnaryMeasurement
	Unary measurement represents a measurement on a single key in a graph. More...
class	BetweenFactor
	A class for a measurement predicted by "between(config[key1],config[key2])". More...
struct	traits< BetweenFactor< VALUE > >
	traits More...
class	BetweenConstraint
	Binary between constraint - forces between to a given value This constraint requires the underlying type to a Lie type. More...
struct	traits< BetweenConstraint< VALUE > >
	traits More...
struct	FastSyncProjection
	Projection customization point used by fastSync(). More...
struct	FastSyncProjection< Rot2 >
	Project an ambient 2-by-2 matrix onto Rot2. More...
struct	FastSyncProjection< Rot3 >
	Project an ambient 3-by-3 matrix onto Rot3. More...
struct	FastSyncProjection< Pose2 >
	Project an ambient 3-by-3 homogeneous matrix onto Pose2. More...
struct	FastSyncProjection< Pose3 >
	Project an ambient 4-by-4 homogeneous matrix onto Pose3. More...
struct	FastSyncProjection< Similarity2 >
	Project an ambient 3-by-3 matrix onto Similarity2. More...
struct	FastSyncProjection< Similarity3 >
	Project an ambient 4-by-4 matrix onto Similarity3. More...
struct	FastSyncProjection< SL4 >
	Project an ambient 4-by-4 matrix onto SL4. More...
struct	FastSync
	Solver for the fixed-size ambient linear problem underlying FAST-Sync. More...
class	FrobeniusPrior
	FrobeniusPrior calculates the Frobenius norm between a given matrix and a fixed-size matrix Lie group element. More...
class	FrobeniusFactor
	FrobeniusFactor calculates the Frobenius norm between matrix Lie group elements. More...
class	FrobeniusBetweenFactorNL
	FrobeniusBetweenFactorNL is a BetweenFactor that evaluates the Frobenius norm of the matrix error between measured and predicted (rather than the Logmap of the error). More...
class	FrobeniusBetweenFactor
	FrobeniusBetweenFactor uses ||T2 - T1*T12_||_F, which only works if the Frobenius error is invariant to multiplying with an arbitrary element T. More...
class	FrobeniusLeftBetweenFactor
	FrobeniusLeftBetweenFactor uses ||iTw - iTj*jTw||_F, where measured iTj maps frame j into frame i. More...
class	GeneralSFMFactor
	Non-linear factor for a constraint derived from a 2D measurement. More...
struct	traits< GeneralSFMFactor< CAMERA, LANDMARK > >
class	GeneralSFMFactor2
	Non-linear factor for a constraint derived from a 2D measurement. More...
struct	traits< GeneralSFMFactor2< CALIBRATION > >
struct	InitializePose3
class	JacobianFactorQ
	JacobianFactor for Schur complement that uses Q noise model. More...
struct	traits< JacobianFactorQ< D, ZDim > >
class	JacobianFactorQR
	JacobianFactor for Schur complement that uses Q noise model. More...
class	JacobianFactorSVD
	JacobianFactor for Schur complement that uses the "Nullspace Trick" by Mourikis et al. More...
class	KarcherMeanFactor
	The KarcherMeanFactor creates a constraint on all SO(n) variables with given keys that the Karcher mean (see above) will stay the same. More...
class	KnownLandmarkFactor
	A unary known-landmark factor using the conventional GTSAM state direction. More...
class	KnownLandmarkFactor2
	A unary known-landmark factor using a k-from-world state direction. More...
class	OrientedPlane3Factor
	Factor to measure a planar landmark from a given pose. More...
class	OrientedPlane3DirectionPrior
class	PlanarProjectionFactorBase
	Camera projection for robot on the floor. More...
class	PlanarProjectionFactor1
	One variable: the pose. More...
struct	traits< PlanarProjectionFactor1 >
class	PlanarProjectionFactor2
	Two unknowns: the pose and the landmark. More...
struct	traits< PlanarProjectionFactor2 >
class	PlanarProjectionFactor3
	Three unknowns: the pose, the camera offset, and the camera calibration. More...
struct	traits< PlanarProjectionFactor3 >
class	PoseRotationPrior
class	PoseTranslationPrior
	A prior on the translation part of a pose. More...
class	GenericProjectionFactor
	Non-linear factor for a constraint derived from a 2D measurement. More...
struct	traits< GenericProjectionFactor< POSE, LANDMARK, CALIBRATION > >
	traits More...
class	ReferenceFrameFactor
	A constraint between two landmarks in separate maps Templated on: Point : Type of landmark Transform : Transform variable class. More...
struct	traits< ReferenceFrameFactor< T1, T2 > >
	traits More...
class	RegularImplicitSchurFactor
	RegularImplicitSchurFactor. More...
struct	traits< RegularImplicitSchurFactor< CAMERA > >
class	RelativeTranslationFactor
class	RotateFactor
	Factor on unknown rotation iRC that relates two incremental rotations c1Rc2 = iRc' * i1Ri2 * iRc Which we can write (see doc/math.lyx) e^[z] = iRc' * e^[p] * iRc = e^([iRc'*p]) with z and p measured and predicted angular velocities, and hence p = iRc * z. More...
class	RotateDirectionsFactor
	Factor on unknown rotation iRc that relates two directions c Directions provide less constraints than a full rotation. More...
class	SmartFactorBase
	Base class for smart factors. More...
struct	SmartProjectionParams
class	SmartProjectionFactor
	Smart factor for monocular cameras whose pose and calibration are variables. More...
struct	traits< SmartProjectionFactor< CAMERA > >
	Testable traits for SmartProjectionFactor. More...
class	SmartProjectionFactorBase
	Common base for monocular smart projection factors. More...
struct	traits< SmartProjectionFactorBase< CAMERA > >
	traits More...
class	SmartProjectionPoseFactor
	If you are using the factor, please cite: L. More...
struct	traits< SmartProjectionPoseFactor< CALIBRATION > >
	traits More...
class	SmartProjectionRigFactor
	If you are using the factor, please cite: L. More...
struct	traits< SmartProjectionRigFactor< CAMERA > >
	traits More...
class	GenericStereoFactor
	A Generic Stereo Factor. More...
struct	traits< GenericStereoFactor< T1, T2 > >
	traits More...
class	TriangulationFactor
	Non-linear factor for a constraint derived from a 2D measurement. More...
class	WahbaFactor
	A chordal Wahba factor relating two directions through an unknown rotation. More...
class	IndexedJunctionTree
	A symbolic junction tree whose factors record the original factor indices from a corresponding (non-symbolic) factor graph. More...
class	SymbolicBayesNet
	A SymbolicBayesNet is a Bayes Net of purely symbolic conditionals. More...
struct	traits< SymbolicBayesNet >
	traits More...
class	SymbolicBayesTreeClique
	A clique in a SymbolicBayesTree. More...
class	SymbolicBayesTree
	A Bayes tree that represents the connectivity between variables but is not associated with any probability functions. More...
struct	traits< SymbolicBayesTreeClique >
	traits More...
struct	traits< SymbolicBayesTree >
class	SymbolicConditional
	SymbolicConditional is a conditional with keys but no probability data, produced by symbolic elimination of SymbolicFactor. More...
struct	traits< SymbolicConditional >
	traits More...
class	SymbolicEliminationTree
struct	traits< SymbolicEliminationTree >
	traits More...
class	SymbolicFactor
	SymbolicFactor represents a symbolic factor that specifies graph topology but is not associated with any numerical function. More...
struct	traits< SymbolicFactor >
	traits More...
struct	EliminationTraits< SymbolicFactorGraph >
class	SymbolicFactorGraph
	Symbolic Factor Graph. More...
struct	traits< SymbolicFactorGraph >
	traits More...
class	SymbolicISAM
class	SymbolicJunctionTree
	A EliminatableClusterTree, i.e., a set of variable clusters with factors, arranged in a tree, with the additional property that it represents the clique tree associated with a Bayes net. More...
class	BTree
	Binary tree. More...
class	DSF
	Disjoint Set Forest class. More...
class	CSP
	Constraint Satisfaction Problem class A specialization of a DiscreteFactorGraph. More...
struct	traits< abc::State< N > >
struct	traits< const abc::State< N > >
class	InvDepthCamera3
	A pinhole camera class that has a Pose3 and a Calibration. More...
class	Pose3Upright
	A 3D Pose with fixed pitch and roll. More...
struct	traits< Pose3Upright >
class	SimPolygon2D
	General polygon class for convex polygons. More...
class	SimWall2D
	General Wall class for walls defined around unordered endpoints Primarily to handle ray intersections. More...
struct	traits< SimWall2D >
	traits More...
class	ConcurrentBatchFilter
	A Levenberg-Marquardt Batch Filter that implements the Concurrent Filtering and Smoother interface. More...
struct	traits< ConcurrentBatchFilter >
	traits More...
class	ConcurrentBatchSmoother
	A Levenberg-Marquardt Batch Smoother that implements the Concurrent Filtering and Smoother interface. More...
struct	traits< ConcurrentBatchSmoother >
	traits More...
class	ConcurrentFilter
	The interface for the 'Filter' portion of the Concurrent Filtering and Smoother architecture. More...
class	ConcurrentSmoother
	The interface for the 'Smoother' portion of the Concurrent Filtering and Smoother architecture. More...
class	ConcurrentIncrementalFilter
	An iSAM2-based Batch Filter that implements the Concurrent Filtering and Smoother interface. More...
struct	traits< ConcurrentIncrementalFilter >
	traits More...
class	ConcurrentIncrementalSmoother
	A Levenberg-Marquardt Batch Smoother that implements the Concurrent Filtering and Smoother interface. More...
struct	traits< ConcurrentIncrementalSmoother >
	traits More...
class	NonlinearClusterTree
class	BetweenFactorEM
	A class for a measurement predicted by "between(config[key1],config[key2])". More...
struct	traits< BetweenFactorEM< VALUE > >
	traits More...
class	BiasedGPSFactor
	A class to model GPS measurements, including a bias term which models common-mode errors and that can be partially corrected if other sensors are used. More...
class	GaussMarkov1stOrderFactor
struct	traits< GaussMarkov1stOrderFactor< VALUE > >
	traits More...
class	InvDepthFactor3
	Ternary factor representing a visual measurement that includes inverse depth. More...
class	InvDepthFactorVariant1
	Binary factor representing a visual measurement using an inverse-depth parameterization. More...
class	InvDepthFactorVariant2
	Binary factor representing a visual measurement using an inverse-depth parameterization. More...
class	InvDepthFactorVariant3a
	Binary factor representing the first visual measurement using an inverse-depth parameterization. More...
class	InvDepthFactorVariant3b
	Ternary factor representing a visual measurement using an inverse-depth parameterization. More...
class	LocalOrientedPlane3Factor
	Factor to measure a planar landmark from a given pose, with a given local linearization point. More...
class	MultiProjectionFactor
	Non-linear factor for a constraint derived from a 2D measurement. More...
class	PoseBetweenFactor
	A class for a measurement predicted by "between(config[key1],config[key2])". More...
class	PosePriorFactor
	A class for a soft prior on any Value type. More...
class	PoseToPointFactor
	A class for a measurement between a pose and a point. More...
class	ProjectionFactorPPP
	Non-linear factor for a constraint derived from a 2D measurement. More...
struct	traits< ProjectionFactorPPP< POSE, LANDMARK, CALIBRATION > >
	traits More...
class	ProjectionFactorPPPC
	Non-linear factor for a constraint derived from a 2D measurement. More...
struct	traits< ProjectionFactorPPPC< POSE, LANDMARK, CALIBRATION > >
	traits More...
class	ProjectionFactorRollingShutter
	Non-linear factor for 2D projection measurement obtained using a rolling shutter camera. More...
struct	traits< ProjectionFactorRollingShutter >
	traits More...
class	RelativeElevationFactor
	Binary factor for a relative elevation. More...
class	SmartProjectionPoseFactorRollingShutter
	If you are using the factor, please cite: L. More...
struct	traits< SmartProjectionPoseFactorRollingShutter< CAMERA > >
	traits More...
class	SmartRangeFactor
	Smart factor for range SLAM. More...
class	SmartStereoProjectionFactor
	SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around. More...
struct	traits< SmartStereoProjectionFactor >
	traits More...
class	SmartStereoProjectionFactorPP
	If you are using the factor, please cite: L. More...
struct	traits< SmartStereoProjectionFactorPP >
	traits More...
class	SmartStereoProjectionPoseFactor
	If you are using the factor, please cite: L. More...
struct	traits< SmartStereoProjectionPoseFactor >
	traits More...
class	TOAFactor
	A "Time of Arrival" factor - so little code seems hardly worth it :-). More...
class	TransformBtwRobotsUnaryFactor
	A class for a measurement predicted by "between(config[key1],config[key2])". More...
struct	traits< TransformBtwRobotsUnaryFactor< VALUE > >
	traits More...
class	TransformBtwRobotsUnaryFactorEM
	A class for a measurement predicted by "between(config[key1],config[key2])". More...
struct	traits< TransformBtwRobotsUnaryFactorEM< VALUE > >
	traits More...
class	DeltaFactor
	DeltaFactor: relative 2D measurement between Pose2 and Point2. More...
class	DeltaFactorBase
	DeltaFactorBase: relative 2D measurement between Pose2 and Point2, with Basenodes. More...
class	OdometryFactorBase
	OdometryFactorBase: Pose2 odometry, with Basenodes. More...
class	vector
	STL class. More...
class	Diagonal
	A diagonal noise model implements a diagonal covariance matrix, with the elements of the diagonal specified in a Vector. More...
class	Gaussian
	Gaussian implements the mathematical model |R*x|^2 = |y|^2 with R'*R=inv(Sigma) where y = whiten(x) = R*x x = unwhiten(x) = inv(R)*y as indeed |y|^2 = y'*y = x'*R'*R*x Various derived classes are available that are more efficient. More...
class	Isotropic
	An isotropic noise model corresponds to a scaled diagonal covariance To construct, use one of the static methods. More...
class	Constrained
	A Constrained constrained model is a specialization of Diagonal which allows some or all of the sigmas to be zero, forcing the error to be zero there. More...

Typedefs
typedef std::vector< IndexPair >	IndexPairVector
typedef std::set< IndexPair >	IndexPairSet
typedef std::map< IndexPair, IndexPairSet >	IndexPairSetMap
typedef DSFMap< IndexPair >	DSFMapIndexPair
template<typename T>
using	FastVector
	FastVector is a type alias to a std::vector with a custom memory allocator.
typedef Eigen::MatrixXd	Matrix
typedef Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor >	MatrixRowMajor
using	ConstMatrixView
	Dynamic-stride const Matrix view for accepting NumPy arrays without copies.
typedef Eigen::Block< Matrix >	SubMatrix
typedef Eigen::Block< const Matrix >	ConstSubMatrix
template<typename Y>
using	PriorityScheduler = Scheduler<Y, detail::PrioritySchedulerPolicy>
	Thread pool scheduler that prioritizes tasks by numeric priority.
template<typename Y>
using	TaskScheduler = Scheduler<Y, detail::TaskSchedulerPolicy>
	Thread pool scheduler that executes tasks without priority ordering.
typedef std::uint64_t	Key
	Integer nonlinear key type.
typedef std::uint64_t	FactorIndex
	Integer nonlinear factor index type.
typedef ptrdiff_t	DenseIndex
	The index type for Eigen objects.
template<class... T>
using	index_sequence_for = make_index_sequence<sizeof...(T)>
typedef Eigen::VectorXd	Vector
typedef Eigen::Matrix< double, 1, 1 >	Vector1
typedef Eigen::Vector2d	Vector2
typedef Eigen::Vector3d	Vector3
using	Vector4 = Eigen::Matrix<double, 4, 1>
using	Vector5 = Eigen::Matrix<double, 5, 1>
using	Vector6 = Eigen::Matrix<double, 6, 1>
using	Vector7 = Eigen::Matrix<double, 7, 1>
using	Vector8 = Eigen::Matrix<double, 8, 1>
using	Vector9 = Eigen::Matrix<double, 9, 1>
using	Vector10 = Eigen::Matrix<double, 10, 1>
using	Vector11 = Eigen::Matrix<double, 11, 1>
using	Vector12 = Eigen::Matrix<double, 12, 1>
using	Vector15 = Eigen::Matrix<double, 15, 1>
typedef Eigen::VectorBlock< Vector >	SubVector
typedef Eigen::VectorBlock< const Vector >	ConstSubVector
using	Weights = Eigen::Matrix<double, 1, -1>
using	Sequence = std::map<double, double>
	Our sequence representation is a map of {x: y} values where y = f(x).
using	Sample = std::pair<double, double>
	A sample is a key-value pair from a sequence.
using	Domains = std::map<Key, Domain>
using	DiscreteCluster = DiscreteJunctionTree::Cluster
	typedef for wrapper:
using	DiscreteKey = std::pair<Key,size_t>
	Key type for discrete variables.
using	Slot = DiscreteSearch::Slot
using	Solution = DiscreteSearch::Solution
using	SearchNodeQueue
using	DiscreteSearchSolution = DiscreteSearch::Solution
using	Row = std::vector<double>
using	Table = std::vector<Row>
using	Se23 = ExtendedPose3<2>
	Convenience typedef for dynamic-k ExtendedPose3.
using	ExtendedPose3d = ExtendedPose3<Eigen::Dynamic>
using	Velocity3 = Vector3
	Velocity is currently typedef'd to Vector3.
typedef Vector1	Point1
using	Point1Pair = std::pair<Point1, Point1>
	Convenience typedefs.
using	Point1Pairs = std::vector<Point1Pair>
typedef Vector2	Point2
	As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2.
using	Point2Pair = std::pair<Point2, Point2>
using	Point2Pairs = std::vector<Point2Pair>
typedef std::vector< Point2, Eigen::aligned_allocator< Point2 > >	Point2Vector
using	Point3 = Vector3
	As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3.
using	Point3Vector = std::vector<Point3, Eigen::aligned_allocator<Point3>>
using	Point3Pair = std::pair<Point3, Point3>
using	Point3Pairs = std::vector<Point3Pair>
using	Pose2Pair = std::pair<Pose2, Pose2>
using	Pose2Pairs = std::vector<Pose2Pair>
using	Pose3Pair = std::pair<Pose3, Pose3>
using	Pose3Pairs = std::vector<std::pair<Pose3, Pose3> >
typedef std::vector< Pose3 >	Pose3Vector
typedef Eigen::Quaternion< double, Eigen::DontAlign >	Quaternion
using	Rot3Vector = std::vector<Rot3, Eigen::aligned_allocator<Rot3>>
	std::vector of Rot3s, used in Matlab wrapper
using	PinholePoseCal3_S2 = gtsam::PinholePose<gtsam::Cal3_S2>
	Convenient aliases for Pinhole camera classes with different calibrations.
using	PinholePoseCal3Bundler = gtsam::PinholePose<gtsam::Cal3Bundler>
using	PinholePoseCal3DS2 = gtsam::PinholePose<gtsam::Cal3DS2>
using	PinholePoseCal3Unified = gtsam::PinholePose<gtsam::Cal3Unified>
using	PinholePoseCal3Fisheye = gtsam::PinholePose<gtsam::Cal3Fisheye>
using	PinholeCameraCal3_S2 = gtsam::PinholeCamera<gtsam::Cal3_S2>
using	PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>
using	PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>
using	PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>
using	PinholeCameraCal3Fisheye = gtsam::PinholeCamera<gtsam::Cal3Fisheye>
using	SO3 = SO<3>
using	SO4 = SO<4>
using	SOn = SO<Eigen::Dynamic>
using	DynamicJacobian = OptionalJacobian<Eigen::Dynamic, Eigen::Dynamic>
typedef std::vector< StereoPoint2 >	StereoPoint2Vector
using	CameraSetPinholePoseCal3Bundler = CameraSet<PinholePose<Cal3Bundler>>
using	CameraSetPinholePoseCal3_S2 = CameraSet<PinholePose<Cal3_S2>>
using	CameraSetPinholePoseCal3DS2 = CameraSet<PinholePose<Cal3DS2>>
using	CameraSetPinholePoseCal3Fisheye = CameraSet<PinholePose<Cal3Fisheye>>
using	CameraSetPinholePoseCal3Unified = CameraSet<PinholePose<Cal3Unified>>
using	CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>
using	CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>
using	CameraSetCal3DS2 = CameraSet<PinholeCamera<Cal3DS2>>
using	CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>
using	CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>
using	CameraSetSpherical = CameraSet<SphericalCamera>
using	AlgebraicDecisionTreeKey = AlgebraicDecisionTree<Key>
using	SharedFactor = std::shared_ptr<Factor>
using	GaussianFactorValuePair = std::pair<GaussianFactor::shared_ptr, double>
	Alias for pair of GaussianFactor::shared_pointer and a double value.
using	OrphanWrapper = BayesTreeOrphanWrapper<HybridBayesTree::Clique>
using	ResultTree = DecisionTree<Key, Result>
using	Y = GaussianFactorGraphValuePair
using	GaussianFactorGraphValuePair = std::pair<GaussianFactorGraph, double>
using	NonlinearFactorValuePair
	Alias for a NoiseModelFactor shared pointer and double scalar pair.
typedef FastVector< FactorIndex >	FactorIndices
	Define collection types:
typedef FastSet< FactorIndex >	FactorIndexSet
using	FactorErrorPredicate
	Predicate used to select factor errors for graph diagnostics.
using	KeyFormatter = std::function<std::string(Key)>
	Typedef for a function to format a key, i.e. to convert it to a string.
using	KeyVector = FastVector<Key>
	Define collection type once and for all - also used in wrappers.
using	KeyList = FastList<Key>
using	KeySet = FastSet<Key>
using	KeyGroupMap = FastMap<Key, int>
using	Sparse = Eigen::SparseMatrix<double>
using	Errors = FastList<Vector>
	Errors is a vector of errors.
using	SparseTriplets = std::vector<std::tuple<int, int, double> >
using	Dims = std::vector<Key>
using	KeyDimMap = std::map<Key, size_t>
	Map from variable key to dimension.
typedef noiseModel::Base::shared_ptr	SharedNoiseModel
	Aliases.
typedef noiseModel::Gaussian::shared_ptr	SharedGaussian
typedef noiseModel::Diagonal::shared_ptr	SharedDiagonal
typedef noiseModel::Constrained::shared_ptr	SharedConstrained
typedef noiseModel::Isotropic::shared_ptr	SharedIsotropic
typedef Eigen::SparseMatrix< double, Eigen::ColMajor, int >	SparseEigen
	Eigen-format sparse matrix.
using	AHRSFactor = AHRSFactorT<PreintegratedAhrsMeasurements>
using	CarrierPhaseBase = GnssMeasurementBase
	Base class storing common members for carrier phase factors.
typedef ManifoldPreintegration	DefaultPreintegrationType
using	PreintegratedCombinedMeasurements = PreintegratedCombinedMeasurementsT<DefaultPreintegrationType>
using	CombinedImuFactor = CombinedImuFactorT<>
using	CombinedImuFactorWithGravityDirection
	CombinedImuFactor variant optimizing the gravity direction (Unit3) with a fixed, known magnitude; see ImuFactorWithGravityDirection.
using	CombinedImuFactorWithGravityVector
	CombinedImuFactor variant optimizing the free gravity vector (Point3); see ImuFactorWithGravityVector.
typedef Expression< NavState >	NavState_
typedef Expression< Velocity3 >	Velocity3_
using	PreintegratedImuMeasurementsG
	Galilean preintegration with generic PIM covariance propagation.
using	GalileanImuFactor = ImuFactorT<PreintegratedImuMeasurementsG>
	Five-way IMU factor using Galilean preintegration.
using	GalileanImuFactor2 = ImuFactor2T<PreintegratedImuMeasurementsG>
	Three-way NavState IMU factor using Galilean preintegration.
using	PreintegratedCombinedMeasurementsG
	Galilean preintegration with combined IMU and bias covariance propagation.
using	GalileanCombinedImuFactor
	Six-way Combined IMU factor using Galilean preintegration.
using	PreintegratedImuMeasurements = PreintegratedImuMeasurementsT<DefaultPreintegrationType>
using	ImuFactor = ImuFactorT<>
using	ImuFactor2 = ImuFactor2T<>
using	ImuFactorWithGravityDirection
	ImuFactor variant with the gravity direction as an optimized Unit3 variable and a fixed, known magnitude (given at construction, defaulting to the norm of the gravity vector in the preintegration params).
using	ImuFactorWithGravityVector
	ImuFactor variant with the gravity vector as a free Point3 variable, ie.
using	ImuFactor2WithGravityDirection
	ImuFactor2 variant with the gravity direction as an optimized Unit3 variable and a fixed, known magnitude; see ImuFactorWithGravityDirection.
using	ImuFactor2WithGravityVector
	ImuFactor2 variant with the gravity vector as a free Point3 variable, ie.
using	PseudorangeBase = GnssMeasurementBase
	Base class storing common members for GNSS-related pseudorange factors.
using	JacobianVector = std::vector<Matrix>
using	OptionalJacobianVector = std::optional<std::reference_wrapper<JacobianVector>>
	Jacobians handed to the Python callback, absent when only the error is wanted.
using	CustomErrorFunction = std::function<Vector(const CustomFactor &, const Values &, OptionalJacobianVector)>
typedef internal::DoglegState	State
typedef Expression< double >	Double_
typedef Expression< Vector1 >	Vector1_
typedef Expression< Vector2 >	Vector2_
typedef Expression< Vector3 >	Vector3_
typedef Expression< Vector4 >	Vector4_
typedef Expression< Vector5 >	Vector5_
typedef Expression< Vector6 >	Vector6_
typedef Expression< Vector7 >	Vector7_
typedef Expression< Vector8 >	Vector8_
typedef Expression< Vector9 >	Vector9_
typedef FixedLagSmoother::KeyTimestampMap	FixedLagSmootherKeyTimestampMap
	Typedef for matlab wrapping.
typedef FixedLagSmootherKeyTimestampMap::value_type	FixedLagSmootherKeyTimestampMapValue
typedef FixedLagSmoother::Result	FixedLagSmootherResult
typedef FastMap< char, Vector >	ISAM2ThresholdMap
	Parameters for the ISAM2 algorithm.
typedef ISAM2ThresholdMap::value_type	ISAM2ThresholdMapValue
template<class... ValueTypes>
using	NoiseModelFactorN = NoiseModelFactorT<Vector, ValueTypes...>
	Noise model factor with N value types and dynamic-sized error vector.
using	OptionalMatrixType = Matrix*
	This typedef will be used everywhere boost::optional<Matrix&> reference was used previously.
using	OptionalMatrixVecType = std::vector<Matrix>*
	The OptionalMatrixVecType is a pointer to a vector of matrices.
typedef NonlinearOptimizerParams	SuccessiveLinearizationParams
using	StateDataInterval
	Type alias for a pair of optional bordering states for an interpolated state.
typedef std::map< Key, Matrix >	InterpCovarianceMap
	Type alias for a map from variable keys to interpolated state covariances.
using	QuadraticRangeFactor2 = QuadraticRangeFactor<2>
using	QuadraticRangeFactor3 = QuadraticRangeFactor<3>
typedef std::map< std::pair< Key, Key >, double >	KeyPairDoubleMap
typedef PinholeCamera< Cal3Bundler >	SfmCamera
	Define the structure for the camera poses.
typedef std::pair< size_t, Point2 >	SfmMeasurement
	A measurement with its camera index.
typedef std::pair< size_t, size_t >	SiftIndex
	Sift index for SfmTrack.
using	SfmTrack2dVector = std::vector<SfmTrack2d>
using	ShonanAveragingParameters2 = ShonanAveragingParameters<2>
using	ShonanAveragingParameters3 = ShonanAveragingParameters<3>
using	ShonanFactor2 = ShonanFactor<2>
using	ShonanFactor3 = ShonanFactor<3>
template<typename T>
using	Parser
using	BearingRange2D = BearingRange<Pose2, Point2>
using	BearingRange3D = BearingRange<Pose3, Point3>
typedef std::pair< size_t, Pose2 >	IndexedPose
	Return type for auxiliary functions.
typedef std::pair< size_t, Point2 >	IndexedLandmark
typedef std::pair< std::pair< size_t, size_t >, Pose2 >	IndexedEdge
using	GraphAndValues
	Return type for load functions, which return a graph and initial values.
using	BetweenFactorPose2s = std::vector<BetweenFactor<Pose2>::shared_ptr>
using	BetweenFactorPose3s = std::vector<BetweenFactor<Pose3>::shared_ptr>
using	BetweenFactorSL4s = std::vector<BetweenFactor<SL4>::shared_ptr>
using	BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>
using	BinaryMeasurementsPoint3 = std::vector<BinaryMeasurement<Point3>>
using	BinaryMeasurementsRot3 = std::vector<BinaryMeasurement<Rot3>>
typedef Expression< Point2 >	Point2_
typedef Expression< Rot2 >	Rot2_
typedef Expression< Pose2 >	Pose2_
typedef Expression< Point3 >	Point3_
typedef Expression< Unit3 >	Unit3_
typedef Expression< Rot3 >	Rot3_
typedef Expression< Pose3 >	Pose3_
typedef Expression< Line3 >	Line3_
typedef Expression< OrientedPlane3 >	OrientedPlane3_
typedef Expression< Cal3_S2 >	Cal3_S2_
typedef Expression< Cal3Bundler >	Cal3Bundler_
template<class T>
using	FrobeniusErrorVector
typedef std::map< Key, std::vector< size_t > >	KeyVectorMap
typedef std::map< Key, Rot3 >	KeyRotMap
using	RelativeTranslationFactor2 = RelativeTranslationFactor<2>
using	RelativeTranslationFactor3 = RelativeTranslationFactor<3>
using	SymbolicCluster = SymbolicJunctionTree::Cluster
	typedef for wrapper:
typedef DSF< int >	DSFInt
typedef std::vector< SimPolygon2D >	SimPolygon2DVector
typedef std::vector< SimWall2D >	SimWall2DVector
typedef ConcurrentBatchFilter::Result	ConcurrentBatchFilterResult
	Typedef for Matlab wrapping.
typedef ConcurrentBatchSmoother::Result	ConcurrentBatchSmootherResult
	Typedef for Matlab wrapping.
typedef ConcurrentIncrementalFilter::Result	ConcurrentIncrementalFilterResult
	Typedef for Matlab wrapping.
typedef ConcurrentIncrementalSmoother::Result	ConcurrentIncrementalSmootherResult
	Typedef for Matlab wrapping.
typedef SmartProjectionParams	SmartStereoProjectionParams

Enumerations
enum class	ActionType { Left , Right }
	Enum to specify whether the action is a Left or Right action.
enum class	ConjugateGradientTerminationReason { kConverged , kMaxIterations , kNumericalBreakdown }
	Reason a conjugate-gradient solve stopped. More...
enum class	ImuFactorErrorMode { Legacy , ComponentWise , Logmap }
	Error chart used by IMU factors. More...
enum class	GncFactorType {   Normal , Inlier , Outlier , NonNoiseModel ,   NullPointer }
	Enum to classify factor types in GNC optimization. More...
enum	GncLossType { GM , TLS }
	Choice of robust loss function for GNC.
enum class	GncScheduler { Linear , SuperLinear }
	Choice of GNC scheduling strategy. More...
enum class	DirectionMethod { FletcherReeves , PolakRibiere , HestenesStiefel , DaiYuan }
enum class	SfmEliminationMode { Full , Schur }
	Determines whether an SFM optimizer solves the joint or reduced system. More...
enum	NoiseFormat {   NoiseFormatG2O , NoiseFormatTORO , NoiseFormatGRAPH , NoiseFormatCOV ,   NoiseFormatAUTO }
	Indicates how noise parameters are stored in file. More...
enum	KernelFunctionType { KernelFunctionTypeNONE , KernelFunctionTypeHUBER , KernelFunctionTypeTUKEY }
	Robust kernel type to wrap around quadratic noise model.
enum	LinearizationMode { HESSIAN , IMPLICIT_SCHUR , JACOBIAN_Q , JACOBIAN_SVD }
	SmartFactorParams: parameters and (linearization/degeneracy) modes for SmartProjection and SmartStereoProjection factors. More...
enum	DegeneracyMode { IGNORE_DEGENERACY , ZERO_ON_DEGENERACY , HANDLE_INFINITY }
	How to manage degeneracy.

Functions
template<typename T>
void	testDefaultChart (TestResult &result_, const std::string &name_, const T &value)
pair< size_t, bool >	choleskyCareful (Matrix &ATA, int order=-1)
	"Careful" Cholesky computes the positive square-root of a positive symmetric semi-definite matrix (i.e.
bool	choleskyPartial (Matrix &ABC, size_t nFrontal, size_t topleft=0)
	Partial Cholesky computes a factor [R S such that [R' 0 [R S = [A B 0 L] S' I] 0 L] B' C].
bool	guardedIsDebug (const std::string &s)
void	guardedSetDebug (const std::string &s, const bool v)
bool	isDebugVersion ()
IndexPairVector	IndexPairSetAsArray (IndexPairSet &set)
template<class T>
GenericValue< T >	genericValue (const T &v)
	Functional constructor of GenericValue<T> so T can be automatically deduced.
template<typename G>
	check_group_invariants (const G &a, const G &b, double tol=1e-9)
	Check invariants.
template<typename G>
	compose_pow (const G &g, size_t n)
	compose multiple times
template<class Action, class M_, class G_>
bool	rightActionEqual (const Action &phi, const M_ &m, const G_ &g1, const G_ &g2)
	Check right action property: φ(m, g1 g2) = φ(φ(m, g1), g2).
template<class Action, class G_, class M_>
bool	leftActionEqual (const Action &phi, const G_ &g1, const G_ &g2, const M_ &m)
	Check left action property: φ(g1 g2, m) = φ(g1, φ(g2, m)).
template<class Class>
Class	between_default (const Class &l1, const Class &l2)
	These core global functions can be specialized by new Lie types for better performance.
template<class Class>
Vector	logmap_default (const Class &l0, const Class &lp)
	Log map centered at l0, s.t.
template<class Class>
Class	expmap_default (const Class &t, const Vector &d)
	Exponential map centered at l0, s.t.
template<typename T>
T	interpolate (const T &X, const T &Y, double t, typename MakeOptionalJacobian< T, T >::type Hx={}, typename MakeOptionalJacobian< T, T >::type Hy={}, typename MakeOptionalJacobian< T, double >::type Ht={})
	Linear interpolation between X and Y by coefficient t.
template<typename T>
	check_manifold_invariants (const T &a, const T &b, double tol=1e-9)
	Check invariants for Manifold type.
bool	assert_equal (const Matrix &A, const Matrix &B, double tol=1e-9)
	equals with an tolerance, prints out message if unequal
bool	assert_inequal (const Matrix &A, const Matrix &B, double tol=1e-9)
	inequals with an tolerance, prints out message if within tolerance
bool	assert_equal (const std::list< Matrix > &As, const std::list< Matrix > &Bs, double tol=1e-9)
	equals with an tolerance, prints out message if unequal
bool	linear_independent (const Matrix &A, const Matrix &B, double tol=1e-9)
	check whether the rows of two matrices are linear independent
bool	linear_dependent (const Matrix &A, const Matrix &B, double tol=1e-9)
	check whether the rows of two matrices are linear dependent
const Eigen::IOFormat &	matlabFormat ()
void	print (const Matrix &A, const std::string &s, std::ostream &stream)
	print without optional string, must specify cout yourself
void	print (const Matrix &A, const std::string &s="")
	print with optional string to cout
void	save (const Matrix &A, const std::string &s, const std::string &filename)
	save a matrix to file, which can be loaded by matlab
istream &	operator>> (std::istream &inputStream, Matrix &destinationMatrix)
	Read a matrix from an input stream, such as a file.
Matrix	diag (const std::vector< Matrix > &Hs)
	Create a matrix with submatrices along its diagonal.
pair< Matrix, Matrix >	qr (const Matrix &A)
	Householder QR factorization, Golub & Van Loan p 224, explicit version.
list< std::tuple< Vector, double, double > >	weighted_eliminate (Matrix &A, Vector &b, const Vector &sigmas)
	Imperative algorithm for in-place full elimination with weights and constraint handling.
void	householder_ (Matrix &A, size_t k, bool copy_vectors)
	Imperative version of Householder QR factorization, Golub & Van Loan p 224 version with Householder vectors below diagonal, as in GVL.
void	householder (Matrix &A, size_t k)
	Householder tranformation, zeros below diagonal.
Matrix	stack (const std::vector< Matrix > &blocks)
Matrix	collect (const std::vector< const Matrix * > &matrices, size_t m=0, size_t n=0)
	create a matrix by concatenating Given a set of matrices: A1, A2, A3... If all matrices have the same size, specifying single matrix dimensions will avoid the lookup of dimensions
Matrix	inverse_square_root (const Matrix &A)
	Use Cholesky to calculate inverse square root of a matrix.
void	svd (const Matrix &A, Matrix &U, Vector &S, Matrix &V)
	SVD computes economy SVD A=U*S*V'.
std::tuple< int, double, Vector >	DLT (const Matrix &A, double rank_tol=1e-9)
	Direct linear transform algorithm that calls svd to find a vector v that minimizes the algebraic error A*v.
Matrix	expm (const Matrix &A, size_t K=7)
	Numerical exponential map, naive approach, not industrial strength !
std::string	formatMatrixIndented (const std::string &label, const Matrix &matrix, bool makeVectorHorizontal)
void	inplace_QR (Matrix &A)
	QR factorization using Eigen's internal block QR algorithm.
template<class MATRIX>
bool	equal_with_abs_tol (const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
	equals with a tolerance
bool	operator== (const Matrix &A, const Matrix &B)
	equality is just equal_with_abs_tol 1e-9
bool	operator!= (const Matrix &A, const Matrix &B)
	inequality
Matrix3	skewSymmetric (double wx, double wy, double wz)
	skew symmetric matrix returns this: 0 -wz wy wz 0 -wx -wy wx 0
template<class Derived>
Matrix3	skewSymmetric (const Eigen::MatrixBase< Derived > &w)
template<class T>
T	BCH (const T &X, const T &Y)
	Three term approximation of the Baker-Campbell-Hausdorff formula In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y) it is not true that Z = X+Y.
template<class T>
T	expm (const Vector &x, int K=7)
	Exponential map given exponential coordinates class T needs a constructor from Matrix.
void	print (float v, const std::string &s="")
void	print (double v, const std::string &s="")
template<class T>
bool	equal (const T &obj1, const T &obj2, double tol)
	Call equal on the object.
template<class T>
bool	equal (const T &obj1, const T &obj2)
	Call equal without tolerance (use default tolerance).
template<class V>
bool	assert_equal (const V &expected, const V &actual, double tol=1e-9)
	This template works for any type with equals.
bool	assert_equal (const Key &expected, const Key &actual)
	Equals testing for basic types.
template<class V>
bool	assert_equal (const std::optional< V > &expected, const std::optional< V > &actual, double tol=1e-9)
	Comparisons for std.optional objects that checks whether objects exist before comparing their values.
template<class V>
bool	assert_equal (const V &expected, const std::optional< V > &actual, double tol=1e-9)
template<class V>
bool	assert_equal (const V &expected, const std::optional< std::reference_wrapper< const V > > &actual, double tol=1e-9)
template<class V1, class V2>
bool	assert_container_equal (const std::map< V1, V2 > &expected, const std::map< V1, V2 > &actual, double tol=1e-9)
	Function for comparing maps of testable->testable TODO: replace with more generalized version.
template<class V2>
bool	assert_container_equal (const std::map< size_t, V2 > &expected, const std::map< size_t, V2 > &actual, double tol=1e-9)
	Function for comparing maps of size_t->testable.
template<class V1, class V2>
bool	assert_container_equal (const std::vector< std::pair< V1, V2 > > &expected, const std::vector< std::pair< V1, V2 > > &actual, double tol=1e-9)
	Function for comparing vector of pairs (testable, testable).
template<class V>
bool	assert_container_equal (const V &expected, const V &actual, double tol=1e-9)
	General function for comparing containers of testable objects.
template<class V2>
bool	assert_container_equality (const std::map< size_t, V2 > &expected, const std::map< size_t, V2 > &actual)
	Function for comparing maps of size_t->testable Types are assumed to have operator ==.
template<class V>
bool	assert_container_equality (const V &expected, const V &actual)
	General function for comparing containers of objects with operator==.
bool	assert_equal (const std::string &expected, const std::string &actual)
	Compare strings for unit tests.
template<class V>
bool	assert_inequal (const V &expected, const V &actual, double tol=1e-9)
	Allow for testing inequality.
template<class V>
bool	assert_stdout_equal (const std::string &expected, const V &actual)
	Capture std out via cout stream and compare against string.
template<class V>
bool	assert_print_equal (const std::string &expected, const V &actual, const std::string &s="")
	Capture print function output and compare against string.
template<typename G>
void	testLieGroupDerivatives (TestResult &result_, const std::string &name_, const G &t1, const G &t2)
template<typename G>
void	testChartDerivatives (TestResult &result_, const std::string &name_, const G &t1, const G &t2)
void	tictoc_finishedIteration_ ()
void	tictoc_print_ ()
void	tictoc_printCsv_ (bool displayHeader=false)
void	tictoc_print2_ ()
void	tictoc_reset_ ()
std::string	demangle (const char *name)
	Pretty print Value type name.
bool	fpEqual (double a, double b, double tol, bool check_relative_also=true)
	Ensure we are not including a different version of Eigen in user code than while compiling gtsam, since it can lead to hard-to-understand runtime crashes.
void	print (const Vector &v, const std::string &s, std::ostream &stream)
	print without optional string, must specify cout yourself
void	print (const Vector &v, const std::string &s="")
	print with optional string to cout
void	save (const Vector &A, const std::string &s, const std::string &filename)
	save a vector to file, which can be loaded by matlab
bool	equal_with_abs_tol (const Vector &vec1, const Vector &vec2, double tol=1e-9)
	VecA == VecB up to tolerance.
bool	equal_with_abs_tol (const SubVector &vec1, const SubVector &vec2, double tol)
bool	assert_equal (const Vector &vec1, const Vector &vec2, double tol=1e-9)
	Same, prints if error.
bool	assert_inequal (const Vector &vec1, const Vector &vec2, double tol=1e-9)
	Not the same, prints if error.
bool	assert_equal (const SubVector &vec1, const SubVector &vec2, double tol=1e-9)
	Same, prints if error.
bool	assert_equal (const ConstSubVector &expected, const ConstSubVector &actual, double tol)
bool	linear_dependent (const Vector &vec1, const Vector &vec2, double tol=1e-9)
	check whether two vectors are linearly dependent
Vector	ediv_ (const Vector &a, const Vector &b)
	elementwise division, but 0/0 = 0, not inf
double	houseInPlace (Vector &x)
	beta = house(x) computes the HouseHolder vector in place
pair< double, Vector >	house (const Vector &x)
	house(x,j) computes HouseHolder vector v and scaling factor beta from x, such that the corresponding Householder reflection zeroes out all but x.
double	weightedPseudoinverse (const Vector &a, const Vector &weights, Vector &pseudo)
pair< Vector, double >	weightedPseudoinverse (const Vector &v, const Vector &weights)
	Weighted Householder solution vector, a.k.a., the pseudoinverse of the column NOTE: if any sigmas are zero (indicating a constraint) the pseudoinverse will be a selection vector, and the variance will be zero.
Vector	concatVectors (const std::list< Vector > &vs)
	concatenate Vectors
bool	equal (const Vector &vec1, const Vector &vec2, double tol)
	Override of equal in Lie.h.
bool	equal (const Vector &vec1, const Vector &vec2)
	Override of equal in Lie.h.
template<class V1, class V2>
double	dot (const V1 &a, const V2 &b)
	Dot product.
Matrix	kroneckerProductIdentity (size_t M, const Weights &w)
	Function for computing the kronecker product of the 1*N Weight vector w with the MxM identity matrix I efficiently.
template<typename L, typename Y>
DecisionTree< L, Y >	apply (const DecisionTree< L, Y > &f, const typename DecisionTree< L, Y >::Unary &op)
	free versions of apply
template<typename L, typename Y>
DecisionTree< L, Y >	apply (const DecisionTree< L, Y > &f, const typename DecisionTree< L, Y >::UnaryAssignment &op)
	Apply unary operator op with Assignment to DecisionTree f.
template<typename L, typename Y>
DecisionTree< L, Y >	apply (const DecisionTree< L, Y > &f, const DecisionTree< L, Y > &g, const typename DecisionTree< L, Y >::Binary &op)
	Apply binary operator op to DecisionTree f.
template<typename L, typename T1, typename T2>
std::pair< DecisionTree< L, T1 >, DecisionTree< L, T2 > >	unzip (const DecisionTree< L, std::pair< T1, T2 > > &input)
	unzip a DecisionTree with std::pair values.
std::pair< DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr >	EliminateForMPE (const DiscreteFactorGraph &factors, const Ordering &frontalKeys)
	Alternate elimination function for that creates non-normalized lookup tables.
std::pair< DiscreteConditional::shared_ptr, DiscreteFactor::shared_ptr >	EliminateDiscrete (const DiscreteFactorGraph &factors, const Ordering &frontalKeys)
	Main elimination function for DiscreteFactorGraph.
DiscreteKeys	operator& (const DiscreteKey &key1, const DiscreteKey &key2)
	Create a list from two keys.
std::ostream &	operator<< (std::ostream &os, const DiscreteValues &x)
string	markdown (const DiscreteValues &values, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DiscreteValues::Names &names={})
	Free version of markdown.
string	html (const DiscreteValues &values, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DiscreteValues::Names &names={})
	Free version of html.
std::vector< DiscreteValues >	cartesianProduct (const DiscreteKeys &keys)
	Free version of CartesianProduct.
ostream &	operator<< (ostream &os, const Signature::Row &row)
ostream &	operator<< (ostream &os, const Signature::Table &table)
ostream &	operator<< (std::ostream &os, const Signature &s)
Signature	operator| (const DiscreteKey &key, const DiscreteKey &parent)
	Helper function to create Signature objects example: Signature s = D | E;.
Signature	operator% (const DiscreteKey &key, const std::string &parent)
	Helper function to create Signature objects example: Signature s(D % "99/1");.
Signature	operator% (const DiscreteKey &key, const Signature::Table &parent)
	Helper function to create Signature objects, using table construction directly example: Signature s(D % table);.
std::ostream &	operator<< (std::ostream &os, const Cal3 &cal)
template<typename Cal, size_t Dim>
void	calibrateJacobians (const Cal &calibration, const Point2 &pn, OptionalJacobian< 2, Dim > Dcal={}, OptionalJacobian< 2, 2 > Dp={})
	Function which makes use of the Implicit Function Theorem to compute the Jacobians of calibrate using uncalibrate.
std::ostream &	operator<< (std::ostream &os, const Cal3_S2 &cal)
std::ostream &	operator<< (std::ostream &os, const Cal3_S2Stereo &cal)
std::ostream &	operator<< (std::ostream &os, const Cal3Bundler &cal)
std::ostream &	operator<< (std::ostream &os, const Cal3DS2 &cal)
std::ostream &	operator<< (std::ostream &os, const Cal3DS2_Base &cal)
std::ostream &	operator<< (std::ostream &os, const Cal3f &cal)
std::ostream &	operator<< (std::ostream &os, const Cal3Fisheye &cal)
std::ostream &	operator<< (std::ostream &os, const Cal3Unified &cal)
ostream &	operator<< (std::ostream &os, const EssentialMatrix &E)
istream &	operator>> (std::istream &is, EssentialMatrix &E)
Point2	EpipolarTransfer (const Matrix3 &Fca, const Point2 &pa, const Matrix3 &Fcb, const Point2 &pb)
	Transfer projections from cameras a and b to camera c.
std::ostream &	operator<< (std::ostream &os, const Gal3 &state)
Line3	transformTo (const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose={}, OptionalJacobian< 4, 4 > Dline={})
	Transform a line from world to camera frame.
double	norm1 (const Point1 &p, OptionalJacobian< 1, 1 > H={})
	L1 norm (absolute value) of a Point1, with optional Jacobian.
double	distance1 (const Point1 &p1, const Point1 &q, OptionalJacobian< 1, 1 > H1={}, OptionalJacobian< 1, 1 > H2={})
	Distance between two 1D points, with optional Jacobians.
ostream &	operator<< (std::ostream &os, const gtsam::Point1Pair &p)
	Stream insertion operator for Point1Pair.
double	norm2 (const Point2 &p, OptionalJacobian< 1, 2 > H={})
	Distance of the point from the origin, with Jacobian.
double	distance2 (const Point2 &p1, const Point2 &q, OptionalJacobian< 1, 2 > H1={}, OptionalJacobian< 1, 2 > H2={})
	distance between two points
std::optional< Point2 >	circleCircleIntersection (double R_d, double r_d, double tol)
list< Point2 >	circleCircleIntersection (Point2 c1, Point2 c2, std::optional< Point2 > fh)
list< Point2 >	circleCircleIntersection (Point2 c1, double r1, Point2 c2, double r2, double tol=1e-9)
	Intersect 2 circles.
Point2Pair	means (const std::vector< Point2Pair > &abPointPairs)
	Calculate the two means of a set of Point2 pairs.
ostream &	operator<< (ostream &os, const gtsam::Point2Pair &p)
Point2	operator* (double s, const Point2 &p)
	multiply with scalar
double	distance3 (const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1={}, OptionalJacobian< 1, 3 > H2={})
	distance between two points
double	norm3 (const Point3 &p, OptionalJacobian< 1, 3 > H={})
	Distance of the point from the origin, with Jacobian.
Point3	normalize (const Point3 &p, OptionalJacobian< 3, 3 > H={})
	normalize, with optional Jacobian
Point3	cross (const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H_p={}, OptionalJacobian< 3, 3 > H_q={})
	cross product
Point3	doubleCross (const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={})
	double cross product
double	dot (const Point3 &p, const Point3 &q, OptionalJacobian< 1, 3 > H_p={}, OptionalJacobian< 1, 3 > H_q={})
	dot product
Point3Pair	means (const std::vector< Point3Pair > &abPointPairs)
	Calculate the two means of a set of Point3 pairs.
ostream &	operator<< (ostream &os, const gtsam::Point3Pair &p)
template<class CONTAINER>
Point3	mean (const CONTAINER &points)
	mean
std::ostream &	operator<< (std::ostream &os, const Pose2 &pose)
std::ostream &	operator<< (std::ostream &os, const Pose3 &pose)
pair< Matrix3, Vector3 >	RQ (const Matrix3 &A, OptionalJacobian< 3, 9 > H={})
	[RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' such that A = R*Q = R*Qz'*Qy'*Qx'.
ostream &	operator<< (std::ostream &os, const Rot3 &R)
std::ostream &	operator<< (std::ostream &os, const Similarity2 &p)
std::ostream &	operator<< (std::ostream &os, const Similarity3 &p)
GTSAM_EXPORT Matrix3	topLeft (const SO4 &Q, OptionalJacobian< 9, 6 > H={})
	Project to top-left 3*3 matrix.
GTSAM_EXPORT Matrix43	stiefel (const SO4 &Q, OptionalJacobian< 12, 6 > H={})
	Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) -> \( S \in St(3,4) \).
ostream &	operator<< (std::ostream &os, const StereoPoint2 &p)
Vector4	triangulateHomogeneousDLT (const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &projection_matrices, const Point2Vector &measurements, double rank_tol=1e-9)
	DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312.
Vector4	triangulateHomogeneousDLT (const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &projection_matrices, const std::vector< Unit3 > &measurements, double rank_tol=1e-9)
	Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman).
Point3	triangulateLOST (const std::vector< Pose3 > &poses, const Point3Vector &calibratedMeasurements, const SharedIsotropic &measurementNoise, double rank_tol=1e-9)
	Triangulation using the LOST (Linear Optimal Sine Triangulation) algorithm proposed in https://arxiv.org/pdf/2205.12197.pdf by Sebastien Henry and John Christian.
Point3	triangulateDLT (const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &projection_matrices, const Point2Vector &measurements, double rank_tol=1e-9)
	DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312.
Point3	triangulateDLT (const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &projection_matrices, const std::vector< Unit3 > &measurements, double rank_tol=1e-9)
	overload of previous function to work with Unit3 (projected to canonical camera)
Point3	optimize (const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
	Optimize for triangulation.
template<class CALIBRATION>
std::pair< NonlinearFactorGraph, Values >	triangulationGraph (const std::vector< Pose3 > &poses, std::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, Key landmarkKey, const Point3 &initialEstimate, const SharedNoiseModel &model=noiseModel::Unit::Create(2))
	Create a factor graph with projection factors from poses and one calibration.
template<class CAMERA>
std::pair< NonlinearFactorGraph, Values >	triangulationGraph (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements, Key landmarkKey, const Point3 &initialEstimate, const SharedNoiseModel &model=nullptr)
	Create a factor graph with projection factors from pinhole cameras (each camera has a pose and calibration).
template<class CALIBRATION>
Point3	triangulateNonlinear (const std::vector< Pose3 > &poses, std::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, const Point3 &initialEstimate, const SharedNoiseModel &model=nullptr)
	Given an initial estimate , refine a point using measurements in several cameras.
template<class CAMERA>
Point3	triangulateNonlinear (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements, const Point3 &initialEstimate, const SharedNoiseModel &model=nullptr)
	Given an initial estimate , refine a point using measurements in several cameras.
template<class CAMERA>
std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > >	projectionMatricesFromCameras (const CameraSet< CAMERA > &cameras)
template<class CALIBRATION>
std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > >	projectionMatricesFromPoses (const std::vector< Pose3 > &poses, std::shared_ptr< CALIBRATION > sharedCal)
template<class CALIBRATION>
Cal3_S2	createPinholeCalibration (const CALIBRATION &cal)
	Create a pinhole calibration from a different Cal3 object, removing distortion.
template<class CALIBRATION, class MEASUREMENT>
MEASUREMENT	undistortMeasurementInternal (const CALIBRATION &cal, const MEASUREMENT &measurement, std::optional< Cal3_S2 > pinholeCal={})
	Internal undistortMeasurement to be used by undistortMeasurement and undistortMeasurements.
template<class CALIBRATION>
Point2Vector	undistortMeasurements (const CALIBRATION &cal, const Point2Vector &measurements)
	Remove distortion for measurements so as if the measurements came from a pinhole camera.
template<>
Point2Vector	undistortMeasurements (const Cal3_S2 &cal, const Point2Vector &measurements)
	Specialization for Cal3_S2 as it doesn't need to be undistorted.
template<class CAMERA>
CAMERA::MeasurementVector	undistortMeasurements (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements)
	Remove distortion for measurements so as if the measurements came from a pinhole camera.
template<class CAMERA = PinholeCamera<Cal3_S2>>
PinholeCamera< Cal3_S2 >::MeasurementVector	undistortMeasurements (const CameraSet< PinholeCamera< Cal3_S2 > > &cameras, const PinholeCamera< Cal3_S2 >::MeasurementVector &measurements)
	Specialize for Cal3_S2 to do nothing.
template<class CAMERA = SphericalCamera>
SphericalCamera::MeasurementVector	undistortMeasurements (const CameraSet< SphericalCamera > &cameras, const SphericalCamera::MeasurementVector &measurements)
	Specialize for SphericalCamera to do nothing.
template<class CALIBRATION>
Point3Vector	calibrateMeasurementsShared (const CALIBRATION &cal, const Point2Vector &measurements)
	Convert pixel measurements in image to homogeneous measurements in the image plane using shared camera intrinsics.
template<class CAMERA>
Point3Vector	calibrateMeasurements (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements)
	Convert pixel measurements in image to homogeneous measurements in the image plane using camera intrinsics of each measurement.
template<class CAMERA = SphericalCamera>
Point3Vector	calibrateMeasurements (const CameraSet< SphericalCamera > &cameras, const SphericalCamera::MeasurementVector &measurements)
	Specialize for SphericalCamera to do nothing.
template<class CALIBRATION>
Point3	triangulatePoint3 (const std::vector< Pose3 > &poses, std::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, double rank_tol=1e-9, bool optimize=false, const SharedNoiseModel &model=nullptr, const bool useLOST=false)
	Function to triangulate 3D landmark point from an arbitrary number of poses (at least 2) using the DLT.
template<class CAMERA>
Point3	triangulatePoint3 (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements, double rank_tol=1e-9, bool optimize=false, const SharedNoiseModel &model=nullptr, const bool useLOST=false)
	Function to triangulate 3D landmark point from an arbitrary number of poses (at least 2) using the DLT.
template<class CALIBRATION>
Point3	triangulatePoint3 (const CameraSet< PinholeCamera< CALIBRATION > > &cameras, const Point2Vector &measurements, double rank_tol=1e-9, bool optimize=false, const SharedNoiseModel &model=nullptr, const bool useLOST=false)
	Pinhole-specific version.
template<class CAMERA>
TriangulationResult	triangulateSafe (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
	triangulateSafe: extensive checking of the outcome
template<class CAMERA>
std::vector< TriangulationResult >	triangulateSafe (const CameraSet< CAMERA > &cameras, const std::vector< std::map< size_t, typename CAMERA::Measurement > > &tracks, const TriangulationParameters &params)
	Batch triangulation: triangulate multiple (possibly incomplete) tracks.
std::ostream &	operator<< (std::ostream &os, const Unit3 &pair)
Unit3	cross (const Unit3 &p, const Unit3 &q, OptionalJacobian< 2, 2 > H_p={}, OptionalJacobian< 2, 2 > H_q={})
	cross product Unit3 x Unit3
Point3	cross (const Unit3 &p, const Point3 &q, OptionalJacobian< 3, 2 > H_p={}, OptionalJacobian< 3, 3 > H_q={})
	cross product Unit3 x Point3
Point3	cross (const Point3 &p, const Unit3 &q, OptionalJacobian< 3, 3 > H_p={}, OptionalJacobian< 3, 2 > H_q={})
	cross product Point3 x Unit3
DecisionTreeFactor	DiscreteFactorFromErrors (const DiscreteKeys &discreteKeys, const AlgebraicDecisionTree< Key > &errors)
	Take negative log-values, shift them so that the minimum value is 0, and then exponentiate to create a TableFactor (not normalized yet!).
KeyVector	CollectKeys (const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys)
KeyVector	CollectKeys (const KeyVector &keys1, const KeyVector &keys2)
DiscreteKeys	CollectDiscreteKeys (const DiscreteKeys &key1, const DiscreteKeys &key2)
HybridFactor::Category	GetCategory (const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys)
GaussianConditional::shared_ptr	checkConditional (const GaussianFactor::shared_ptr &factor)
std::set< DiscreteKey >	DiscreteKeysAsSet (const DiscreteKeys &discreteKeys)
	Return the DiscreteKey vector as a set.
const Ordering	HybridOrdering (const HybridGaussianFactorGraph &graph)
	Return a Colamd constrained ordering where the discrete keys are eliminated after the continuous keys.
std::pair< HybridConditional::shared_ptr, std::shared_ptr< Factor > >	EliminateHybrid (const HybridGaussianFactorGraph &factors, const Ordering &keys)
	Main elimination function for HybridGaussianFactorGraph.
HybridGaussianProductFactor	operator+ (const HybridGaussianProductFactor &a, const HybridGaussianProductFactor &b)
std::istream &	operator>> (std::istream &is, GaussianFactorGraphValuePair &pair)
	Create a dummy overload of >> for GaussianFactorGraphValuePair so that HybridGaussianProductFactor compiles with the constructor DecisionTree(const std::vector<LabelC>& labelCs, const std::string& table).
template<class CLIQUE>
bool	check_sharedCliques (const std::pair< Key, typename BayesTree< CLIQUE >::sharedClique > &v1, const std::pair< Key, typename BayesTree< CLIQUE >::sharedClique > &v2)
GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const EdgeKey &key)
template<class KEY>
std::list< KEY >	predecessorMap2Keys (const PredecessorMap< KEY > &p_map)
	Generate a list of keys from a spanning tree represented by its predecessor map.
template<class G, class F, class KEY>
SDGraph< KEY >	toBoostGraph (const G &graph)
	Convert the factor graph to an SDGraph G = Graph type F = Factor type Key = Key type.
template<class G, class V, class KEY>
std::tuple< G, V, std::map< KEY, V > >	predecessorMap2Graph (const PredecessorMap< KEY > &p_map)
	Build takes a predecessor map, and builds a directed graph corresponding to the tree.
template<class G, class Factor, class POSE, class KEY>
std::shared_ptr< Values >	composePoses (const G &graph, const PredecessorMap< KEY > &tree, const POSE &rootPose)
	Compose the poses by following the chain specified by the spanning tree.
template<class G, class KEY, class FACTOR2>
PredecessorMap< KEY >	findMinimumSpanningTree (const G &g)
	find the minimum spanning tree using boost graph library
template<class G, class KEY, class FACTOR2>
void	split (const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2)
	Split the graph into two parts: one corresponds to the given spanning tree, and the other corresponds to the rest of the factors.
string	_defaultKeyFormatter (Key key)
void	PrintKey (Key key, const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
	Utility function to print one key with optional prefix.
string	_multirobotKeyFormatter (Key key)
template<class CONTAINER>
void	Print (const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
void	PrintKeyList (const KeyList &keys, const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
	Utility function to print sets of keys with optional prefix.
void	PrintKeyVector (const KeyVector &keys, const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
	Utility function to print sets of keys with optional prefix.
void	PrintKeySet (const KeySet &keys, const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter)
	Utility function to print sets of keys with optional prefix.
ostream &	operator<< (std::ostream &os, const key_formatter &m)
ostream &	operator<< (std::ostream &os, const StreamedKey &streamedKey)
GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const LabeledSymbol &symbol)
Key	mrsymbol (unsigned char c, unsigned char label, std::uint64_t j)
	Create a symbol key from a character, label and index, i.e. xA5.
unsigned char	mrsymbolChr (Key key)
	Return the character portion of a symbol key.
unsigned char	mrsymbolLabel (Key key)
	Return the label portion of a symbol key.
std::uint64_t	mrsymbolIndex (Key key)
	Return the index portion of a symbol key.
GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const Symbol &symbol)
Key	symbol (unsigned char c, std::uint64_t j)
	Create a symbol key from a character and index, i.e.
unsigned char	symbolChr (Key key)
	Return the character portion of a symbol key.
std::uint64_t	symbolIndex (Key key)
	Return the index portion of a symbol key.
template<class S, class V>
ConjugateGradientResult< V >	preconditionedConjugateGradientDetailed (const S &system, const V &initial, const ConjugateGradientParameters &parameters, bool collectResidualHistory=true)
	Solve a linear system with split-preconditioned conjugate gradients.
template<class S, class V>
V	preconditionedConjugateGradient (const S &system, const V &initial, const ConjugateGradientParameters &parameters)
	Solve a preconditioned linear system and return only the estimate.
Errors	createErrors (const VectorValues &V)
	Break V into pieces according to its start indices.
void	print (const Errors &e, const std::string &s="Errors")
	Print an Errors instance.
bool	equality (const Errors &actual, const Errors &expected, double tol)
Errors	operator+ (const Errors &a, const Errors &b)
	Addition.
Errors	operator- (const Errors &a, const Errors &b)
	Subtraction.
Errors	operator- (const Errors &a)
	Negation.
double	dot (const Errors &a, const Errors &b)
	Dot product.
void	axpy (double alpha, const Errors &x, Errors &y)
	BLAS level 2 style AXPY, y := alpha*x + y.
bool	hasConstraints (const GaussianFactorGraph &factors)
	Evaluates whether linear factors have any constrained noise models.
std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< HessianFactor > >	EliminateCholesky (const GaussianFactorGraph &factors, const Ordering &keys)
	Densely partially eliminate with Cholesky factorization.
std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< GaussianFactor > >	EliminatePreferCholesky (const GaussianFactorGraph &factors, const Ordering &keys)
	Densely partially eliminate with Cholesky factorization.
template<class S, class V, class E>
V	conjugateGradients (const S &Ab, V x, const ConjugateGradientParameters &parameters, bool steepest=false)
	Method of conjugate gradients (CG) template "System" class S needs gradient(S,v), e=S*v, v=S^e "Vector" class V needs dot(v,v), -v, v+v, s*v "Vector" class E needs dot(v,v).
Vector	steepestDescent (const System &Ab, const Vector &x, const ConjugateGradientParameters &parameters)
Vector	conjugateGradientDescent (const System &Ab, const Vector &x, const ConjugateGradientParameters &parameters)
	Method of conjugate gradients (CG), System version.
Vector	steepestDescent (const Matrix &A, const Vector &b, const Vector &x, const ConjugateGradientParameters &parameters)
	convenience calls using matrices, will create System class internally:
Vector	conjugateGradientDescent (const Matrix &A, const Vector &b, const Vector &x, const ConjugateGradientParameters &parameters)
	Method of conjugate gradients (CG), Matrix version.
VectorValues	steepestDescent (const GaussianFactorGraph &fg, const VectorValues &x, const ConjugateGradientParameters &parameters)
	Method of steepest gradients, Gaussian Factor Graph version.
VectorValues	conjugateGradientDescent (const GaussianFactorGraph &fg, const VectorValues &x, const ConjugateGradientParameters &parameters)
	Method of conjugate gradients (CG), Gaussian Factor Graph version.
GTSAM_EXPORT Vector	steepestDescent (const System &Ab, const Vector &x, const IterativeOptimizationParameters &parameters)
	Method of steepest gradients, System version.
ostream &	operator<< (std::ostream &os, const IterativeOptimizationParameters &p)
FastVector< VariableSlots::const_iterator >	orderedSlotsHelper (const Ordering &ordering, const VariableSlots &variableSlots)
std::pair< GaussianConditional::shared_ptr, JacobianFactor::shared_ptr >	EliminateQR (const GaussianFactorGraph &factors, const Ordering &keys)
	Multiply all factors and eliminate the given keys from the resulting factor using a QR variant that handles constraints (zero sigmas).
VectorValues	buildVectorValues (const Vector &vector, const KeyInfo &keyInfo)
	Split a flat vector into keyed blocks using KeyInfo offsets and dimensions.
std::ostream &	operator<< (std::ostream &os, const MultifrontalClique &clique)
std::ostream &	operator<< (std::ostream &os, const MultifrontalSolver &solver)
ostream &	operator<< (std::ostream &os, const PreconditionerParameters &p)
std::shared_ptr< Preconditioner >	createPreconditioner (const std::shared_ptr< PreconditionerParameters > params)
SparseEigen	sparseJacobianEigen (const GaussianFactorGraph &gfg, const Ordering &ordering)
	Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph.
SparseEigen	sparseJacobianEigen (const GaussianFactorGraph &gfg)
ostream &	operator<< (std::ostream &os, const Subgraph::Edge &edge)
ostream &	operator<< (std::ostream &os, const Subgraph &subgraph)
ostream &	operator<< (ostream &os, const SubgraphBuilderParameters &p)
GaussianFactorGraph	buildFactorSubgraph (const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone)
	Select the factors in a factor graph according to the subgraph.
std::pair< GaussianFactorGraph, GaussianFactorGraph >	splitFactorGraph (const GaussianFactorGraph &factorGraph, const Subgraph &subgraph)
	Split the graph into a subgraph and the remaining edges.
GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const VectorValues &v)
VectorValues	operator* (const double a, const VectorValues &c)
template<class PIM>
std::ostream &	operator<< (std::ostream &os, const CombinedImuFactorT< PIM > &f)
template GTSAM_EXPORT std::ostream &	operator<<<PreintegratedCombinedMeasurementsT< ManifoldPreintegration > > (std::ostream &os, const CombinedImuFactorT< PreintegratedCombinedMeasurementsT< ManifoldPreintegration > > &f)
template GTSAM_EXPORT std::ostream &	operator<<<PreintegratedCombinedMeasurementsT< TangentPreintegration > > (std::ostream &os, const CombinedImuFactorT< PreintegratedCombinedMeasurementsT< TangentPreintegration > > &f)
template GTSAM_EXPORT std::ostream &	operator<<< PreintegratedCombinedMeasurementsT< LieGroupPreintegration > > (std::ostream &os, const CombinedImuFactorT< PreintegratedCombinedMeasurementsT< LieGroupPreintegration > > &f)
template GTSAM_EXPORT std::ostream &	operator<<< PreintegratedCombinedMeasurementsG > (std::ostream &os, const CombinedImuFactorT< PreintegratedCombinedMeasurementsG > &f)
template<class PIM>
GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const CombinedImuFactorT< PIM > &f)
template<class PIM, class GRAVITY>
std::ostream &	operator<< (std::ostream &os, const CombinedImuFactorWithGravityT< PIM, GRAVITY > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const CombinedImuFactorWithGravityT< PreintegratedCombinedMeasurementsT< ManifoldPreintegration >, Unit3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const CombinedImuFactorWithGravityT< PreintegratedCombinedMeasurementsT< TangentPreintegration >, Unit3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const CombinedImuFactorWithGravityT< PreintegratedCombinedMeasurementsT< LieGroupPreintegration >, Unit3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const CombinedImuFactorWithGravityT< PreintegratedCombinedMeasurementsG, Unit3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const CombinedImuFactorWithGravityT< PreintegratedCombinedMeasurementsT< ManifoldPreintegration >, Point3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const CombinedImuFactorWithGravityT< PreintegratedCombinedMeasurementsT< TangentPreintegration >, Point3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const CombinedImuFactorWithGravityT< PreintegratedCombinedMeasurementsT< LieGroupPreintegration >, Point3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const CombinedImuFactorWithGravityT< PreintegratedCombinedMeasurementsG, Point3 > &f)
template<class PIM, class GRAVITY>
GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const CombinedImuFactorWithGravityT< PIM, GRAVITY > &f)
Rot3_	attitude (const NavState_ &X)
Point3_	position (const NavState_ &X)
Velocity3_	velocity (const NavState_ &X)
template<class PIM>
std::ostream &	operator<< (std::ostream &os, const ImuFactorT< PIM > &f)
template<class PIM>
std::ostream &	operator<< (std::ostream &os, const ImuFactor2T< PIM > &f)
template GTSAM_EXPORT std::ostream &	operator<<<PreintegratedImuMeasurementsT< ManifoldPreintegration > > (std::ostream &os, const ImuFactorT< PreintegratedImuMeasurementsT< ManifoldPreintegration > > &f)
template GTSAM_EXPORT std::ostream &	operator<<<PreintegratedImuMeasurementsT< TangentPreintegration > > (std::ostream &os, const ImuFactorT< PreintegratedImuMeasurementsT< TangentPreintegration > > &f)
template GTSAM_EXPORT std::ostream &	operator<<< PreintegratedImuMeasurementsT< LieGroupPreintegration > > (std::ostream &os, const ImuFactorT< PreintegratedImuMeasurementsT< LieGroupPreintegration > > &f)
template GTSAM_EXPORT std::ostream &	operator<<<PreintegratedImuMeasurementsG > (std::ostream &os, const ImuFactorT< PreintegratedImuMeasurementsG > &f)
template GTSAM_EXPORT std::ostream &	operator<<<PreintegratedImuMeasurementsT< ManifoldPreintegration > > (std::ostream &os, const ImuFactor2T< PreintegratedImuMeasurementsT< ManifoldPreintegration > > &f)
template GTSAM_EXPORT std::ostream &	operator<<<PreintegratedImuMeasurementsT< TangentPreintegration > > (std::ostream &os, const ImuFactor2T< PreintegratedImuMeasurementsT< TangentPreintegration > > &f)
template GTSAM_EXPORT std::ostream &	operator<<< PreintegratedImuMeasurementsT< LieGroupPreintegration > > (std::ostream &os, const ImuFactor2T< PreintegratedImuMeasurementsT< LieGroupPreintegration > > &f)
template GTSAM_EXPORT std::ostream &	operator<<<PreintegratedImuMeasurementsG > (std::ostream &os, const ImuFactor2T< PreintegratedImuMeasurementsG > &f)
template<class PIM>
GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const ImuFactorT< PIM > &f)
template<class PIM>
GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const ImuFactor2T< PIM > &f)
template<class PIM, class GRAVITY>
std::ostream &	operator<< (std::ostream &os, const ImuFactorWithGravityT< PIM, GRAVITY > &f)
template<class PIM, class GRAVITY>
std::ostream &	operator<< (std::ostream &os, const ImuFactor2WithGravityT< PIM, GRAVITY > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const ImuFactorWithGravityT< PreintegratedImuMeasurementsT< ManifoldPreintegration >, Unit3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const ImuFactorWithGravityT< PreintegratedImuMeasurementsT< TangentPreintegration >, Unit3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const ImuFactorWithGravityT< PreintegratedImuMeasurementsT< LieGroupPreintegration >, Unit3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const ImuFactorWithGravityT< PreintegratedImuMeasurementsG, Unit3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const ImuFactorWithGravityT< PreintegratedImuMeasurementsT< ManifoldPreintegration >, Point3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const ImuFactorWithGravityT< PreintegratedImuMeasurementsT< TangentPreintegration >, Point3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const ImuFactorWithGravityT< PreintegratedImuMeasurementsT< LieGroupPreintegration >, Point3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const ImuFactorWithGravityT< PreintegratedImuMeasurementsG, Point3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const ImuFactor2WithGravityT< PreintegratedImuMeasurementsT< ManifoldPreintegration >, Unit3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const ImuFactor2WithGravityT< PreintegratedImuMeasurementsT< TangentPreintegration >, Unit3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const ImuFactor2WithGravityT< PreintegratedImuMeasurementsT< LieGroupPreintegration >, Unit3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const ImuFactor2WithGravityT< PreintegratedImuMeasurementsG, Unit3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const ImuFactor2WithGravityT< PreintegratedImuMeasurementsT< ManifoldPreintegration >, Point3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const ImuFactor2WithGravityT< PreintegratedImuMeasurementsT< TangentPreintegration >, Point3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const ImuFactor2WithGravityT< PreintegratedImuMeasurementsT< LieGroupPreintegration >, Point3 > &f)
template GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const ImuFactor2WithGravityT< PreintegratedImuMeasurementsG, Point3 > &f)
template<class PIM, class GRAVITY>
GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const ImuFactorWithGravityT< PIM, GRAVITY > &f)
template<class PIM, class GRAVITY>
GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const ImuFactor2WithGravityT< PIM, GRAVITY > &f)
int	leggedFootBlockStart (size_t foot)
	Return the tangent-space start index of a foot block.
Vector3	extendedPoseContactPrediction (const ExtendedPose3d &state, size_t footColumn, OptionalMatrixType H={})
	Predict the IMU-frame contact vector for an ExtendedPose3 state.
std::ostream &	operator<< (std::ostream &os, const NavState &state)
Rot3	integrateSequentialRotations (const Vector &times, ConstMatrixView measuredOmegas, const Vector3 &biasHat=Vector3::Zero(), const Rot3 &body_R_sensor=Rot3())
	Integrate timed gyroscope samples using sequential trapezoidal rotation increments.
Rot3	integrateSingleSpeedConing (const Vector &times, ConstMatrixView measuredOmegas, const Vector3 &biasHat=Vector3::Zero(), const Rot3 &body_R_sensor=Rot3())
	Integrate timed gyroscope samples with a single-speed coning correction.
ostream &	operator<< (std::ostream &os, const PreintegrationBase &pim)
std::unique_ptr< internal::ExecutionTraceStorage[]>	allocAligned (size_t size)
template<typename T>
Expression< T >	operator* (const Expression< T > &expression1, const Expression< T > &expression2)
	Construct a product expression, assumes T::compose(T) -> T.
template<typename T>
std::vector< Expression< T > >	createUnknowns (size_t n, char c, size_t start)
	Construct an array of leaves.
template<typename T, typename A>
Expression< T >	linearExpression (const std::function< T(A)> &f, const Expression< A > &expression, const Eigen::Matrix< double, traits< T >::dimension, traits< A >::dimension > &dTdA)
	Create an expression out of a linear function f:T->A with (constant) Jacobian dTdA TODO(frank): create a more efficient version like ScalarMultiplyExpression.
template<typename T>
ScalarMultiplyExpression< T >	operator* (double s, const Expression< T > &e)
	Construct an expression that executes the scalar multiplication with an input expression The type T must be a vector space Example: Expression<Point2> a(0), b = 12 * a;.
template<typename T>
BinarySumExpression< T >	operator+ (const Expression< T > &e1, const Expression< T > &e2)
	Construct an expression that sums two input expressions of the same type T The type T must be a vector space Example: Expression<Point2> a(0), b(1), c = a + b;.
template<typename T>
BinarySumExpression< T >	operator- (const Expression< T > &e1, const Expression< T > &e2)
	Construct an expression that subtracts one expression from another.
template<typename T>
Expression< T >	between (const Expression< T > &t1, const Expression< T > &t2)
template<typename T>
Expression< T >	compose (const Expression< T > &t1, const Expression< T > &t2)
template<typename T>
Expression< T >	expmap (const Expression< T > &origin, const Expression< typename traits< T >::TangentVector > &tangent)
	Apply an exponential-map increment to a Lie-group expression.
template<typename T, typename std::enable_if< !std::is_same< T, double >::value >::type * = nullptr>
Expression< T >	operator* (const Expression< double > &scale, const Expression< T > &vector)
	Multiply a non-scalar expression by a scalar expression.
JacobianFactor	linearizeNumerically (const NoiseModelFactor &factor, const Values &values, double delta=1e-5)
	Linearize a nonlinear factor using numerical differentiation The benefit of this method is that it does not need to know what types are involved to evaluate the factor.
template<typename T, typename R, typename FUNC>
FunctorizedFactor< R, T >	MakeFunctorizedFactor (Key key, const R &z, const SharedNoiseModel &model, const FUNC func)
	Helper function to create a functorized factor.
template<typename T1, typename T2, typename R, typename FUNC>
FunctorizedFactor2< R, T1, T2 >	MakeFunctorizedFactor2 (Key key1, Key key2, const R &z, const SharedNoiseModel &model, const FUNC func)
	Helper function to create a functorized factor.
double	Chi2inv (const double alpha, const size_t dofs)
bool	isNullType (GncFactorType type)
bool	isNonNoiseModelType (GncFactorType type)
bool	needsWeightUpdate (GncFactorType type)
bool	hasNoise (GncFactorType type)
size_t	optimizeWildfire (const ISAM2Clique::shared_ptr &root, double threshold, const KeySet &replaced, VectorValues *delta)
	Optimize the BayesTree, starting from the root.
size_t	optimizeWildfireNonRecursive (const ISAM2Clique::shared_ptr &root, double threshold, const KeySet &keys, VectorValues *delta)
template<typename Gradient>
double	FletcherReeves (const Gradient &currentGradient, const Gradient &prevGradient)
	Fletcher-Reeves formula for computing β, the direction of steepest descent.
template<typename Gradient>
double	PolakRibiere (const Gradient &currentGradient, const Gradient &prevGradient)
	Polak-Ribiere formula for computing β, the direction of steepest descent.
template<typename Gradient>
double	HestenesStiefel (const Gradient &currentGradient, const Gradient &prevGradient, const Gradient &direction)
	The Hestenes-Stiefel formula for computing β, the direction of steepest descent.
template<typename Gradient>
double	DaiYuan (const Gradient &currentGradient, const Gradient &prevGradient, const Gradient &direction)
	The Dai-Yuan formula for computing β, the direction of steepest descent.
template<class S, class V, class W>
double	lineSearch (const S &system, const V currentValues, const W &gradient)
	Implement the golden-section line search algorithm.
template<class S, class V>
std::tuple< V, int >	nonlinearConjugateGradient (const S &system, const V &initial, const NonlinearOptimizerParams &params, const bool singleIteration, const DirectionMethod &directionMethod=DirectionMethod::PolakRibiere, const bool gradientDescent=false)
	Implement the nonlinear conjugate gradient method using the Polak-Ribiere formula suggested in http://en.wikipedia.org/wiki/Nonlinear_conjugate_gradient_method.
bool	checkConvergence (double relativeErrorThreshold, double absoluteErrorThreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity=NonlinearOptimizerParams::SILENT)
	Check whether the relative error decrease is less than relativeErrorThreshold, the absolute error decrease is less than absoluteErrorThreshold, or the error itself is less than errorThreshold.
GTSAM_EXPORT bool	checkConvergence (const NonlinearOptimizerParams &params, double currentError, double newError)
template<class PoseType, class FactorGraphType = NonlinearFactorGraph>
FactorGraphType	interpolateFactorGraph (const NonlinearFactorGraph &graph, const std::set< StateData > &estimated_states, const std::set< StateData > &interp_states, Vector q_psd_diag, bool fixed_noise=false)
	Utility functions for working with Factor Graphs containing interpolated variables.
template<class PoseType>
WnoaFactorGraph< PoseType >	interpolateWnoaFactorGraph (const NonlinearFactorGraph &graph, const std::set< StateData > &estimated_states, const std::set< StateData > &interp_states, Vector q_psd_diag, bool fixed_noise=false)
	WnoaFactorGraph specialization of interpolateFactorGraph for use in Python bindings.
template<class PoseType>
Values	updateInterpValues (const NonlinearFactorGraph &interp_graph, const Values &values, const std::set< StateData > &estim_states, const std::set< StateData > &interp_states, const Vector q_psd_diag, std::shared_ptr< InterpCovarianceMap > covarianceMapOut=nullptr)
	Update a Values with interpolated pose and velocity entries.
template<class PoseType>
std::pair< Values, InterpCovarianceMap >	updateInterpValuesWithCovariance (const NonlinearFactorGraph &interp_graph, const Values &values, const std::set< StateData > &estim_states, const std::set< StateData > &interp_states, const Vector q_psd_diag)
	Update Values with interpolated states and return covariances.
Eigen::Vector4d	fetzer_d (const Eigen::Vector3d &ai, const Eigen::Vector3d &bi, const Eigen::Vector3d &aj, const Eigen::Vector3d &bj, int u, int v)
	Build one 4-vector of bilinear coefficients from two pairs of 3-vectors, picking rows (u, v) — the 2x2 minor of (ai/bi) against (aj/bj) used by the Fetzer focal-length residual.
std::array< Eigen::Vector4d, 3 >	fetzer_ds (const Eigen::Matrix3d &G)
	Precompute the Fetzer coefficient vectors from G = K_j^T F K_i.
Rot3	openGLFixedRotation ()
Pose3	openGL2gtsam (const Rot3 &R, double tx, double ty, double tz)
	This function converts an openGL camera pose to an GTSAM camera pose.
Pose3	gtsam2openGL (const Rot3 &R, double tx, double ty, double tz)
	This function converts a GTSAM camera pose to an openGL camera pose.
Pose3	gtsam2openGL (const Pose3 &PoseGTSAM)
	This function converts a GTSAM camera pose to an openGL camera pose.
bool	writeBAL (const std::string &filename, const SfmData &data)
	This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure.
SfmData	readBal (const std::string &filename)
	This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data as a SfmData structure.
bool	writeBALfromValues (const std::string &filename, const SfmData &data, const Values &values)
	This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure and a value structure (measurements are the same as the SfM input data, while camera poses and values are read from Values).
Values	initialCamerasEstimate (const SfmData &db)
	This function creates initial values for cameras from db.
Values	initialCamerasAndPointsEstimate (const SfmData &db)
	This function creates initial values for cameras and points from db.
std::string	findExampleDataFile (const std::string &name)
	Find the full path to an example dataset distributed with gtsam.
std::string	createRewrittenFileName (const std::string &name)
	Creates a temporary file name that needs to be ignored in .gitingnore for checking read-write oprations.
template<typename T>
std::map< size_t, T >	parseToMap (const std::string &filename, Parser< std::pair< size_t, T > > parse, size_t maxIndex)
std::optional< IndexedPose >	parseVertexPose (std::istream &is, const std::string &tag)
	Parse TORO/G2O vertex "id x y yaw".
template<>
GTSAM_EXPORT std::map< size_t, Pose2 >	parseVariables< Pose2 > (const std::string &filename, size_t maxIndex)
std::optional< IndexedLandmark >	parseVertexLandmark (std::istream &is, const std::string &tag)
	Parse G2O landmark vertex "id x y".
template<>
GTSAM_EXPORT std::map< size_t, Point2 >	parseVariables< Point2 > (const std::string &filename, size_t maxIndex)
std::optional< IndexedEdge >	parseEdge (std::istream &is, const std::string &tag)
	Parse TORO/G2O edge "id1 id2 x y yaw".
std::shared_ptr< Sampler >	createSampler (const SharedNoiseModel &model)
template<>
GTSAM_EXPORT std::vector< BinaryMeasurement< Pose2 > >	parseMeasurements (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
template<>
GTSAM_EXPORT std::vector< BinaryMeasurement< Rot2 > >	parseMeasurements (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
template<>
GTSAM_EXPORT std::vector< BetweenFactor< Pose2 >::shared_ptr >	parseFactors< Pose2 > (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
GraphAndValues	load2D (const std::string &filename, SharedNoiseModel model=SharedNoiseModel(), size_t maxIndex=0, bool addNoise=false, bool smart=true, NoiseFormat noiseFormat=NoiseFormatAUTO, KernelFunctionType kernelFunctionType=KernelFunctionTypeNONE)
	Load TORO/G2O style graph files.
GraphAndValues	load2D (std::pair< std::string, SharedNoiseModel > dataset, size_t maxIndex=0, bool addNoise=false, bool smart=true, NoiseFormat noiseFormat=NoiseFormatAUTO, KernelFunctionType kernelFunctionType=KernelFunctionTypeNONE)
	Load TORO 2D Graph.
GraphAndValues	load2D_robust (const std::string &filename, const noiseModel::Base::shared_ptr &model, size_t maxIndex)
void	save2D (const NonlinearFactorGraph &graph, const Values &config, const noiseModel::Diagonal::shared_ptr model, const std::string &filename)
	save 2d graph
GraphAndValues	readG2o (const std::string &g2oFile, const bool is3D=false, KernelFunctionType kernelFunctionType=KernelFunctionTypeNONE)
	This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initial guess in a Values structure.
void	writeG2o (const NonlinearFactorGraph &graph, const Values &estimate, const std::string &filename)
	This function writes a g2o file from NonlinearFactorGraph and a Values structure.
std::istream &	operator>> (std::istream &is, Quaternion &q)
std::optional< std::pair< size_t, Pose3 > >	parseParameterSE3Offset (std::istream &is, const std::string &tag)
std::istream &	operator>> (std::istream &is, Rot3 &R)
std::optional< std::pair< size_t, Pose3 > >	parseVertexPose3 (std::istream &is, const std::string &tag)
template<>
GTSAM_EXPORT std::map< size_t, Pose3 >	parseVariables< Pose3 > (const std::string &filename, size_t maxIndex)
std::optional< std::pair< size_t, Point3 > >	parseVertexPoint3 (std::istream &is, const std::string &tag)
template<>
GTSAM_EXPORT std::map< size_t, Point3 >	parseVariables< Point3 > (const std::string &filename, size_t maxIndex)
std::istream &	operator>> (std::istream &is, Matrix6 &m)
template<>
GTSAM_EXPORT std::vector< BinaryMeasurement< Pose3 > >	parseMeasurements (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
template<>
GTSAM_EXPORT std::vector< BinaryMeasurement< Rot3 > >	parseMeasurements (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
template<>
GTSAM_EXPORT std::vector< BetweenFactor< Pose3 >::shared_ptr >	parseFactors< Pose3 > (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
GraphAndValues	load3D (const std::string &filename)
	Load a TORO/G2O-style 3D graph.
BetweenFactorPose2s	parse2DFactors (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
BetweenFactorPose3s	parse3DFactors (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
template<typename T>
GTSAM_EXPORT std::map< size_t, T >	parseVariables (const std::string &filename, size_t maxIndex=0)
	Parse variables in a line-based text format (like g2o) into a map.
template<typename T>
GTSAM_EXPORT std::vector< BinaryMeasurement< T > >	parseMeasurements (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model=nullptr, size_t maxIndex=0)
	Parse binary measurements in a line-based text format (like g2o) into a vector.
template<typename T>
GTSAM_EXPORT std::vector< typename BetweenFactor< T >::shared_ptr >	parseFactors (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model=nullptr, size_t maxIndex=0)
	Parse BetweenFactors in a line-based text format (like g2o) into a vector of shared pointers.
Point2_	transformTo (const Pose2_ &x, const Point2_ &p)
Double_	range (const Point2_ &p, const Point2_ &q)
Point3_	transformTo (const Pose3_ &x, const Point3_ &p)
Point3_	transformFrom (const Pose3_ &x, const Point3_ &p)
Line3_	transformTo (const Pose3_ &wTc, const Line3_ &wL)
Pose3_	transformPoseTo (const Pose3_ &p, const Pose3_ &q)
Pose3_	interpolateRt (const Pose3_ &p, const Pose3_ &q, const Double_ &t)
Point3_	normalize (const Point3_ &a)
Point3_	cross (const Point3_ &a, const Point3_ &b)
Double_	dot (const Point3_ &a, const Point3_ &b)
Rot3_	rotation (const Pose3_ &pose)
Point3_	translation (const Pose3_ &pose)
Point3_	rotate (const Rot3_ &x, const Point3_ &p)
Point3_	point3 (const Unit3_ &v)
Unit3_	rotate (const Rot3_ &x, const Unit3_ &p)
Point3_	unrotate (const Rot3_ &x, const Point3_ &p)
Unit3_	unrotate (const Rot3_ &x, const Unit3_ &p)
Double_	distance (const OrientedPlane3_ &p)
Unit3_	normal (const OrientedPlane3_ &p)
Point2_	project (const Point3_ &p_cam)
	Expression version of PinholeBase::Project.
Point2_	project (const Unit3_ &p_cam)
template<class CAMERA, class POINT>
Point2_	project2 (const Expression< CAMERA > &camera_, const Expression< POINT > &p_)
template<class CALIBRATION, class POINT>
Point2_	project3 (const Pose3_ &x, const Expression< POINT > &p, const Expression< CALIBRATION > &K)
template<class CALIBRATION>
Point2_	uncalibrate (const Expression< CALIBRATION > &K, const Point2_ &xy_hat)
template<class CALIBRATION>
Pose3_	getPose (const Expression< PinholeCamera< CALIBRATION > > &cam)
template<typename T>
gtsam::Expression< typename gtsam::traits< T >::TangentVector >	logmap (const gtsam::Expression< T > &x1, const gtsam::Expression< T > &x2)
	logmap
template<typename T>
Expression< T >	interpolate (const Expression< T > &p, const Expression< T > &q, const Expression< double > &t)
template<class T>
Values	fastSync (const NonlinearFactorGraph &graph, Ordering::OrderingType orderingType=Ordering::METIS)
	Initialize a synchronization graph using FAST-Sync.
template<class T>
Values	fastSync (const NonlinearFactorGraph &graph, const Ordering &ordering)
	Initialize a synchronization graph using a caller-supplied ordering.
SharedNoiseModel	ConvertNoiseModel (const SharedNoiseModel &model, size_t n, bool defaultToUnit=true)
	Convert a possibly robust noise model to an isotropic model for Frobenius factors.
template<class T, size_t Dim>
SharedNoiseModel	ConvertModel (const SharedNoiseModel &model)
	Ensure a noise model has the correct dimension for a given type.
template<class T, class ALLOC>
T	FindKarcherMeanImpl (const std::vector< T, ALLOC > &rotations)
template<class T>
T	FindKarcherMean (const std::vector< T > &rotations)
template<class T>
T	FindKarcherMean (const std::vector< T, Eigen::aligned_allocator< T > > &rotations)
	Optimize for the Karcher mean, minimizing the geodesic distance to each of the given Lie groups elements, by constructing a factor graph out of simple PriorFactors.
template<class T>
T	FindKarcherMean (std::initializer_list< T > &&rotations)
	FindKarcherMean version from initializer list.
template<class T>
std::enable_if< traits< T >::IsLieGroup, T >::type	FindKarcherMean (const std::vector< T, Eigen::aligned_allocator< T > > &elements)
	Optimize for the Karcher mean, minimizing the geodesic distance to each of the given Lie groups elements, by constructing a factor graph out of simple PriorFactors.
template<class T>
std::enable_if< traits< T >::IsLieGroup, T >::type	FindKarcherMean (std::initializer_list< T > &&elements)
	FindKarcherMean version from initializer list.
template<class T, class P>
P	transform_point (const T &trans, const P &global, OptionalMatrixType Dtrans, OptionalMatrixType Dglobal)
	Transform function that must be specialized specific domains.
std::pair< std::shared_ptr< SymbolicConditional >, std::shared_ptr< SymbolicFactor > >	EliminateSymbolic (const SymbolicFactorGraph &factors, const Ordering &keys)
	Dense elimination function for symbolic factors.
std::pair< Pose2, bool >	moveWithBounce (const Pose2 &cur_pose, double step_size, const std::vector< SimWall2D > walls, Sampler &angle_drift, Sampler &reflect_noise, const Rot2 &bias=Rot2())
	Calculates the next pose in a trajectory constrained by walls, with noise on angular drift and reflection noise.
void	synchronize (ConcurrentFilter &filter, ConcurrentSmoother &smoother)
std::string	serializeGraph (const NonlinearFactorGraph &graph)
NonlinearFactorGraph::shared_ptr	deserializeGraph (const std::string &serialized_graph)
std::string	serializeGraphXML (const NonlinearFactorGraph &graph, const std::string &name="graph")
NonlinearFactorGraph::shared_ptr	deserializeGraphXML (const std::string &serialized_graph, const std::string &name="graph")
std::string	serializeValues (const Values &values)
Values::shared_ptr	deserializeValues (const std::string &serialized_values)
std::string	serializeValuesXML (const Values &values, const std::string &name="values")
Values::shared_ptr	deserializeValuesXML (const std::string &serialized_values, const std::string &name="values")
bool	serializeGraphToFile (const NonlinearFactorGraph &graph, const std::string &fname)
bool	serializeGraphToXMLFile (const NonlinearFactorGraph &graph, const std::string &fname, const std::string &name="graph")
bool	serializeValuesToFile (const Values &values, const std::string &fname)
bool	serializeValuesToXMLFile (const Values &values, const std::string &fname, const std::string &name="values")
NonlinearFactorGraph::shared_ptr	deserializeGraphFromFile (const std::string &fname)
NonlinearFactorGraph::shared_ptr	deserializeGraphFromXMLFile (const std::string &fname, const std::string &name="graph")
Values::shared_ptr	deserializeValuesFromFile (const std::string &fname)
Values::shared_ptr	deserializeValuesFromXMLFile (const std::string &fname, const std::string &name="values")
Key	X (std::uint64_t j)
First-Order Derivative Helpers
template<class X, int N = traits<X>::dimension, class F, internal::EnableIfScalarInvocable< F, X > = 0>
Eigen::Matrix< double, N, 1 >	numericalGradient (F &&h, const X &x, double delta=1e-5)
	Numerically compute gradient of scalar function.
template<class X, int N = traits<X>::dimension>
Eigen::Matrix< double, N, 1 >	numericalGradient (double(&h)(const X &), const X &x, double delta=1e-5)
	Raw-function overload for numericalGradient.
template<class Y = internal::DeducedOutput, class X, int N = traits<X>::dimension, class F, internal::EnableIfInvocable< F, X > = 0>
internal::MatrixMN< traits< internal::OutputType< Y, F, X > >::dimension, N >::type	numericalDerivative11 (F &&h, const X &x, double delta=1e-5)
	New-style numerical derivatives using manifold_traits.
template<class Y, class X, int N = traits<X>::dimension>
internal::MatrixMN< traits< Y >::dimension, N >::type	numericalDerivative11 (Y(&h)(const X &), const X &x, double delta=1e-5)
	Raw-function overload for numericalDerivative11.
template<class Y = internal::DeducedOutput, class X1, class X2, int N = traits<X1>::dimension, class F, internal::EnableIfInvocable< F, X1, X2 > = 0>
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2 > >::dimension, N >::type	numericalDerivative21 (F &&h, const X1 &x1, const X2 &x2, double delta=1e-5)
	Compute numerical derivative in argument 1 of binary function.
template<class Y, class X1, class X2, int N = traits<X1>::dimension>
internal::MatrixMN< traits< Y >::dimension, N >::type	numericalDerivative21 (Y(&h)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
	Raw-function overload for numericalDerivative21.
template<class Y = internal::DeducedOutput, class X1, class X2, int N = traits<X2>::dimension, class F, internal::EnableIfInvocable< F, X1, X2 > = 0>
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2 > >::dimension, N >::type	numericalDerivative22 (F &&h, const X1 &x1, const X2 &x2, double delta=1e-5)
	Compute numerical derivative in argument 2 of binary function.
template<class Y, class X1, class X2, int N = traits<X2>::dimension>
internal::MatrixMN< traits< Y >::dimension, N >::type	numericalDerivative22 (Y(&h)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
	Raw-function overload for numericalDerivative22.
template<class Y = internal::DeducedOutput, class X1, class X2, class X3, int N = traits<X1>::dimension, class F, internal::EnableIfInvocable< F, X1, X2, X3 > = 0>
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3 > >::dimension, N >::type	numericalDerivative31 (F &&h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Compute numerical derivative in argument 1 of ternary function.
template<class Y, class X1, class X2, class X3, int N = traits<X1>::dimension>
internal::MatrixMN< traits< Y >::dimension, N >::type	numericalDerivative31 (Y(&h)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Raw-function overload for numericalDerivative31.
template<class Y = internal::DeducedOutput, class X1, class X2, class X3, int N = traits<X2>::dimension, class F, internal::EnableIfInvocable< F, X1, X2, X3 > = 0>
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3 > >::dimension, N >::type	numericalDerivative32 (F &&h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Compute numerical derivative in argument 2 of ternary function.
template<class Y, class X1, class X2, class X3, int N = traits<X2>::dimension>
internal::MatrixMN< traits< Y >::dimension, N >::type	numericalDerivative32 (Y(&h)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Raw-function overload for numericalDerivative32.
template<class Y = internal::DeducedOutput, class X1, class X2, class X3, int N = traits<X3>::dimension, class F, internal::EnableIfInvocable< F, X1, X2, X3 > = 0>
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3 > >::dimension, N >::type	numericalDerivative33 (F &&h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Compute numerical derivative in argument 3 of ternary function.
template<class Y, class X1, class X2, class X3, int N = traits<X3>::dimension>
internal::MatrixMN< traits< Y >::dimension, N >::type	numericalDerivative33 (Y(&h)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Raw-function overload for numericalDerivative33.
template<class Y = internal::DeducedOutput, class X1, class X2, class X3, class X4, int N = traits<X1>::dimension, class F, internal::EnableIfInvocable< F, X1, X2, X3, X4 > = 0>
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4 > >::dimension, N >::type	numericalDerivative41 (F &&h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
	Compute numerical derivative in argument 1 of 4-argument function.
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X1>::dimension>
internal::MatrixMN< traits< Y >::dimension, N >::type	numericalDerivative41 (Y(&h)(const X1 &, const X2 &, const X3 &, const X4 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
	Raw-function overload for numericalDerivative41.
template<class Y = internal::DeducedOutput, class X1, class X2, class X3, class X4, int N = traits<X2>::dimension, class F, internal::EnableIfInvocable< F, X1, X2, X3, X4 > = 0>
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4 > >::dimension, N >::type	numericalDerivative42 (F &&h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
	Compute numerical derivative in argument 2 of 4-argument function.
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X2>::dimension>
internal::MatrixMN< traits< Y >::dimension, N >::type	numericalDerivative42 (Y(&h)(const X1 &, const X2 &, const X3 &, const X4 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
	Raw-function overload for numericalDerivative42.
template<class Y = internal::DeducedOutput, class X1, class X2, class X3, class X4, int N = traits<X3>::dimension, class F, internal::EnableIfInvocable< F, X1, X2, X3, X4 > = 0>
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4 > >::dimension, N >::type	numericalDerivative43 (F &&h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
	Compute numerical derivative in argument 3 of 4-argument function.
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X3>::dimension>
internal::MatrixMN< traits< Y >::dimension, N >::type	numericalDerivative43 (Y(&h)(const X1 &, const X2 &, const X3 &, const X4 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
	Raw-function overload for numericalDerivative43.
template<class Y = internal::DeducedOutput, class X1, class X2, class X3, class X4, int N = traits<X4>::dimension, class F, internal::EnableIfInvocable< F, X1, X2, X3, X4 > = 0>
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4 > >::dimension, N >::type	numericalDerivative44 (F &&h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
	Compute numerical derivative in argument 4 of 4-argument function.
template<class Y, class X1, class X2, class X3, class X4, int N = traits<X4>::dimension>
internal::MatrixMN< traits< Y >::dimension, N >::type	numericalDerivative44 (Y(&h)(const X1 &, const X2 &, const X3 &, const X4 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5)
	Raw-function overload for numericalDerivative44.
template<class Y = internal::DeducedOutput, class X1, class X2, class X3, class X4, class X5, int N = traits<X1>::dimension, class F, internal::EnableIfInvocable< F, X1, X2, X3, X4, X5 > = 0>
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5 > >::dimension, N >::type	numericalDerivative51 (F &&h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
	Compute numerical derivative in argument 1 of 5-argument function.
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X1>::dimension>
internal::MatrixMN< traits< Y >::dimension, N >::type	numericalDerivative51 (Y(&h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
	Raw-function overload for numericalDerivative51.
template<class Y = internal::DeducedOutput, class X1, class X2, class X3, class X4, class X5, int N = traits<X2>::dimension, class F, internal::EnableIfInvocable< F, X1, X2, X3, X4, X5 > = 0>
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5 > >::dimension, N >::type	numericalDerivative52 (F &&h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
	Compute numerical derivative in argument 2 of 5-argument function.
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X2>::dimension>
internal::MatrixMN< traits< Y >::dimension, N >::type	numericalDerivative52 (Y(&h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
	Raw-function overload for numericalDerivative52.
template<class Y = internal::DeducedOutput, class X1, class X2, class X3, class X4, class X5, int N = traits<X3>::dimension, class F, internal::EnableIfInvocable< F, X1, X2, X3, X4, X5 > = 0>
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5 > >::dimension, N >::type	numericalDerivative53 (F &&h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
	Compute numerical derivative in argument 3 of 5-argument function.
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X3>::dimension>
internal::MatrixMN< traits< Y >::dimension, N >::type	numericalDerivative53 (Y(&h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
	Raw-function overload for numericalDerivative53.
template<class Y = internal::DeducedOutput, class X1, class X2, class X3, class X4, class X5, int N = traits<X4>::dimension, class F, internal::EnableIfInvocable< F, X1, X2, X3, X4, X5 > = 0>
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5 > >::dimension, N >::type	numericalDerivative54 (F &&h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
	Compute numerical derivative in argument 4 of 5-argument function.
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X4>::dimension>
internal::MatrixMN< traits< Y >::dimension, N >::type	numericalDerivative54 (Y(&h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
	Raw-function overload for numericalDerivative54.
template<class Y = internal::DeducedOutput, class X1, class X2, class X3, class X4, class X5, int N = traits<X5>::dimension, class F, internal::EnableIfInvocable< F, X1, X2, X3, X4, X5 > = 0>
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5 > >::dimension, N >::type	numericalDerivative55 (F &&h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
	Compute numerical derivative in argument 5 of 5-argument function.
template<class Y, class X1, class X2, class X3, class X4, class X5, int N = traits<X5>::dimension>
internal::MatrixMN< traits< Y >::dimension, N >::type	numericalDerivative55 (Y(&h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5)
	Raw-function overload for numericalDerivative55.
template<class Y = internal::DeducedOutput, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X1>::dimension, class F, internal::EnableIfInvocable< F, X1, X2, X3, X4, X5, X6 > = 0>
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5, X6 > >::dimension, N >::type	numericalDerivative61 (F &&h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
	Compute numerical derivative in argument 1 of 6-argument function.
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X1>::dimension>
internal::MatrixMN< traits< Y >::dimension, N >::type	numericalDerivative61 (Y(&h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
	Raw-function overload for numericalDerivative61.
template<class Y = internal::DeducedOutput, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X2>::dimension, class F, internal::EnableIfInvocable< F, X1, X2, X3, X4, X5, X6 > = 0>
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5, X6 > >::dimension, N >::type	numericalDerivative62 (F &&h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
	Compute numerical derivative in argument 2 of 6-argument function.
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X2>::dimension>
internal::MatrixMN< traits< Y >::dimension, N >::type	numericalDerivative62 (Y(&h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
	Raw-function overload for numericalDerivative62.
template<class Y = internal::DeducedOutput, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X3>::dimension, class F, internal::EnableIfInvocable< F, X1, X2, X3, X4, X5, X6 > = 0>
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5, X6 > >::dimension, N >::type	numericalDerivative63 (F &&h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
	Compute numerical derivative in argument 3 of 6-argument function.
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X3>::dimension>
internal::MatrixMN< traits< Y >::dimension, N >::type	numericalDerivative63 (Y(&h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
	Raw-function overload for numericalDerivative63.
template<class Y = internal::DeducedOutput, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X4>::dimension, class F, internal::EnableIfInvocable< F, X1, X2, X3, X4, X5, X6 > = 0>
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5, X6 > >::dimension, N >::type	numericalDerivative64 (F &&h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
	Compute numerical derivative in argument 4 of 6-argument function.
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X4>::dimension>
internal::MatrixMN< traits< Y >::dimension, N >::type	numericalDerivative64 (Y(&h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
	Raw-function overload for numericalDerivative64.
template<class Y = internal::DeducedOutput, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X5>::dimension, class F, internal::EnableIfInvocable< F, X1, X2, X3, X4, X5, X6 > = 0>
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5, X6 > >::dimension, N >::type	numericalDerivative65 (F &&h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
	Compute numerical derivative in argument 5 of 6-argument function.
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X5>::dimension>
internal::MatrixMN< traits< Y >::dimension, N >::type	numericalDerivative65 (Y(&h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
	Raw-function overload for numericalDerivative65.
template<class Y = internal::DeducedOutput, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X6>::dimension, class F, internal::EnableIfInvocable< F, X1, X2, X3, X4, X5, X6 > = 0>
internal::MatrixMN< traits< internal::OutputType< Y, F, X1, X2, X3, X4, X5, X6 > >::dimension, N >::type	numericalDerivative66 (F &&h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
	Compute numerical derivative in argument 6 of 6-argument function.
template<class Y, class X1, class X2, class X3, class X4, class X5, class X6, int N = traits<X6>::dimension>
internal::MatrixMN< traits< Y >::dimension, N >::type	numericalDerivative66 (Y(&h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5)
	Raw-function overload for numericalDerivative66.
Hessian Helpers
template<class X, int N = traits<X>::dimension, class F, internal::EnableIfScalarInvocable< F, X > = 0>
internal::MatrixMN< N, N >::type	numericalHessian (F &&f, const X &x, double delta=1e-5)
	Compute numerical Hessian matrix.
template<class X, int N = traits<X>::dimension>
internal::MatrixMN< N, N >::type	numericalHessian (double(&f)(const X &), const X &x, double delta=1e-5)
	Raw-function overload for numericalHessian.
template<class X1, class X2, int N1 = traits<X1>::dimension, int N2 = traits<X2>::dimension, class F, internal::EnableIfScalarInvocable< F, X1, X2 > = 0>
internal::MatrixMN< N1, N2 >::type	numericalHessian212 (F &&f, const X1 &x1, const X2 &x2, double delta=1e-5)
	Mixed Hessian with respect to argument 1 then argument 2.
template<class X1, class X2, int N1 = traits<X1>::dimension, int N2 = traits<X2>::dimension>
internal::MatrixMN< N1, N2 >::type	numericalHessian212 (double(&f)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
	Raw-function overload for numericalHessian212.
template<class X1, class X2, int N1 = traits<X1>::dimension, class F, internal::EnableIfScalarInvocable< F, X1, X2 > = 0>
internal::MatrixMN< N1, N1 >::type	numericalHessian211 (F &&f, const X1 &x1, const X2 &x2, double delta=1e-5)
	Hessian with respect to argument 1 of a binary scalar function.
template<class X1, class X2, int N1 = traits<X1>::dimension>
internal::MatrixMN< N1, N1 >::type	numericalHessian211 (double(&f)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
	Raw-function overload for numericalHessian211.
template<class X1, class X2, int N2 = traits<X2>::dimension, class F, internal::EnableIfScalarInvocable< F, X1, X2 > = 0>
internal::MatrixMN< N2, N2 >::type	numericalHessian222 (F &&f, const X1 &x1, const X2 &x2, double delta=1e-5)
	Hessian with respect to argument 2 of a binary scalar function.
template<class X1, class X2, int N2 = traits<X2>::dimension>
internal::MatrixMN< N2, N2 >::type	numericalHessian222 (double(&f)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
	Raw-function overload for numericalHessian222.
template<class X1, class X2, class X3, int N1 = traits<X1>::dimension, class F, internal::EnableIfScalarInvocable< F, X1, X2, X3 > = 0>
internal::MatrixMN< N1, N1 >::type	numericalHessian311 (F &&f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Numerical Hessian for ternary functions.
template<class X1, class X2, class X3, int N1 = traits<X1>::dimension>
internal::MatrixMN< N1, N1 >::type	numericalHessian311 (double(&f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Raw-function overload for numericalHessian311.
template<class X1, class X2, class X3, int N2 = traits<X2>::dimension, class F, internal::EnableIfScalarInvocable< F, X1, X2, X3 > = 0>
internal::MatrixMN< N2, N2 >::type	numericalHessian322 (F &&f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Hessian with respect to argument 2 of a ternary scalar function.
template<class X1, class X2, class X3, int N2 = traits<X2>::dimension>
internal::MatrixMN< N2, N2 >::type	numericalHessian322 (double(&f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Raw-function overload for numericalHessian322.
template<class X1, class X2, class X3, int N3 = traits<X3>::dimension, class F, internal::EnableIfScalarInvocable< F, X1, X2, X3 > = 0>
internal::MatrixMN< N3, N3 >::type	numericalHessian333 (F &&f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Hessian with respect to argument 3 of a ternary scalar function.
template<class X1, class X2, class X3, int N3 = traits<X3>::dimension>
internal::MatrixMN< N3, N3 >::type	numericalHessian333 (double(&f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Raw-function overload for numericalHessian333.
template<class X1, class X2, class X3, int N1 = traits<X1>::dimension, int N2 = traits<X2>::dimension, class F, internal::EnableIfScalarInvocable< F, X1, X2, X3 > = 0>
internal::MatrixMN< N1, N2 >::type	numericalHessian312 (F &&f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Mixed Hessian with respect to arguments 1 and 2 of a ternary scalar function.
template<class X1, class X2, class X3, int N1 = traits<X1>::dimension, int N3 = traits<X3>::dimension, class F, internal::EnableIfScalarInvocable< F, X1, X2, X3 > = 0>
internal::MatrixMN< N1, N3 >::type	numericalHessian313 (F &&f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Mixed Hessian with respect to arguments 1 and 3 of a ternary scalar function.
template<class X1, class X2, class X3, int N2 = traits<X2>::dimension, int N3 = traits<X3>::dimension, class F, internal::EnableIfScalarInvocable< F, X1, X2, X3 > = 0>
internal::MatrixMN< N2, N3 >::type	numericalHessian323 (F &&f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Mixed Hessian with respect to arguments 2 and 3 of a ternary scalar function.
template<class X1, class X2, class X3, int N1 = traits<X1>::dimension, int N2 = traits<X2>::dimension>
internal::MatrixMN< N1, N2 >::type	numericalHessian312 (double(&f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Raw-function overload for numericalHessian312.
template<class X1, class X2, class X3, int N1 = traits<X1>::dimension, int N3 = traits<X3>::dimension>
internal::MatrixMN< N1, N3 >::type	numericalHessian313 (double(&f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Raw-function overload for numericalHessian313.
template<class X1, class X2, class X3, int N2 = traits<X2>::dimension, int N3 = traits<X3>::dimension>
internal::MatrixMN< N2, N3 >::type	numericalHessian323 (double(&f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Raw-function overload for numericalHessian323.
utility functions
VectorValues	buildVectorValues (const Vector &v, const Ordering &ordering, const std::map< Key, size_t > &dimensions)
	Convert a flat vector to keyed blocks using an explicit ordering.

Variables
GTSAM_EXPORT FastMap< std::string, ValueWithDefault< bool, false > >	debugFlags
template class GTSAM_EXPORT	Conditional< DecisionTreeFactor, DiscreteConditional >
template<class CAMERA>
const int	CameraSet< CAMERA >::D
template<class CAMERA>
const int	CameraSet< CAMERA >::ZDim
template class GTSAM_EXPORT	ExtendedPose3< 1 >
template class GTSAM_EXPORT	ExtendedPose3< 2 >
template class GTSAM_EXPORT	ExtendedPose3< 3 >
template class GTSAM_EXPORT	ExtendedPose3< 4 >
template class GTSAM_EXPORT	ExtendedPose3< 6 >
KeyFormatter	DefaultKeyFormatter = &_defaultKeyFormatter
	Assign default key formatter.
KeyFormatter	MultiRobotKeyFormatter = &_multirobotKeyFormatter
	A KeyFormatter that will check for LabeledSymbol keys, as well as Symbol and plain integer keys.
template class GTSAM_EXPORT	AHRSFactorT< PreintegratedAhrsMeasurements >
template class GTSAM_EXPORT	PreintegratedCombinedMeasurementsT< ManifoldPreintegration >
template class GTSAM_EXPORT	PreintegratedCombinedMeasurementsT< TangentPreintegration >
template class GTSAM_EXPORT	PreintegratedCombinedMeasurementsT< LieGroupPreintegration >
template class GTSAM_EXPORT	PreintegratedCombinedMeasurementsT< GalileanPreintegration >
template class GTSAM_EXPORT	CombinedImuFactorT< PreintegratedCombinedMeasurementsT< ManifoldPreintegration > >
template class GTSAM_EXPORT	CombinedImuFactorT< PreintegratedCombinedMeasurementsT< TangentPreintegration > >
template class GTSAM_EXPORT	CombinedImuFactorT< PreintegratedCombinedMeasurementsT< LieGroupPreintegration > >
template class GTSAM_EXPORT	CombinedImuFactorT< PreintegratedCombinedMeasurementsG >
template class GTSAM_EXPORT	CombinedImuFactorWithGravityT< PreintegratedCombinedMeasurementsT< ManifoldPreintegration >, Unit3 >
template class GTSAM_EXPORT	CombinedImuFactorWithGravityT< PreintegratedCombinedMeasurementsT< TangentPreintegration >, Unit3 >
template class GTSAM_EXPORT	CombinedImuFactorWithGravityT< PreintegratedCombinedMeasurementsT< LieGroupPreintegration >, Unit3 >
template class GTSAM_EXPORT	CombinedImuFactorWithGravityT< PreintegratedCombinedMeasurementsG, Unit3 >
template class GTSAM_EXPORT	CombinedImuFactorWithGravityT< PreintegratedCombinedMeasurementsT< ManifoldPreintegration >, Point3 >
template class GTSAM_EXPORT	CombinedImuFactorWithGravityT< PreintegratedCombinedMeasurementsT< TangentPreintegration >, Point3 >
template class GTSAM_EXPORT	CombinedImuFactorWithGravityT< PreintegratedCombinedMeasurementsT< LieGroupPreintegration >, Point3 >
template class GTSAM_EXPORT	CombinedImuFactorWithGravityT< PreintegratedCombinedMeasurementsG, Point3 >
template class GTSAM_EXPORT	PreintegratedImuMeasurementsT< ManifoldPreintegration >
template class GTSAM_EXPORT	PreintegratedImuMeasurementsT< TangentPreintegration >
template class GTSAM_EXPORT	PreintegratedImuMeasurementsT< LieGroupPreintegration >
template class GTSAM_EXPORT	PreintegratedImuMeasurementsT< GalileanPreintegration >
template class GTSAM_EXPORT	ImuFactorT< PreintegratedImuMeasurementsT< ManifoldPreintegration > >
template class GTSAM_EXPORT	ImuFactorT< PreintegratedImuMeasurementsT< TangentPreintegration > >
template class GTSAM_EXPORT	ImuFactorT< PreintegratedImuMeasurementsT< LieGroupPreintegration > >
template class GTSAM_EXPORT	ImuFactorT< PreintegratedImuMeasurementsG >
template class GTSAM_EXPORT	ImuFactor2T< PreintegratedImuMeasurementsT< ManifoldPreintegration > >
template class GTSAM_EXPORT	ImuFactor2T< PreintegratedImuMeasurementsT< TangentPreintegration > >
template class GTSAM_EXPORT	ImuFactor2T< PreintegratedImuMeasurementsT< LieGroupPreintegration > >
template class GTSAM_EXPORT	ImuFactor2T< PreintegratedImuMeasurementsG >
template class GTSAM_EXPORT	ImuFactorWithGravityT< PreintegratedImuMeasurementsT< ManifoldPreintegration >, Unit3 >
template class GTSAM_EXPORT	ImuFactorWithGravityT< PreintegratedImuMeasurementsT< TangentPreintegration >, Unit3 >
template class GTSAM_EXPORT	ImuFactorWithGravityT< PreintegratedImuMeasurementsT< LieGroupPreintegration >, Unit3 >
template class GTSAM_EXPORT	ImuFactorWithGravityT< PreintegratedImuMeasurementsG, Unit3 >
template class GTSAM_EXPORT	ImuFactorWithGravityT< PreintegratedImuMeasurementsT< ManifoldPreintegration >, Point3 >
template class GTSAM_EXPORT	ImuFactorWithGravityT< PreintegratedImuMeasurementsT< TangentPreintegration >, Point3 >
template class GTSAM_EXPORT	ImuFactorWithGravityT< PreintegratedImuMeasurementsT< LieGroupPreintegration >, Point3 >
template class GTSAM_EXPORT	ImuFactorWithGravityT< PreintegratedImuMeasurementsG, Point3 >
template class GTSAM_EXPORT	ImuFactor2WithGravityT< PreintegratedImuMeasurementsT< ManifoldPreintegration >, Unit3 >
template class GTSAM_EXPORT	ImuFactor2WithGravityT< PreintegratedImuMeasurementsT< TangentPreintegration >, Unit3 >
template class GTSAM_EXPORT	ImuFactor2WithGravityT< PreintegratedImuMeasurementsT< LieGroupPreintegration >, Unit3 >
template class GTSAM_EXPORT	ImuFactor2WithGravityT< PreintegratedImuMeasurementsG, Unit3 >
template class GTSAM_EXPORT	ImuFactor2WithGravityT< PreintegratedImuMeasurementsT< ManifoldPreintegration >, Point3 >
template class GTSAM_EXPORT	ImuFactor2WithGravityT< PreintegratedImuMeasurementsT< TangentPreintegration >, Point3 >
template class GTSAM_EXPORT	ImuFactor2WithGravityT< PreintegratedImuMeasurementsT< LieGroupPreintegration >, Point3 >
template class GTSAM_EXPORT	ImuFactor2WithGravityT< PreintegratedImuMeasurementsG, Point3 >
const double	logSqrt2PI = log(std::sqrt(2.0 * M_PI))
	constant needed below
template class GTSAM_EXPORT	WnoaFactorGraph< Point1 >
template class GTSAM_EXPORT	WnoaFactorGraph< Point2 >
template class GTSAM_EXPORT	WnoaFactorGraph< Point3 >
template class GTSAM_EXPORT	WnoaFactorGraph< Pose2 >
template class GTSAM_EXPORT	WnoaFactorGraph< Pose3 >
template<class CAMERA>
const int	RegularImplicitSchurFactor< CAMERA >::D
template<class CAMERA>
const int	RegularImplicitSchurFactor< CAMERA >::ZDim
template<class CAMERA>
const int	SmartFactorBase< CAMERA >::Dim
template<class CAMERA>
const int	SmartFactorBase< CAMERA >::ZDim
const size_t	max_it = 100000
constexpr double	C_LIGHT
	Speed of light in a vacuum (m/s).
constexpr double	C_LIGHT
	Speed of light in a vacuum (m/s).
constexpr double	OMGE
	WGS-84 Earth rotation rate (rad/s).
constexpr double	C_LIGHT
	Speed of light in a vacuum (m/s).

Detailed Description

Global functions in a separate testing namespace.

triangulationFactor.h

Interface and implementations for GraduationScheduler These are used to define the sequence of problems solved by RISAM.

Factor that implements a graduated robust cost function.

DiscreteSearch.cpp.

This file supports creating continuous functions f(x;p) as a linear combination of Basis functions such as the Fourier basis on SO(2) or a set of Chebyshev polynomials on [-1,1].

Matrix is a typedef in the gtsam namespace TODO: make a version to work with matlab wrapping we use the default < double,col_major,unbounded_array<double> >.

These should not be used outside of tests, as they are just remappings of the original functions. We use these to avoid needing to do too much std::bind magic or writing a bunch of separate proxy functions.

Don't expect all classes to work for all of these functions.

In the expression f(x;p) the variable x is the continuous argument at which the function is evaluated, and p are the parameters which are coefficients of the different basis functions, e.g. p = [4; 3; 2] => 4 + 3x + 2x^2 for a polynomial. However, different parameterizations are also possible.

The Basis class below defines a number of functors that can be used to evaluate f(x;p) at a given x, and these functors also calculate the Jacobian of f(x;p) with respect to the parameters p. This is actually the most important calculation, as it will allow GTSAM to optimize over the parameters p.

This functionality is implemented using the CRTP or "Curiously recurring template pattern" C++ idiom, which is a meta-programming technique in which the derived class is passed as a template argument to Basis<DERIVED>. The DERIVED class is assumed to satisfy a C++ concept, i.e., we expect it to define the following types and methods:

• type Parameters: the parameters p in f(x;p)
• CalculateWeights(size_t N, double x, double a=default, double b=default)
• DerivativeWeights(size_t N, double x, double a=default, double b=default)

where Weights is an N*1 row vector which defines the basis values for the polynomial at the specified point x.

E.g. A Fourier series would give the following:

• CalculateWeights -> For N=5, the values for the bases: [1, cos(x), sin(x), cos(2x), sin(2x)]
• DerivativeWeights -> For N=5, these are: [0, -sin(x), cos(x), -2sin(2x), 2cos(x)]

Note that for a pseudo-spectral basis (as in Chebyshev2), the weights are instead the values for the Barycentric interpolation formula, since the values at the polynomial points (e.g. Chebyshev points) define the bases.

Date

January, 2025

Author

Frank Dellaert

A Graduated factor overrides a portion of the NoiseModelFactor interface to implement a graduated robust cost function for the factor.

Author

Dan McGann

Date

Mar 2022

The scheduler defines these problems using the the control parameter $\mu$ for a specific Graduated Robust Loss Function.

Author

Dan McGann

Date

Mar 2022

March 2, 2014

Author

Frank Dellaert

Typedef Documentation

CarrierPhaseBase

using gtsam::CarrierPhaseBase = GnssMeasurementBase

Base class storing common members for carrier phase factors.

Aliased to the shared GnssMeasurementBase. In this alias the measurement_ field stores the carrier phase measurement in meters (= lambda * phi_cycles).

Clock biases are in seconds (same convention as PseudorangeFactorArm). Ambiguity is in meters (= lambda * N). To recover the integer ambiguity N, divide by the wavelength: N = ambiguity_meters / lambda

CombinedImuFactorWithGravityDirection

using gtsam::CombinedImuFactorWithGravityDirection

Initial value:

 
CombinedImuFactorWithGravityT<PreintegratedCombinedMeasurements, Unit3>

CombinedImuFactor variant optimizing the gravity direction (Unit3) with a fixed, known magnitude; see ImuFactorWithGravityDirection.

CombinedImuFactorWithGravityVector

using gtsam::CombinedImuFactorWithGravityVector

Initial value:

 
CombinedImuFactorWithGravityT<PreintegratedCombinedMeasurements, Point3>

CombinedImuFactor variant optimizing the free gravity vector (Point3); see ImuFactorWithGravityVector.

ConstMatrixView

using gtsam::ConstMatrixView

Initial value:

 
Eigen::Ref<const Matrix, 0, Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>>

Dynamic-stride const Matrix view for accepting NumPy arrays without copies.

FactorErrorPredicate

using gtsam::FactorErrorPredicate

Initial value:

 
std::function<bool(const Factor*, double, std::size_t)>

Predicate used to select factor errors for graph diagnostics.

FactorIndices

typedef FastVector< FactorIndex > gtsam::FactorIndices

Define collection types:

Define collection type:

FrobeniusErrorVector

template<class T>

using gtsam::FrobeniusErrorVector

Initial value:

Eigen::Matrix<
   double, T::LieAlgebra::RowsAtCompileTime * T::LieAlgebra::RowsAtCompileTime,
   1>

GalileanCombinedImuFactor

using gtsam::GalileanCombinedImuFactor

Initial value:

 
CombinedImuFactorT<PreintegratedCombinedMeasurementsG>

Six-way Combined IMU factor using Galilean preintegration.

GraphAndValues

using gtsam::GraphAndValues

Initial value:

 
std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr>

Return type for load functions, which return a graph and initial values.

For landmarks, the gtsam::Symbol L(index) is used to insert into the Values. Bearing-range measurements also refer to landmarks with L(index).

ImuFactor2WithGravityDirection

using gtsam::ImuFactor2WithGravityDirection

Initial value:

 
ImuFactor2WithGravityT<PreintegratedImuMeasurements, Unit3>

ImuFactor2 variant with the gravity direction as an optimized Unit3 variable and a fixed, known magnitude; see ImuFactorWithGravityDirection.

ImuFactor2WithGravityVector

using gtsam::ImuFactor2WithGravityVector

Initial value:

 
ImuFactor2WithGravityT<PreintegratedImuMeasurements, Point3>

ImuFactor2 variant with the gravity vector as a free Point3 variable, ie.

direction and magnitude both optimized; see ImuFactorWithGravityVector.

ImuFactorWithGravityDirection

using gtsam::ImuFactorWithGravityDirection

Initial value:

 
ImuFactorWithGravityT<PreintegratedImuMeasurements, Unit3>

ImuFactor variant with the gravity direction as an optimized Unit3 variable and a fixed, known magnitude (given at construction, defaulting to the norm of the gravity vector in the preintegration params).

This is the preferred parametrization when the magnitude is known, eg. on Earth where standard gravity is accurate to ~0.3% everywhere; it removes the magnitude degree of freedom by construction. See eg. Nemiroff, Chen and Lopez, "Joint On-Manifold Gravity and Accelerometer Intrinsics Estimation for Inertially Aligned Mapping", 2023.

ImuFactorWithGravityVector

using gtsam::ImuFactorWithGravityVector

Initial value:

 
ImuFactorWithGravityT<PreintegratedImuMeasurements, Point3>

ImuFactor variant with the gravity vector as a free Point3 variable, ie.

direction and magnitude both optimized, following Lupton and Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions", TRO 2012. If the magnitude is approximately known, add a VectorNormFactor<3> on the gravity variable once (Lupton's magnitude pseudo-observation); if it is known exactly, prefer ImuFactorWithGravityDirection.

NoiseModelFactorN

template<class... ValueTypes>

using gtsam::NoiseModelFactorN = NoiseModelFactorT<Vector, ValueTypes...>

Noise model factor with N value types and dynamic-sized error vector.

Template Parameters

ValueTypes

NonlinearFactorValuePair

using gtsam::NonlinearFactorValuePair

Initial value:

 
std::pair<NoiseModelFactor::shared_ptr, double>

Alias for a NoiseModelFactor shared pointer and double scalar pair.

OptionalMatrixType

using gtsam::OptionalMatrixType = Matrix*

This typedef will be used everywhere boost::optional<Matrix&> reference was used previously.

This is used to indicate that the Jacobian is optional. In the future we will change this to OptionalJacobian

OptionalMatrixVecType

using gtsam::OptionalMatrixVecType = std::vector<Matrix>*

The OptionalMatrixVecType is a pointer to a vector of matrices.

It will be used in situations where a vector of matrices is optional, like in unwhitenedError.

Parser

template<typename T>

using gtsam::Parser

Initial value:

 
std::function<std::optional<T>(std::istream &is, const std::string &tag)>

PinholePoseCal3_S2

using gtsam::PinholePoseCal3_S2 = gtsam::PinholePose<gtsam::Cal3_S2>

Convenient aliases for Pinhole camera classes with different calibrations.

Also needed as forward declarations in the wrapper.

PreintegratedCombinedMeasurementsG

using gtsam::PreintegratedCombinedMeasurementsG

Initial value:

 
PreintegratedCombinedMeasurementsT<GalileanPreintegration>

Galilean preintegration with combined IMU and bias covariance propagation.

PreintegratedImuMeasurementsG

using gtsam::PreintegratedImuMeasurementsG

Initial value:

 
PreintegratedImuMeasurementsT<GalileanPreintegration>

Galilean preintegration with generic PIM covariance propagation.

PriorityScheduler

template<typename Y>

using gtsam::PriorityScheduler = Scheduler<Y, detail::PrioritySchedulerPolicy>

Thread pool scheduler that prioritizes tasks by numeric priority.

• Lower numeric values are executed first.
• Tasks are executed by worker threads created at construction.
• schedule returns a std::future<Y> for the task result.
• Per-thread queues reduce contention; workers steal from peers when idle.
• External submissions are round-robin distributed across worker queues.
• A condition variable parks workers when no work is available.

Template Parameters

Y	Result type returned by tasks. Use void for no return value.

PseudorangeBase

using gtsam::PseudorangeBase = GnssMeasurementBase

Base class storing common members for GNSS-related pseudorange factors.

Aliased to the shared GnssMeasurementBase so that pseudorange and carrier phase factors share a single representation. In this alias the measurement_ field stores the pseudorange measurement in meters.

SearchNodeQueue

using gtsam::SearchNodeQueue

Initial value:

std::priority_queue<SearchNode, std::vector<SearchNode>,
                                           SearchNode::Compare>

SharedNoiseModel

typedef noiseModel::Base::shared_ptr gtsam::SharedNoiseModel

Aliases.

Deliberately not in noiseModel namespace.

SparseEigen

typedef Eigen::SparseMatrix<double, Eigen::ColMajor, int> gtsam::SparseEigen

Eigen-format sparse matrix.

Note: ColMajor is ~20% faster since InnerIndices must be sorted

StateDataInterval

using gtsam::StateDataInterval

Initial value:

 
std::pair<std::optional<StateData>, std::optional<StateData>>

Type alias for a pair of optional bordering states for an interpolated state.

The pair holds the left and right bordering StateData for an interpolated state. Either entry may be empty (std::nullopt) to indicate a missing border; in that case extrapolation is used instead of interpolation.

TaskScheduler

template<typename Y>

using gtsam::TaskScheduler = Scheduler<Y, detail::TaskSchedulerPolicy>

Thread pool scheduler that executes tasks without priority ordering.

• Tasks are executed in an efficient deque-based order (LIFO locally).
• Per-thread queues reduce contention; workers steal from peers when idle.
• External submissions are round-robin distributed across worker queues.
• A condition variable parks workers when no work is available.

Template Parameters

Y	Result type returned by tasks. Use void for no return value.

Enumeration Type Documentation

ConjugateGradientTerminationReason

enum class gtsam::ConjugateGradientTerminationReason

strong

Reason a conjugate-gradient solve stopped.

Enumerator
kConverged	The requested residual tolerance was reached.
kMaxIterations	The iteration limit was reached first.
kNumericalBreakdown	The recurrence encountered invalid numerics.

GncFactorType

enum class gtsam::GncFactorType

strong

Enum to classify factor types in GNC optimization.

Enumerator
Normal	Normal case.
Inlier	Factor is a known inlier.
Outlier	Factor is a known outlier.
NonNoiseModel	Factor does not have a noise model.
NullPointer	Factor pointer is null.

GncScheduler

enum class gtsam::GncScheduler

strong

Choice of GNC scheduling strategy.

SuperLinear reference https://openaccess.thecvf.com/content/CVPR2023/papers/Peng_On_the_Convergence_of_IRLS_and_Its_Variants_in_Outlier-Robust_CVPR_2023_paper.pdf

ImuFactorErrorMode

enum class gtsam::ImuFactorErrorMode

strong

Error chart used by IMU factors.

Enumerator
Legacy	Historical backend-dependent error chart.
ComponentWise	Use the component-wise NavState error for every backend.
Logmap	Use the SE_2(3) NavState Logmap for every backend.

LinearizationMode

enum gtsam::LinearizationMode

SmartFactorParams: parameters and (linearization/degeneracy) modes for SmartProjection and SmartStereoProjection factors.

Linearization mode: what factor to linearize to

NoiseFormat

enum gtsam::NoiseFormat

Indicates how noise parameters are stored in file.

Enumerator
NoiseFormatG2O	Information matrix I11, I12, I13, I22, I23, I33.
NoiseFormatTORO	Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr.
NoiseFormatGRAPH	default: toro-style order, but covariance matrix !
NoiseFormatCOV	Covariance matrix C11, C12, C13, C22, C23, C33.
NoiseFormatAUTO	Try to guess covariance matrix layout.

SfmEliminationMode

enum class gtsam::SfmEliminationMode

strong

Determines whether an SFM optimizer solves the joint or reduced system.

Enumerator
Full	Send the complete system to the selected solver.
Schur	Eliminate landmark variables before solving the reduced system.

Function Documentation

apply()

template<typename L, typename Y>

DecisionTree< L, Y > gtsam::apply	(	const DecisionTree< L, Y > &	f,
		const typename DecisionTree< L, Y >::Unary &	op )

free versions of apply

Apply unary operator op to DecisionTree f.

assert_equal() [1/3]

template<class V>

bool gtsam::assert_equal	(	const std::optional< V > &	expected,
		const std::optional< V > &	actual,
		double	tol = 1e-9 )

Comparisons for std.optional objects that checks whether objects exist before comparing their values.

First version allows for both to be std::nullopt, but the second, with expected given rather than optional

Concept requirement: V is testable

assert_equal() [2/3]

GTSAM_EXPORT bool gtsam::assert_equal	(	const SubVector &	vec1,
		const SubVector &	vec2,
		double	tol = 1e-9 )

Same, prints if error.

Parameters

vec1	Vector
vec2	Vector
tol	1e-9

Returns

bool

assert_equal() [3/3]

GTSAM_EXPORT bool gtsam::assert_equal	(	const Vector &	vec1,
		const Vector &	vec2,
		double	tol = 1e-9 )

Same, prints if error.

Parameters

vec1	Vector
vec2	Vector
tol	1e-9

Returns

bool

assert_inequal()

GTSAM_EXPORT bool gtsam::assert_inequal	(	const Vector &	vec1,
		const Vector &	vec2,
		double	tol = 1e-9 )

Not the same, prints if error.

Parameters

vec1	Vector
vec2	Vector
tol	1e-9

Returns

bool

assert_print_equal()

template<class V>

bool gtsam::assert_print_equal	(	const std::string &	expected,
		const V &	actual,
		const std::string &	s = "" )

Capture print function output and compare against string.

Parameters

s	Optional string to pass to the print() method.

BCH()

template<class T>

T gtsam::BCH	(	const T &	X,
		const T &	Y )

Three term approximation of the Baker-Campbell-Hausdorff formula In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y) it is not true that Z = X+Y.

Instead, Z can be calculated using the BCH formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24 http://en.wikipedia.org/wiki/Baker-Campbell-Hausdorff_formula AGC: bracket() only appears in Rot3 tests, should this be used elsewhere?

between_default()

template<class Class>

Class gtsam::between_default ( const Class & l1, const Class & l2 )

inline

These core global functions can be specialized by new Lie types for better performance.

Compute l0 s.t. l2=l1*l0

buildVectorValues() [1/2]

GTSAM_EXPORT VectorValues gtsam::buildVectorValues	(	const Vector &	v,
		const Ordering &	ordering,
		const std::map< Key, size_t > &	dimensions )

Convert a flat vector to keyed blocks using an explicit ordering.

Parameters

v	Flat vector whose blocks follow ordering.
ordering	Key order used to slice v.
dimensions	Scalar dimension of each ordered key.

Returns

Keyed vector blocks copied from v.

Exceptions

std::invalid_argument

if an ordered key has no dimension entry.

buildVectorValues() [2/2]

GTSAM_EXPORT VectorValues gtsam::buildVectorValues	(	const Vector &	vector,
		const KeyInfo &	keyInfo )

Split a flat vector into keyed blocks using KeyInfo offsets and dimensions.

Exceptions

std::invalid_argument

if the flat dimension differs from keyInfo.numCols().

calibrateJacobians()

template<typename Cal, size_t Dim>

void gtsam::calibrateJacobians	(	const Cal &	calibration,
		const Point2 &	pn,
		OptionalJacobian< 2, Dim >	Dcal = {},
		OptionalJacobian< 2, 2 >	Dp = {} )

Function which makes use of the Implicit Function Theorem to compute the Jacobians of calibrate using uncalibrate.

This is useful when there are iterative operations in the calibrate function which make computing jacobians difficult.

Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can easily compute the Jacobians: df/pi = -I (pn and pi are independent args) Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn) Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K

Template Parameters

Cal	Calibration model.
Dim	The number of parameters in the calibration model.

Parameters

p	Calibrated point.
Dcal	optional 2*p Jacobian wrpt p Cal3DS2 parameters.
Dp	optional 2*2 Jacobian wrpt intrinsic coordinates.

calibrateMeasurements()

template<class CAMERA>

Point3Vector gtsam::calibrateMeasurements ( const CameraSet< CAMERA > & cameras, const typename CAMERA::MeasurementVector & measurements )

inline

Convert pixel measurements in image to homogeneous measurements in the image plane using camera intrinsics of each measurement.

Template Parameters

CAMERA

Camera type to use.

Parameters

cameras	Cameras corresponding to each measurement.
measurements	Vector of measurements to undistort.

Returns

homogeneous measurements in image plane

calibrateMeasurementsShared()

template<class CALIBRATION>

Point3Vector gtsam::calibrateMeasurementsShared ( const CALIBRATION & cal, const Point2Vector & measurements )

inline

Convert pixel measurements in image to homogeneous measurements in the image plane using shared camera intrinsics.

Template Parameters

CALIBRATION

Calibration type to use.

Parameters

cal	Calibration with which measurements were taken.
measurements	Vector of measurements to undistort.

Returns

homogeneous measurements in image plane

choleskyCareful()

GTSAM_EXPORT std::pair< size_t, bool > gtsam::choleskyCareful	(	Matrix &	ATA,
		int	order = -1 )

"Careful" Cholesky computes the positive square-root of a positive symmetric semi-definite matrix (i.e.

that may be rank-deficient). Unlike standard Cholesky, the square-root factor may have all-zero rows for free variables.

Additionally, this function returns the index of the row after the last non-zero row in the computed factor, so that it may be truncated to an upper-trapazoidal matrix.

The second element of the return value is true if the matrix was factored successfully, or false if it was non-positive-semidefinite (i.e. indefinite or negative-(semi-)definite.

Note that this returned index is the rank of the matrix if and only if all of the zero-rows of the factor occur after any non-zero rows. This is (always?) the case during elimination of a fully-constrained least-squares problem.

The optional order argument specifies the size of the square upper-left submatrix to operate on, ignoring the rest of the matrix.

choleskyPartial()

GTSAM_EXPORT bool gtsam::choleskyPartial	(	Matrix &	ABC,
		size_t	nFrontal,
		size_t	topleft = 0 )

Partial Cholesky computes a factor [R S such that [R' 0 [R S = [A B 0 L] S' I] 0 L] B' C].

The input to this function is the matrix ABC = [A B], and the parameter [B' C] nFrontal determines the split between A, B, and C, with A being of size nFrontal x nFrontal.

if non-zero, factorization proceeds in bottom-right corner starting at topleft

Returns

true if the decomposition is successful, false if A was not positive-definite or has a Cholesky pivot that is too small relative to its original diagonal entry.

circleCircleIntersection()

GTSAM_EXPORT std::list< Point2 > gtsam::circleCircleIntersection	(	Point2	c1,
		double	r1,
		Point2	c2,
		double	r2,
		double	tol = 1e-9 )

Intersect 2 circles.

Parameters

c1	center of first circle
r1	radius of first circle
c2	center of second circle
r2	radius of second circle
tol	absolute tolerance below which we consider touching circles

Returns

list of solutions (0,1, or 2). Identical circles will return empty list, as well.

collect()

GTSAM_EXPORT Matrix gtsam::collect	(	const std::vector< const Matrix * > &	matrices,
		size_t	m = 0,
		size_t	n = 0 )

create a matrix by concatenating Given a set of matrices: A1, A2, A3... If all matrices have the same size, specifying single matrix dimensions will avoid the lookup of dimensions

Parameters

matrices	is a vector of matrices in the order to be collected
m	is the number of rows of a single matrix
n	is the number of columns of a single matrix

Returns

combined matrix [A1 A2 A3]

conjugateGradients()

template<class S, class V, class E>

V gtsam::conjugateGradients	(	const S &	Ab,
		V	x,
		const ConjugateGradientParameters &	parameters,
		bool	steepest = false )

Method of conjugate gradients (CG) template "System" class S needs gradient(S,v), e=S*v, v=S^e "Vector" class V needs dot(v,v), -v, v+v, s*v "Vector" class E needs dot(v,v).

Parameters

Ab,the	"system" that needs to be solved, examples below
x	is the initial estimate
steepest	flag, if true does steepest descent, not CG

ConvertModel()

template<class T, size_t Dim>

SharedNoiseModel gtsam::ConvertModel ( const SharedNoiseModel & model )

inline

Ensure a noise model has the correct dimension for a given type.

If the model is already of dimension Dim, it is returned as-is. Otherwise, ConvertNoiseModel is called to convert it. Throws if the model's dimension does not match T's expected dimension before conversion.

Template Parameters

T	The type whose dimension is checked.
Dim	The required dimension.

Parameters

model

The input noise model.

Returns

A noise model of dimension Dim.

ConvertNoiseModel()

GTSAM_EXPORT SharedNoiseModel gtsam::ConvertNoiseModel	(	const SharedNoiseModel &	model,
		size_t	n,
		bool	defaultToUnit = true )

Convert a possibly robust noise model to an isotropic model for Frobenius factors.

This function is used to convert a noise model, which may be robust, into an isotropic noise model suitable for Frobenius factors. If the input noise model is null, it returns an n-dimensional isotropic noise model with sigma=1.0. If the input noise model is isotropic, it extends it to the desired dimension. If the noise model is robust, the sigmas of the underlying noise model are used. If the noise model is not isotropic and defaultToUnit is false, an exception is thrown.

Parameters

model	The input noise model (possibly robust).
n	The desired dimension for the isotropic model.
defaultToUnit	If true, fallback to unit if conversion is not possible.

Exceptions

std::runtime_error

if model not isotropic and defaultToUnit =false.

Returns

An isotropic (possibly robust) noise model.

createPinholeCalibration()

template<class CALIBRATION>

Cal3_S2 gtsam::createPinholeCalibration

(

const CALIBRATION &

cal

)

Create a pinhole calibration from a different Cal3 object, removing distortion.

Template Parameters

CALIBRATION

Original calibration object.

Parameters

cal	Input calibration object.

Returns

Cal3_S2 with only the pinhole elements of cal.

createUnknowns()

template<typename T>

std::vector< Expression< T > > gtsam::createUnknowns	(	size_t	n,
		char	c,
		size_t	start )

Construct an array of leaves.

Construct an array of unknown expressions with successive symbol keys Example: createUnknowns<Pose2>(3,'x') creates unknown expressions for x0,x1,x2.

cross()

GTSAM_EXPORT Point3 gtsam::cross	(	const Point3 &	p,
		const Point3 &	q,
		OptionalJacobian< 3, 3 >	H_p = {},
		OptionalJacobian< 3, 3 >	H_q = {} )

cross product

Returns

p x q

demangle()

std::string GTSAM_EXPORT gtsam::demangle

(

const char *

name

)

Pretty print Value type name.

Function to demangle type name of variable, e.g. demangle(typeid(x).name()).

DiscreteFactorFromErrors()

DiscreteFactor::shared_ptr gtsam::DiscreteFactorFromErrors	(	const DiscreteKeys &	discreteKeys,
		const AlgebraicDecisionTree< Key > &	errors )

Take negative log-values, shift them so that the minimum value is 0, and then exponentiate to create a TableFactor (not normalized yet!).

Parameters

discreteKeys	The discrete keys for the resulting DecisionTreeFactor
errors	DecisionTree of (unnormalized) errors.

Returns

TableFactor::shared_ptr

Parameters

errors

DecisionTree of (unnormalized) errors.

Returns

TableFactor::shared_ptr

DLT()

GTSAM_EXPORT std::tuple< int, double, Vector > gtsam::DLT	(	const Matrix &	A,
		double	rank_tol = 1e-9 )

Direct linear transform algorithm that calls svd to find a vector v that minimizes the algebraic error A*v.

Parameters

A	of size m*n, where m>=n (pad with zero rows if not!) Returns rank of A, minimum error (singular value), and corresponding eigenvector (column of V, with A=U*S*V')

doubleCross()

GTSAM_EXPORT Point3 gtsam::doubleCross	(	const Point3 &	p,
		const Point3 &	q,
		OptionalJacobian< 3, 3 >	H1 = {},
		OptionalJacobian< 3, 3 >	H2 = {} )

double cross product

Returns

p x (p x q)

ediv_()

GTSAM_EXPORT Vector gtsam::ediv_	(	const Vector &	a,
		const Vector &	b )

elementwise division, but 0/0 = 0, not inf

Parameters

a	first vector
b	second vector

Returns

vector [a(i)/b(i)]

EliminateQR()

GTSAM_EXPORT std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< JacobianFactor > > gtsam::EliminateQR	(	const GaussianFactorGraph &	factors,
		const Ordering &	keys )

Multiply all factors and eliminate the given keys from the resulting factor using a QR variant that handles constraints (zero sigmas).

Densely partially eliminate with QR factorization, this is usually provided as an argument to one of the factor graph elimination functions (see EliminateableFactorGraph).

Computation happens in noiseModel::Gaussian::QR Returns a conditional on those keys, and a new factor on the separator.

EliminateSymbolic()

GTSAM_EXPORT std::pair< std::shared_ptr< SymbolicConditional >, std::shared_ptr< SymbolicFactor > > gtsam::EliminateSymbolic	(	const SymbolicFactorGraph &	factors,
		const Ordering &	keys )

Dense elimination function for symbolic factors.

This is usually provided as an argument to one of the factor graph elimination functions (see EliminateableFactorGraph). The factor graph elimination functions do sparse variable elimination, and use this function to eliminate single variables or variable cliques.

EpipolarTransfer()

GTSAM_EXPORT Point2 gtsam::EpipolarTransfer	(	const Matrix3 &	Fca,
		const Point2 &	pa,
		const Matrix3 &	Fcb,
		const Point2 &	pb )

Transfer projections from cameras a and b to camera c.

Take two fundamental matrices Fca and Fcb, and two points pa and pb, and returns the 2D point in view (c) where the epipolar lines intersect.

expm() [1/2]

GTSAM_EXPORT Matrix gtsam::expm	(	const Matrix &	A,
		size_t	K = 7 )

Numerical exponential map, naive approach, not industrial strength !

!!

Parameters

A	matrix to exponentiate
K	number of iterations

expm() [2/2]

template<class T>

T gtsam::expm	(	const Vector &	x,
		int	K = 7 )

Exponential map given exponential coordinates class T needs a constructor from Matrix.

Parameters

x	exponential coordinates, vector of size n @ return a T

expmap_default()

template<class Class>

Class gtsam::expmap_default ( const Class & t, const Vector & d )

inline

Exponential map centered at l0, s.t.

exp(t,d) = t*exp(d)

extendedPoseContactPrediction()

Vector3 gtsam::extendedPoseContactPrediction ( const ExtendedPose3d & state, size_t footColumn, OptionalMatrixType H = {} )

inline

Predict the IMU-frame contact vector for an ExtendedPose3 state.

Pose3::transformTo returns the Jacobian with respect to the embedded (R, p) pose as [skew(q) -I], where q = R^T (f - p). The foothold block in ExtendedPose3 uses body-frame tangent coordinates, and x(i) has component Jacobian R, so the foothold chain rule is

d q / d delta_f = (d q / d f_world) * (d f_world / d delta_f) = R^T * R = I.

We therefore reuse transformTo only for the pose block and write the foot block directly as the identity.

fastSync() [1/2]

template<class T>

Values gtsam::fastSync	(	const NonlinearFactorGraph &	graph,
		const Ordering &	ordering )

Initialize a synchronization graph using a caller-supplied ordering.

Parameters

graph	Synchronization factor graph.
ordering	Complete variable elimination ordering.

fastSync() [2/2]

template<class T>

Values gtsam::fastSync	(	const NonlinearFactorGraph &	graph,
		Ordering::OrderingType	orderingType = Ordering::METIS )

Initialize a synchronization graph using FAST-Sync.

The graph may contain BetweenFactor<T> or FrobeniusBetweenFactor<T> measurements and at most one matching PriorFactor<T> or FrobeniusPrior<T>; factors of other types are ignored. Measurement noise models must have the factor's residual dimension and be finite, positive, non-robust, unconstrained, diagonal, and isotropic. Arbitrary keys are preserved in the returned Values.

T must be a fixed-size square matrix Lie group with a FastSyncProjection<T> specialization.

Parameters

graph	Synchronization factor graph.
orderingType	Fill-reducing ordering used by sequential elimination.

Exceptions

std::invalid_argument	for invalid noise models, an empty measurement graph, a prior outside the measurement graph, or multiple matching priors.
IndeterminateSystemException	when the measurement graph is disconnected or otherwise underconstrained.
std::runtime_error	for an unsupported ordering type, or when METIS is selected but unavailable in the current build.

fetzer_d()

Eigen::Vector4d gtsam::fetzer_d ( const Eigen::Vector3d & ai, const Eigen::Vector3d & bi, const Eigen::Vector3d & aj, const Eigen::Vector3d & bj, int u, int v )

inline

Build one 4-vector of bilinear coefficients from two pairs of 3-vectors, picking rows (u, v) — the 2x2 minor of (ai/bi) against (aj/bj) used by the Fetzer focal-length residual.

Ported from GLOMAP (Lindenberger's DMAP).

fetzer_ds()

std::array< Eigen::Vector4d, 3 > gtsam::fetzer_ds ( const Eigen::Matrix3d & G )

inline

Precompute the Fetzer coefficient vectors from G = K_j^T F K_i.

A single SVD of the (principal-point corrected, focal-free) matrix G, from which the essential-matrix constraint becomes a rational equation in the two focal lengths. Returns {d_01, d_02, d_12}; only d_01 and d_12 enter the residual (d_02 is the redundant third constraint, kept for parity/testing).

findExampleDataFile()

GTSAM_EXPORT std::string gtsam::findExampleDataFile

(

const std::string &

name

)

Find the full path to an example dataset distributed with gtsam.

The name may be specified with or without a file extension - if no extension is given, this function first looks for the .graph extension, then .txt. We first check the gtsam source tree for the file, followed by the installed example dataset location. Both the source tree and installed locations are obtained from CMake during compilation.

Returns

The full path and filename to the requested dataset.

Exceptions

std::invalid_argument

if no matching file could be found using the search process described above.

fpEqual()

GTSAM_EXPORT bool gtsam::fpEqual	(	double	a,
		double	b,
		double	tol,
		bool	check_relative_also = true )

Ensure we are not including a different version of Eigen in user code than while compiling gtsam, since it can lead to hard-to-understand runtime crashes.

Numerically stable function for comparing if floating point values are equal within epsilon tolerance. Used for vector and matrix comparison with C++11 compatible functions.

If either value is NaN or Inf, we check for both values to be NaN or Inf respectively for the comparison to be true. If one is NaN/Inf and the other is not, returns false.

Parameters

check_relative_also

is a flag which toggles additional checking for relative error. This means that if either the absolute error or the relative error is within the tolerance, the result will be true. By default, the flag is true.

Return true if two numbers are close wrt tol.

gtsam2openGL() [1/2]

GTSAM_EXPORT Pose3 gtsam::gtsam2openGL

(

const Pose3 &

PoseGTSAM

)

This function converts a GTSAM camera pose to an openGL camera pose.

Parameters

PoseGTSAM

pose in GTSAM format

Returns

Pose3 in openGL format

gtsam2openGL() [2/2]

GTSAM_EXPORT Pose3 gtsam::gtsam2openGL	(	const Rot3 &	R,
		double	tx,
		double	ty,
		double	tz )

This function converts a GTSAM camera pose to an openGL camera pose.

Parameters

R	rotation in GTSAM
tx	x component of the translation in GTSAM
ty	y component of the translation in GTSAM
tz	z component of the translation in GTSAM

Returns

Pose3 in openGL format

hasConstraints()

GTSAM_EXPORT bool gtsam::hasConstraints

(

const GaussianFactorGraph &

factors

)

Evaluates whether linear factors have any constrained noise models.

Returns

true if any factor is constrained.

house()

GTSAM_EXPORT std::pair< double, Vector > gtsam::house

(

const Vector &

x

)

house(x,j) computes HouseHolder vector v and scaling factor beta from x, such that the corresponding Householder reflection zeroes out all but x.

(j), j is base 0. Golub & Van Loan p 210.

householder()

GTSAM_EXPORT void gtsam::householder	(	Matrix &	A,
		size_t	k )

Householder tranformation, zeros below diagonal.

Parameters

k	number of columns to zero out below diagonal
A	matrix

Returns

nothing: in place !!!

householder_()

GTSAM_EXPORT void gtsam::householder_	(	Matrix &	A,
		size_t	k,
		bool	copy_vectors = true )

Imperative version of Householder QR factorization, Golub & Van Loan p 224 version with Householder vectors below diagonal, as in GVL.

Householder transformation, Householder vectors below diagonal.

Parameters

k	number of columns to zero out below diagonal
A	matrix
copy_vectors	- true to copy Householder vectors below diagonal

Returns

nothing: in place !!!

HybridOrdering()

GTSAM_EXPORT const Ordering gtsam::HybridOrdering

(

const HybridGaussianFactorGraph &

graph

)

Return a Colamd constrained ordering where the discrete keys are eliminated after the continuous keys.

Returns

const Ordering

initialCamerasAndPointsEstimate()

GTSAM_EXPORT Values gtsam::initialCamerasAndPointsEstimate

(

const SfmData &

db

)

This function creates initial values for cameras and points from db.

Note: Pose keys are simply integer indices, points use Symbol('p', j).

Parameters

SfmData

Returns

Values

initialCamerasEstimate()

GTSAM_EXPORT Values gtsam::initialCamerasEstimate

(

const SfmData &

db

)

This function creates initial values for cameras from db.

No symbol is used, so camera keys are simply integer indices.

Parameters

SfmData

Returns

Values

inplace_QR()

GTSAM_EXPORT void gtsam::inplace_QR

(

Matrix &

A

)

QR factorization using Eigen's internal block QR algorithm.

Parameters

A	is the input matrix, and is the output
clear_below_diagonal	enables zeroing out below diagonal

integrateSequentialRotations()

GTSAM_EXPORT Rot3 gtsam::integrateSequentialRotations	(	const Vector &	times,
		ConstMatrixView	measuredOmegas,
		const Vector3 &	biasHat = Vector3::Zero(),
		const Rot3 &	body_R_sensor = Rot3() )

Integrate timed gyroscope samples using sequential trapezoidal rotation increments.

The timestamps must increase strictly, and measuredOmegas must have one three-axis sample per timestamp.

The bias is subtracted in the sensor frame before body_R_sensor rotates each sample into the body frame.

Parameters

times	Strictly increasing sample timestamps.
measuredOmegas	M-by-3 matrix of sensor-frame angular velocities.
biasHat	Gyroscope bias expressed in the sensor frame.
body_R_sensor	Rotation from sensor coordinates to body coordinates.

Returns

The integrated body rotation.

Exceptions

std::invalid_argument

if the sample count, matrix shape, or timestamp ordering is invalid.

integrateSingleSpeedConing()

GTSAM_EXPORT Rot3 gtsam::integrateSingleSpeedConing	(	const Vector &	times,
		ConstMatrixView	measuredOmegas,
		const Vector3 &	biasHat = Vector3::Zero(),
		const Rot3 &	body_R_sensor = Rot3() )

Integrate timed gyroscope samples with a single-speed coning correction.

The timestamps must increase strictly, and measuredOmegas must have one three-axis sample per timestamp.

The bias is subtracted in the sensor frame before body_R_sensor rotates each sample into the body frame.

Parameters

times	Strictly increasing sample timestamps.
measuredOmegas	M-by-3 matrix of sensor-frame angular velocities.
biasHat	Gyroscope bias expressed in the sensor frame.
body_R_sensor	Rotation from sensor coordinates to body coordinates.

Returns

The integrated body rotation.

Exceptions

std::invalid_argument

if the sample count, matrix shape, or timestamp ordering is invalid.

interpolate()

template<typename T>

T gtsam::interpolate	(	const T &	X,
		const T &	Y,
		double	t,
		typename MakeOptionalJacobian< T, T >::type	Hx = {},
		typename MakeOptionalJacobian< T, T >::type	Hy = {},
		typename MakeOptionalJacobian< T, double >::type	Ht = {} )

Linear interpolation between X and Y by coefficient t.

Typically t \in [0,1], but can also be used to extrapolate before pose X or after pose Y.

interpolateFactorGraph()

template<class PoseType, class FactorGraphType = NonlinearFactorGraph>

FactorGraphType gtsam::interpolateFactorGraph	(	const NonlinearFactorGraph &	graph,
		const std::set< StateData > &	estimated_states,
		const std::set< StateData > &	interp_states,
		Vector	q_psd_diag,
		bool	fixed_noise = false )

Utility functions for working with Factor Graphs containing interpolated variables.

Convert a factor graph by removing interpolated states.

This helper replaces factors that reference interpolated states with equivalent wrapper factors that act on the bordering estimated states. Additionally, WNOA motion prior factors are added between successive estimated states. Factors that do not reference interpolated states are copied unchanged into the returned graph. Note that only a single type of pose is currently supported.

Template Parameters

PoseType	Pose type used in the graph (e.g. Pose2, Pose3).
FactorGraphType	Type of factor graph to return. Defaults to NonlinearFactorGraph, but can be set to WnoaFactorGraph to NonlinearFactorGraph or WnoaFactorGraph. Note that the user is responsible for ensuring that the WnoaFactorGraph is defined with the same PoseType as the template parameter.

Parameters

graph	Input factor graph possibly containing factors on interpolated states.
estimated_states	Ordered set of estimated StateData (main-solve states).
interp_states	Ordered set of StateData entries to be interpolated/removed.
q_psd_diag	Diagonal PSD vector for the WNOA motion prior (dimension must match PoseType).
fixed_noise	If true, do not augment measurement noise models for interpolation.

Returns

FactorGraphType factor graph with interpolated states removed and factors updated.

interpolateWnoaFactorGraph()

template<class PoseType>

WnoaFactorGraph< PoseType > gtsam::interpolateWnoaFactorGraph	(	const NonlinearFactorGraph &	graph,
		const std::set< StateData > &	estimated_states,
		const std::set< StateData > &	interp_states,
		Vector	q_psd_diag,
		bool	fixed_noise = false )

WnoaFactorGraph specialization of interpolateFactorGraph for use in Python bindings.

Convert a factor graph by removing interpolated states.

This helper replaces factors that reference interpolated states with equivalent wrapper factors that act on the bordering estimated states. Additionally, WNOA motion prior factors are added between successive estimated states. Factors that do not reference interpolated states are copied unchanged into the returned graph. Note that only a single type of pose is currently supported.

Template Parameters

PoseType	Pose type used in the graph (e.g. Pose2, Pose3).
FactorGraphType	Type of factor graph to return. Defaults to NonlinearFactorGraph, but can be set to WnoaFactorGraph to NonlinearFactorGraph or WnoaFactorGraph. Note that the user is responsible for ensuring that the WnoaFactorGraph is defined with the same PoseType as the template parameter.

Parameters

graph	Input factor graph possibly containing factors on interpolated states.
estimated_states	Ordered set of estimated StateData (main-solve states).
interp_states	Ordered set of StateData entries to be interpolated/removed.
q_psd_diag	Diagonal PSD vector for the WNOA motion prior (dimension must match PoseType).
fixed_noise	If true, do not augment measurement noise models for interpolation.

Returns

FactorGraphType factor graph with interpolated states removed and factors updated.

linear_dependent()

GTSAM_EXPORT bool gtsam::linear_dependent	(	const Vector &	vec1,
		const Vector &	vec2,
		double	tol = 1e-9 )

check whether two vectors are linearly dependent

Parameters

vec1	Vector
vec2	Vector
tol	1e-9

Returns

bool

linearExpression()

template<typename T, typename A>

Expression< T > gtsam::linearExpression	(	const std::function< T(A)> &	f,
		const Expression< A > &	expression,
		const Eigen::Matrix< double, traits< T >::dimension, traits< A >::dimension > &	dTdA )

Create an expression out of a linear function f:T->A with (constant) Jacobian dTdA TODO(frank): create a more efficient version like ScalarMultiplyExpression.

This version still does a malloc every linearize.

linearizeNumerically()

JacobianFactor gtsam::linearizeNumerically ( const NoiseModelFactor & factor, const Values & values, double delta = 1e-5 )

inline

Linearize a nonlinear factor using numerical differentiation The benefit of this method is that it does not need to know what types are involved to evaluate the factor.

If all the machinery of gtsam is working correctly, we should get the correct numerical derivatives out the other side. NOTE(frank): factors that have non vector-space measurements use between or LocalCoordinates to evaluate the error, and their derivatives will only be correct for near-zero errors. This is fixable but expensive, and does not matter in practice as most factors will sit near zero errors anyway. However, it means that below will only be exact for the correct measurement.

load2D() [1/2]

GTSAM_EXPORT GraphAndValues gtsam::load2D	(	const std::string &	filename,
		SharedNoiseModel	model = SharedNoiseModel(),
		size_t	maxIndex = 0,
		bool	addNoise = false,
		bool	smart = true,
		NoiseFormat	noiseFormat = NoiseFormatAUTO,
		KernelFunctionType	kernelFunctionType = KernelFunctionTypeNONE )

Load TORO/G2O style graph files.

Parameters

filename
model	optional noise model to use instead of one specified by file
maxIndex	if non-zero cut out vertices >= maxIndex
addNoise	add noise to the edges
smart	try to reduce complexity of covariance to cheapest model
noiseFormat	how noise parameters are stored
kernelFunctionType	whether to wrap the noise model in a robust kernel

Returns

graph and initial values

load2D() [2/2]

GTSAM_EXPORT GraphAndValues gtsam::load2D	(	std::pair< std::string, SharedNoiseModel >	dataset,
		size_t	maxIndex = 0,
		bool	addNoise = false,
		bool	smart = true,
		NoiseFormat	noiseFormat = NoiseFormatAUTO,
		KernelFunctionType	kernelFunctionType = KernelFunctionTypeNONE )

Load TORO 2D Graph.

Parameters

dataset/model	pair as constructed by [dataset]
maxIndex	if non-zero cut out vertices >= maxIndex
addNoise	add noise to the edges
smart	try to reduce complexity of covariance to cheapest model

load3D()

GTSAM_EXPORT GraphAndValues gtsam::load3D

(

const std::string &

filename

)

Load a TORO/G2O-style 3D graph.

In addition to Pose3 edges and vertices, this accepts Cartesian landmark observations of the form EDGE_SE3_TRACKXYZ pose_id landmark_id offset_id x y z I11 I12 I13 I22 I23 I33, where the point and full information matrix are expressed in the sensor frame. The referenced PARAMS_SE3OFFSET must be identity. Each edge is converted to a BearingRangeFactor<Pose3, Point3>. The information models are equivalent at the measurement linearization point, but not globally under the nonlinear Cartesian-to-bearing-range transformation.

logmap_default()

template<class Class>

Vector gtsam::logmap_default ( const Class & l0, const Class & lp )

inline

Log map centered at l0, s.t.

exp(l0,log(l0,lp)) = lp

MakeFunctorizedFactor()

template<typename T, typename R, typename FUNC>

FunctorizedFactor< R, T > gtsam::MakeFunctorizedFactor	(	Key	key,
		const R &	z,
		const SharedNoiseModel &	model,
		const FUNC	func )

Helper function to create a functorized factor.

Uses function template deduction to identify return type and functor type, so template list only needs the functor argument type.

MakeFunctorizedFactor2()

template<typename T1, typename T2, typename R, typename FUNC>

FunctorizedFactor2< R, T1, T2 > gtsam::MakeFunctorizedFactor2	(	Key	key1,
		Key	key2,
		const R &	z,
		const SharedNoiseModel &	model,
		const FUNC	func )

Helper function to create a functorized factor.

Uses function template deduction to identify return type and functor type, so template list only needs the functor argument type.

moveWithBounce()

std::pair< Pose2, bool > gtsam::moveWithBounce	(	const Pose2 &	cur_pose,
		double	step_size,
		const std::vector< SimWall2D >	walls,
		Sampler &	angle_drift,
		Sampler &	reflect_noise,
		const Rot2 &	bias = Rot2() )

Calculates the next pose in a trajectory constrained by walls, with noise on angular drift and reflection noise.

Parameters

cur_pose	is the pose of the robot
step_size	is the size of the forward step the robot tries to take
walls	is a set of walls to use for bouncing
angle_drift	is a sampler for angle drift (dim=1)
reflect_noise	is a sampler for scatter after hitting a wall (dim=3)

Returns

the next pose for the robot NOTE: samplers cannot be const

nonlinearConjugateGradient()

template<class S, class V>

std::tuple< V, int > gtsam::nonlinearConjugateGradient	(	const S &	system,
		const V &	initial,
		const NonlinearOptimizerParams &	params,
		const bool	singleIteration,
		const DirectionMethod &	directionMethod = DirectionMethod::PolakRibiere,
		const bool	gradientDescent = false )

Implement the nonlinear conjugate gradient method using the Polak-Ribiere formula suggested in http://en.wikipedia.org/wiki/Nonlinear_conjugate_gradient_method.

The S (system) class requires three member functions: error(state), gradient(state) and advance(state, step-size, direction). The V class denotes the state or the solution.

The last parameter is a switch between gradient-descent and conjugate gradient

openGL2gtsam()

GTSAM_EXPORT Pose3 gtsam::openGL2gtsam	(	const Rot3 &	R,
		double	tx,
		double	ty,
		double	tz )

This function converts an openGL camera pose to an GTSAM camera pose.

Parameters

R	rotation in openGL
tx	x component of the translation in openGL
ty	y component of the translation in openGL
tz	z component of the translation in openGL

Returns

Pose3 in GTSAM format

operator*()

template<typename T>

Expression< T > gtsam::operator*	(	const Expression< T > &	expression1,
		const Expression< T > &	expression2 )

Construct a product expression, assumes T::compose(T) -> T.

Construct a product expression, assumes T::compose(T) -> T Example: Expression<Point2> a(0), b(1), c = a*b;.

operator>>() [1/2]

GTSAM_EXPORT std::istream & gtsam::operator>>	(	std::istream &	inputStream,
		Matrix &	destinationMatrix )

Read a matrix from an input stream, such as a file.

Entries can be either tab-, space-, or comma-separated, similar to the format read by the MATLAB dlmread command.

operator>>() [2/2]

std::istream & gtsam::operator>>	(	std::istream &	is,
		GaussianFactorGraphValuePair &	pair )

Create a dummy overload of >> for GaussianFactorGraphValuePair so that HybridGaussianProductFactor compiles with the constructor DecisionTree(const std::vector<LabelC>& labelCs, const std::string& table).

Needed to compile on Windows.

optimize()

GTSAM_EXPORT Point3 gtsam::optimize	(	const NonlinearFactorGraph &	graph,
		const Values &	values,
		Key	landmarkKey )

Optimize for triangulation.

Parameters

graph	nonlinear factors for projection
values	initial values
landmarkKey	to refer to landmark

Returns

refined Point3

optimizeWildfire()

size_t gtsam::optimizeWildfire	(	const ISAM2Clique::shared_ptr &	root,
		double	threshold,
		const KeySet &	replaced,
		VectorValues *	delta )

Optimize the BayesTree, starting from the root.

Parameters

threshold	The maximum change against the PREVIOUS delta for non-replaced variables that can be ignored, ie. the old delta entry is kept and recursive backsubstitution might eventually stop if none of the changed variables are contained in the subtree.
replaced	Needs to contain all variables that are contained in the top of the Bayes tree that has been redone.

Returns

The number of variables that were solved for.

Parameters

delta

The current solution, an offset from the linearization point.

parseEdge()

GTSAM_EXPORT std::optional< IndexedEdge > gtsam::parseEdge	(	std::istream &	is,
		const std::string &	tag )

Parse TORO/G2O edge "id1 id2 x y yaw".

Parameters

is	input stream
tag	string parsed from input stream, will only parse if edge type

parseFactors()

template<typename T>

GTSAM_EXPORT std::vector< typename BetweenFactor< T >::shared_ptr > gtsam::parseFactors	(	const std::string &	filename,
		const noiseModel::Diagonal::shared_ptr &	model = nullptr,
		size_t	maxIndex = 0 )

Parse BetweenFactors in a line-based text format (like g2o) into a vector of shared pointers.

Instantiated in .cpp T equal to Pose2 and Pose3.

parseMeasurements()

template<typename T>

GTSAM_EXPORT std::vector< BinaryMeasurement< T > > gtsam::parseMeasurements	(	const std::string &	filename,
		const noiseModel::Diagonal::shared_ptr &	model = nullptr,
		size_t	maxIndex = 0 )

Parse binary measurements in a line-based text format (like g2o) into a vector.

Instantiated in .cpp for Pose2, Rot2, Pose3, and Rot3. The rotation versions parse poses and extract only the rotation part, using the marginal covariance as noise model.

parseVariables()

template<typename T>

GTSAM_EXPORT std::map< size_t, T > gtsam::parseVariables	(	const std::string &	filename,
		size_t	maxIndex = 0 )

Parse variables in a line-based text format (like g2o) into a map.

Instantiated in .cpp Pose2, Point2, Pose3, and Point3. Note the map keys are integer indices, not gtsam::Keys. This is is different below where landmarks will use L(index) symbols.

parseVertexLandmark()

GTSAM_EXPORT std::optional< IndexedLandmark > gtsam::parseVertexLandmark	(	std::istream &	is,
		const std::string &	tag )

Parse G2O landmark vertex "id x y".

Parameters

is	input stream
tag	string parsed from input stream, will only parse if vertex type

parseVertexPose()

GTSAM_EXPORT std::optional< IndexedPose > gtsam::parseVertexPose	(	std::istream &	is,
		const std::string &	tag )

Parse TORO/G2O vertex "id x y yaw".

Parameters

is	input stream
tag	string parsed from input stream, will only parse if vertex type

preconditionedConjugateGradient()

template<class S, class V>

V gtsam::preconditionedConjugateGradient	(	const S &	system,
		const V &	initial,
		const ConjugateGradientParameters &	parameters )

Solve a preconditioned linear system and return only the estimate.

Use preconditionedConjugateGradientDetailed() when convergence diagnostics are required.

Template Parameters

S	Linear-system type providing the PCG vector operations.
V	Vector type accepted by the system.

Parameters

system	Linear system and split preconditioner operations.
initial	Initial estimate.
parameters	Iteration limits, reset interval, and residual tolerances.

Returns

Final estimate.

preconditionedConjugateGradientDetailed()

template<class S, class V>

ConjugateGradientResult< V > gtsam::preconditionedConjugateGradientDetailed	(	const S &	system,
		const V &	initial,
		const ConjugateGradientParameters &	parameters,
		bool	collectResidualHistory = true )

Solve a linear system with split-preconditioned conjugate gradients.

The system must provide residual, multiply, leftPrecondition, rightPrecondition, scal, dot, and axpy. For a preconditioner M = L*L.transpose(), the recurrence operates on the residual L.inverse() * (b - A*x) and search direction L.transpose().inverse() * r.

Template Parameters

S	Linear-system type providing the required vector operations.
V	Vector type accepted by the system.

Parameters

system	Linear system and split preconditioner operations.
initial	Initial estimate.
parameters	Iteration limits, reset interval, and residual tolerances.
collectResidualHistory	Whether to retain the initial and per-iteration residual norms in the returned statistics.

Returns

Final estimate together with convergence diagnostics.

REFERENCES: [1] Y. Saad, "Preconditioned Iterations," in Iterative Methods for Sparse Linear Systems, 2nd ed. SIAM, 2003, ch. 9, sec. 2, pp.276-281.

predecessorMap2Graph()

template<class G, class V, class KEY>

std::tuple< G, V, std::map< KEY, V > > gtsam::predecessorMap2Graph

(

const PredecessorMap< KEY > &

p_map

)

Build takes a predecessor map, and builds a directed graph corresponding to the tree.

G = Graph type V = Vertex type

qr()

GTSAM_EXPORT std::pair< Matrix, Matrix > gtsam::qr

(

const Matrix &

A

)

Householder QR factorization, Golub & Van Loan p 224, explicit version.

QR factorization, inefficient, best use imperative householder below m*n matrix -> m*m Q, m*n R.

Parameters

A

a matrix

Returns

<Q,R> rotation matrix Q, upper triangular R

readBal()

GTSAM_EXPORT SfmData gtsam::readBal

(

const std::string &

filename

)

This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data as a SfmData structure.

Mainly used by wrapped code.

Parameters

filename

The name of the BAL file.

Returns

SfM structure where the data is stored.

readG2o()

GTSAM_EXPORT GraphAndValues gtsam::readG2o	(	const std::string &	g2oFile,
		const bool	is3D = false,
		KernelFunctionType	kernelFunctionType = KernelFunctionTypeNONE )

This function parses a g2o file and stores the measurements into a NonlinearFactorGraph and the initial guess in a Values structure.

Parameters

filename	The name of the g2o file\
is3D	indicates if the file describes a 2D or 3D problem
kernelFunctionType	whether to wrap the noise model in a robust kernel

Returns

graph and initial values

RQ()

GTSAM_EXPORT std::pair< Matrix3, Vector3 > gtsam::RQ	(	const Matrix3 &	A,
		OptionalJacobian< 3, 9 >	H = {} )

[RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' such that A = R*Q = R*Qz'*Qy'*Qx'.

When A is a rotation matrix, R will be the identity and Q is a yaw-pitch-roll decomposition of A. The implementation uses Givens rotations and is based on Hartley-Zisserman.

Parameters

A	3 by 3 matrix A=RQ

Returns

an upper triangular matrix R

a vector [thetax, thetay, thetaz] in radians.

skewSymmetric()

Matrix3 gtsam::skewSymmetric ( double wx, double wy, double wz )

inline

skew symmetric matrix returns this: 0 -wz wy wz 0 -wx -wy wx 0

Parameters

wx	3 dimensional vector
wy
wz

Returns

a 3*3 skew symmetric matrix

splitFactorGraph()

std::pair< GaussianFactorGraph, GaussianFactorGraph > GTSAM_EXPORT gtsam::splitFactorGraph	(	const GaussianFactorGraph &	factorGraph,
		const Subgraph &	subgraph )

Split the graph into a subgraph and the remaining edges.

Note that the remaining factorgraph has null factors.

steepestDescent()

GTSAM_EXPORT Vector gtsam::steepestDescent	(	const Matrix &	A,
		const Vector &	b,
		const Vector &	x,
		const ConjugateGradientParameters &	parameters )

convenience calls using matrices, will create System class internally:

Method of steepest gradients, Matrix version

svd()

GTSAM_EXPORT void gtsam::svd	(	const Matrix &	A,
		Matrix &	U,
		Vector &	S,
		Matrix &	V )

SVD computes economy SVD A=U*S*V'.

Parameters

A	an m*n matrix
U	output argument: rotation matrix
S	output argument: sorted vector of singular values
V	output argument: rotation matrix if m > n then U*S*V' = (m*n)*(n*n)*(n*n) if m < n then U*S*V' = (m*m)*(m*m)*(m*n) Careful! The dimensions above reflect V', not V, which is n*m if m<n. U is a basis in R^m, V is a basis in R^n You can just pass empty matrices U,V, and vector S, they will be re-allocated.

symbol()

Key gtsam::symbol ( unsigned char c, std::uint64_t j )

inline

Create a symbol key from a character and index, i.e.

x5.

topLeft()

GTSAM_EXPORT Matrix3 gtsam::topLeft	(	const SO4 &	Q,
		OptionalJacobian< 9, 6 >	H = {} )

Project to top-left 3*3 matrix.

Note this is not in general \in SO(3).

transform_point()

template<class T, class P>

P gtsam::transform_point	(	const T &	trans,
		const P &	global,
		OptionalMatrixType	Dtrans,
		OptionalMatrixType	Dglobal )

Transform function that must be specialized specific domains.

Template Parameters

T	is a Transform type
P	is a point type

transformTo()

GTSAM_EXPORT Line3 gtsam::transformTo	(	const Pose3 &	wTc,
		const Line3 &	wL,
		OptionalJacobian< 4, 6 >	Dpose = {},
		OptionalJacobian< 4, 4 >	Dline = {} )

Transform a line from world to camera frame.

Parameters

wTc	- Pose3 of camera in world frame
wL	- Line3 in world frame
Dpose	- OptionalJacobian of transformed line with respect to p
Dline	- OptionalJacobian of transformed line with respect to l

Returns

Transformed line in camera frame

triangulateDLT()

GTSAM_EXPORT Point3 gtsam::triangulateDLT	(	const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &	projection_matrices,
		const Point2Vector &	measurements,
		double	rank_tol = 1e-9 )

DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312.

Parameters

projection_matrices	Projection matrices (K*P^-1)
measurements	2D measurements
rank_tol	SVD rank tolerance

Returns

Triangulated Point3

triangulateHomogeneousDLT() [1/2]

GTSAM_EXPORT Vector4 gtsam::triangulateHomogeneousDLT	(	const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &	projection_matrices,
		const Point2Vector &	measurements,
		double	rank_tol = 1e-9 )

DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312.

Parameters

projection_matrices	Projection matrices (K*P^-1)
measurements	2D measurements
rank_tol	SVD rank tolerance

Returns

Triangulated point, in homogeneous coordinates

triangulateHomogeneousDLT() [2/2]

GTSAM_EXPORT Vector4 gtsam::triangulateHomogeneousDLT	(	const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &	projection_matrices,
		const std::vector< Unit3 > &	measurements,
		double	rank_tol = 1e-9 )

Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman).

Parameters

projection_matrices	Projection matrices (K*P^-1)
measurements	Unit3 bearing measurements
rank_tol	SVD rank tolerance

Returns

Triangulated point, in homogeneous coordinates

triangulateLOST()

GTSAM_EXPORT Point3 gtsam::triangulateLOST	(	const std::vector< Pose3 > &	poses,
		const Point3Vector &	calibratedMeasurements,
		const SharedIsotropic &	measurementNoise,
		double	rank_tol = 1e-9 )

Triangulation using the LOST (Linear Optimal Sine Triangulation) algorithm proposed in https://arxiv.org/pdf/2205.12197.pdf by Sebastien Henry and John Christian.

Parameters

poses	camera poses in world frame
calibratedMeasurements	measurements in homogeneous coordinates in each camera pose
measurementNoise	isotropic noise model for the measurements

Returns

triangulated point in world coordinates

triangulateNonlinear() [1/2]

template<class CAMERA>

Point3 gtsam::triangulateNonlinear	(	const CameraSet< CAMERA > &	cameras,
		const typename CAMERA::MeasurementVector &	measurements,
		const Point3 &	initialEstimate,
		const SharedNoiseModel &	model = nullptr )

Given an initial estimate , refine a point using measurements in several cameras.

Parameters

cameras	pinhole cameras (monocular or stereo)
measurements	2D measurements
initialEstimate

Returns

refined Point3

triangulateNonlinear() [2/2]

template<class CALIBRATION>

Point3 gtsam::triangulateNonlinear	(	const std::vector< Pose3 > &	poses,
		std::shared_ptr< CALIBRATION >	sharedCal,
		const Point2Vector &	measurements,
		const Point3 &	initialEstimate,
		const SharedNoiseModel &	model = nullptr )

Given an initial estimate , refine a point using measurements in several cameras.

Parameters

poses	Camera poses
sharedCal	shared pointer to single calibration object
measurements	2D measurements
initialEstimate

Returns

refined Point3

triangulatePoint3() [1/2]

template<class CAMERA>

Point3 gtsam::triangulatePoint3	(	const CameraSet< CAMERA > &	cameras,
		const typename CAMERA::MeasurementVector &	measurements,
		double	rank_tol = 1e-9,
		bool	optimize = false,
		const SharedNoiseModel &	model = nullptr,
		const bool	useLOST = false )

Function to triangulate 3D landmark point from an arbitrary number of poses (at least 2) using the DLT.

This function is similar to the one above, except that each camera has its own calibration. The function checks that the resulting point lies in front of all cameras, but has no other checks to verify the quality of the triangulation.

Parameters

cameras	pinhole cameras
measurements	A vector of camera measurements
rank_tol	rank tolerance, default 1e-9
optimize	Flag to turn on nonlinear refinement of triangulation
useLOST	whether to use the LOST algorithm instead of DLT

Returns

Returns a Point3

triangulatePoint3() [2/2]

template<class CALIBRATION>

Point3 gtsam::triangulatePoint3	(	const std::vector< Pose3 > &	poses,
		std::shared_ptr< CALIBRATION >	sharedCal,
		const Point2Vector &	measurements,
		double	rank_tol = 1e-9,
		bool	optimize = false,
		const SharedNoiseModel &	model = nullptr,
		const bool	useLOST = false )

Function to triangulate 3D landmark point from an arbitrary number of poses (at least 2) using the DLT.

The function checks that the resulting point lies in front of all cameras, but has no other checks to verify the quality of the triangulation.

Parameters

poses	A vector of camera poses
sharedCal	shared pointer to single calibration object
measurements	A vector of camera measurements
rank_tol	rank tolerance, default 1e-9
optimize	Flag to turn on nonlinear refinement of triangulation
useLOST	whether to use the LOST algorithm instead of DLT

Returns

Returns a Point3

triangulateSafe()

template<class CAMERA>

std::vector< TriangulationResult > gtsam::triangulateSafe	(	const CameraSet< CAMERA > &	cameras,
		const std::vector< std::map< size_t, typename CAMERA::Measurement > > &	tracks,
		const TriangulationParameters &	params )

Batch triangulation: triangulate multiple (possibly incomplete) tracks.

Each track is a map from camera index to 2D measurement. Cameras missing from a track are simply skipped, supporting incomplete visibility.

triangulationGraph() [1/2]

template<class CAMERA>

std::pair< NonlinearFactorGraph, Values > gtsam::triangulationGraph	(	const CameraSet< CAMERA > &	cameras,
		const typename CAMERA::MeasurementVector &	measurements,
		Key	landmarkKey,
		const Point3 &	initialEstimate,
		const SharedNoiseModel &	model = nullptr )

Create a factor graph with projection factors from pinhole cameras (each camera has a pose and calibration).

Parameters

cameras	pinhole cameras (monocular or stereo)
measurements	2D measurements
landmarkKey	to refer to landmark
initialEstimate

Returns

graph and initial values

triangulationGraph() [2/2]

template<class CALIBRATION>

std::pair< NonlinearFactorGraph, Values > gtsam::triangulationGraph	(	const std::vector< Pose3 > &	poses,
		std::shared_ptr< CALIBRATION >	sharedCal,
		const Point2Vector &	measurements,
		Key	landmarkKey,
		const Point3 &	initialEstimate,
		const SharedNoiseModel &	model = noiseModel::Unit::Create(2) )

Create a factor graph with projection factors from poses and one calibration.

Parameters

poses	Camera poses
sharedCal	shared pointer to single calibration object (monocular only!)
measurements	2D measurements
landmarkKey	to refer to landmark
initialEstimate

Returns

graph and initial values

undistortMeasurements() [1/2]

template<class CALIBRATION>

Point2Vector gtsam::undistortMeasurements	(	const CALIBRATION &	cal,
		const Point2Vector &	measurements )

Remove distortion for measurements so as if the measurements came from a pinhole camera.

Removes distortion but maintains the K matrix of the initial cal. Operates by calibrating using full calibration and uncalibrating with only the pinhole component of the calibration.

Template Parameters

CALIBRATION

Calibration type to use.

Parameters

cal	Calibration with which measurements were taken.
measurements	Vector of measurements to undistort.

Returns

measurements with the effect of the distortion of sharedCal removed.

undistortMeasurements() [2/2]

template<class CAMERA>

CAMERA::MeasurementVector gtsam::undistortMeasurements	(	const CameraSet< CAMERA > &	cameras,
		const typename CAMERA::MeasurementVector &	measurements )

Remove distortion for measurements so as if the measurements came from a pinhole camera.

Removes distortion but maintains the K matrix of the initial calibrations. Operates by calibrating using full calibration and uncalibrating with only the pinhole component of the calibration.

Template Parameters

CAMERA

Camera type to use.

Parameters

cameras	Cameras corresponding to each measurement.
measurements	Vector of measurements to undistort.

Returns

measurements with the effect of the distortion of the camera removed.

unzip()

template<typename L, typename T1, typename T2>

std::pair< DecisionTree< L, T1 >, DecisionTree< L, T2 > > gtsam::unzip

(

const DecisionTree< L, std::pair< T1, T2 > > &

input

)

unzip a DecisionTree with std::pair values.

Parameters

input

the DecisionTree with (T1,T2) values.

Returns

a pair of DecisionTree on T1 and T2, respectively.

updateInterpValues()

template<class PoseType>

Values gtsam::updateInterpValues	(	const NonlinearFactorGraph &	interp_graph,
		const Values &	values,
		const std::set< StateData > &	estim_states,
		const std::set< StateData > &	interp_states,
		const Vector	q_psd_diag,
		std::shared_ptr< InterpCovarianceMap >	covarianceMapOut = nullptr )

Update a Values with interpolated pose and velocity entries.

Given an interpolated factor graph and the current estimated Values, evaluate the interpolator to produce interpolated pose/velocity entries and merge them into a copy of values which is returned.

Template Parameters

PoseType

Pose type used by the interpolator.

Parameters

interp_graph	Factor graph containing interpolation metadata (borders).
values	Current estimated Values (outer states used as borders).
estim_states	Ordered set of estimated StateData used by the main solve.
interp_states	Ordered set of StateData entries to interpolate.
q_psd_diag	Diagonal PSD vector for the WNOA motion prior.
covarianceMapOut	Optional output pointer to receive per-interpolated-state covariances.

Returns

Values A copy of values updated with interpolated pose and velocity entries.

updateInterpValuesWithCovariance()

template<class PoseType>

std::pair< Values, InterpCovarianceMap > gtsam::updateInterpValuesWithCovariance	(	const NonlinearFactorGraph &	interp_graph,
		const Values &	values,
		const std::set< StateData > &	estim_states,
		const std::set< StateData > &	interp_states,
		const Vector	q_psd_diag )

Update Values with interpolated states and return covariances.

Convenience wrapper around updateInterpValues that always computes interpolation covariances and returns them alongside the updated Values.

Template Parameters

PoseType

Pose type used by the interpolator.

Parameters

interp_graph	Factor graph containing interpolation metadata (borders).
values	Current estimated Values (outer states used as borders).
estim_states	Ordered set of estimated StateData used by the main solve.
interp_states	Ordered set of StateData entries to interpolate.
q_psd_diag	Diagonal PSD vector for the WNOA motion prior.

Returns

std::pair<Values, InterpCovarianceMap> Updated values and per-interpolated-state covariance map keyed by variable Key.

weighted_eliminate()

GTSAM_EXPORT std::list< std::tuple< Vector, double, double > > gtsam::weighted_eliminate	(	Matrix &	A,
		Vector &	b,
		const Vector &	sigmas )

Imperative algorithm for in-place full elimination with weights and constraint handling.

Parameters

A	is a matrix to eliminate
b	is the rhs
sigmas	is a vector of the measurement standard deviation

Returns

list of r vectors, d and sigma

weightedPseudoinverse()

GTSAM_EXPORT std::pair< Vector, double > gtsam::weightedPseudoinverse	(	const Vector &	v,
		const Vector &	weights )

Weighted Householder solution vector, a.k.a., the pseudoinverse of the column NOTE: if any sigmas are zero (indicating a constraint) the pseudoinverse will be a selection vector, and the variance will be zero.

Parameters

v	is the first column of the matrix to solve
weights	is a vector of weights/precisions where w=1/(s*s)

Returns

a pair of the pseudoinverse of v and the associated precision/weight

writeBAL()

GTSAM_EXPORT bool gtsam::writeBAL	(	const std::string &	filename,
		const SfmData &	data )

This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure.

Parameters

filename	The name of the BAL file to write
data	SfM structure where the data is stored

Returns

true if the parsing was successful, false otherwise

writeBALfromValues()

GTSAM_EXPORT bool gtsam::writeBALfromValues	(	const std::string &	filename,
		const SfmData &	data,
		const Values &	values )

This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure and a value structure (measurements are the same as the SfM input data, while camera poses and values are read from Values).

Parameters

filename	The name of the BAL file to write
data	SfM structure where the data is stored
values	structure where the graph values are stored (values can be either Pose3 or PinholeCamera<Cal3Bundler> for the cameras, and should be Point3 for the 3D points). Note: assumes that the keys are "i" for pose i and "Symbol::('p',j)" for landmark j.

Returns

true if the parsing was successful, false otherwise

writeG2o()

GTSAM_EXPORT void gtsam::writeG2o	(	const NonlinearFactorGraph &	graph,
		const Values &	estimate,
		const std::string &	filename )

This function writes a g2o file from NonlinearFactorGraph and a Values structure.

Parameters

filename	The name of the g2o file to write
graph	NonlinearFactor graph storing the measurements
estimate	Values

Note:behavior change in PR #471: to be consistent with load2D and load3D, we write the indices to file and not the full Keys. This change really only affects landmarks, which get read as indices but stored in values with the symbol L(index).

Variable Documentation

DefaultKeyFormatter

GTSAM_EXPORT KeyFormatter gtsam::DefaultKeyFormatter = &_defaultKeyFormatter

Assign default key formatter.

The default KeyFormatter, which is used if no KeyFormatter is passed to a 'print' function.

Automatically detects plain integer keys and Symbol keys.

Marked as extern so that it can be updated by external libraries.

MultiRobotKeyFormatter

GTSAM_EXPORT KeyFormatter gtsam::MultiRobotKeyFormatter = &_multirobotKeyFormatter

A KeyFormatter that will check for LabeledSymbol keys, as well as Symbol and plain integer keys.

This keyformatter will need to be passed in to override the default formatter in print functions.

Checks for LabeledSymbol, Symbol and then plain keys, in order.