gtsam Namespace Reference

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

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

Classes
struct	_ValuesConstKeyValuePair
struct	_ValuesKeyValuePair
class	AcceleratedPowerMethod
	Compute maximum Eigenpair with accelerated power method. More...
class	AcceleratingScenario
	Accelerating from an arbitrary initial state, with optional rotation. More...
class	ActiveSetSolver
	This class implements the active set algorithm for solving convex Programming problems. 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...
struct	additive_group_tag
class	AHRS
class	AHRSFactor
class	AlgebraicDecisionTree
	Algebraic Decision Trees fix the range to double Just has some nice constructors and some syntactic sugar TODO: consider eliminating this class altogether? More...
class	AllDiff
	General AllDiff constraint Returns 1 if values for all keys are different, 0 otherwise DiscreteFactors are all awkward in that they have to store two types of keys: for each variable we have a Key and an Key. More...
class	AntiFactor
class	Assignment
	An assignment from labels to value index (size_t). More...
class	AttitudeFactor
class	Basis
	CRTP Base class for function bases. More...
class	BatchFixedLagSmoother
class	BayesNet
	A BayesNet is a tree of conditionals, stored in elimination order. More...
class	BayesTree
class	BayesTreeCliqueBase
	This is the base class for BayesTree cliques. More...
struct	BayesTreeCliqueData
	store all the sizes More...
struct	BayesTreeCliqueStats
	clique statistics More...
class	BayesTreeOrphanWrapper
struct	Bearing
struct	Bearing< Pose2, T >
struct	Bearing< Pose3, Point3 >
struct	Bearing< Pose3, Pose3 >
struct	BearingFactor
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...
class	BearingRangeFactor
class	BetweenConstraint
	Binary between constraint - forces between to a given value This constraint requires the underlying type to a Lie type. More...
class	BetweenFactor
class	BetweenFactorEM
class	BiasedGPSFactor
class	BinaryAllDiff
	Binary AllDiff constraint Returns 1 if values for two keys are different, 0 otherwise DiscreteFactors are all awkward in that they have to store two types of keys: for each variable we have a Index and an Index. More...
struct	BinaryJacobianFactor
	A binary JacobianFactor specialization that uses fixed matrix math for speed. More...
class	BinaryMeasurement
class	BinarySumExpression
	A BinarySumExpression is a specialization of Expression that adds two expressions together It optimizes the Jacobian calculation for this specific case. More...
class	BlockJacobiPreconditioner
struct	BlockJacobiPreconditionerParameters
struct	BoundingConstraint1
struct	BoundingConstraint2
	Binary scalar inequality constraint, with a similar value() function to implement for specific systems. More...
class	BTree
class	Cal3
class	Cal3_S2
class	Cal3_S2Stereo
class	Cal3Bundler
class	Cal3DS2
class	Cal3DS2_Base
class	Cal3Fisheye
class	Cal3Unified
class	CalibratedCamera
struct	CameraProjectionMatrix
	Create a 3*4 camera projection matrix from calibration and pose. More...
class	CameraSet
	A set of cameras, all with their own calibration. More...
struct	CGState
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...
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...
struct	Chebyshev2Basis
	Basis of Chebyshev polynomials of the second kind. More...
class	CheiralityException
class	CholeskyFailed
	Indicate Cholesky factorization failure. More...
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	CombinedImuFactor
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	compose_key_visitor
class	ConcurrentBatchFilter
	A Levenberg-Marquardt Batch Filter that implements the Concurrent Filtering and Smoother interface. More...
class	ConcurrentBatchSmoother
	A Levenberg-Marquardt Batch Smoother that implements the Concurrent Filtering and Smoother interface. More...
class	ConcurrentFilter
	The interface for the 'Filter' 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...
class	ConcurrentIncrementalSmoother
	A Levenberg-Marquardt Batch Smoother that implements the Concurrent Filtering and Smoother interface. More...
class	ConcurrentMap
class	ConcurrentSmoother
	The interface for the 'Smoother' portion of the Concurrent Filtering and Smoother architecture. More...
class	Conditional
	TODO: Update comments. More...
class	ConjugateGradientParameters
	parameters for the conjugate gradient method More...
struct	const_selector
	Helper class that uses templates to select between two types based on whether TEST_TYPE is const or not. More...
struct	const_selector< BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST >
	Specialization for the non-const version. More...
struct	const_selector< const BASIC_TYPE, BASIC_TYPE, AS_NON_CONST, AS_CONST >
	Specialization for the const version. More...
class	ConstantTwistScenario
	Scenario with constant twist 3D trajectory. More...
class	ConstantVelocityFactor
	Binary factor for applying a constant velocity model to a moving body represented as a NavState. More...
class	Constraint
	Base class for discrete probabilistic factors The most general one is the derived DecisionTreeFactor. More...
struct	ConstructorTraversalData
class	CRefCallAddCopy
	Helper. More...
class	CRefCallPushBack
	Helper. More...
class	CSP
	Constraint Satisfaction Problem class A specialization of a DiscreteFactorGraph. More...
class	CustomFactor
class	Cyclic
	Cyclic group of order N. More...
class	DecisionTree
	Decision Tree L = label for variables Y = function range (any algebra), e.g., bool, int, double. More...
class	DecisionTreeFactor
	A discrete probabilistic factor. 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...
struct	DeltaImpl
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...
struct	DGroundConstraint
	Ground constraint: forces the robot to be upright (no roll, pitch), a fixed height, and no velocity in z direction Dim: 4. More...
struct	DHeightPrior
	Forces the value of the height (z) in a PoseRTV to a specific value. More...
class	DirectProduct
class	DirectSum
	Template to construct the direct sum of two additive groups Assumes existence of three additive operators for both groups. More...
class	DiscreteBayesNet
	A Bayes net made from linear-Discrete densities. More...
class	DiscreteBayesTree
	A Bayes tree representing a Discrete density. More...
class	DiscreteBayesTreeClique
	A clique in a DiscreteBayesTree. More...
class	DiscreteConditional
	Discrete Conditional Density Derives from DecisionTreeFactor. More...
class	DiscreteEliminationTree
class	DiscreteEulerPoincareHelicopter
	Implement the Discrete Euler-Poincare' equation: More...
class	DiscreteFactor
	Base class for discrete probabilistic factors The most general one is the derived DecisionTreeFactor. More...
class	DiscreteFactorGraph
	A Discrete Factor Graph is a factor graph where all factors are Discrete, i.e. More...
class	DiscreteJunctionTree
struct	DiscreteKeys
	DiscreteKeys is a set of keys that can be assembled using the & operator. More...
class	DiscreteMarginals
	A class for computing marginals of variables in a DiscreteFactorGraph. More...
class	DoglegOptimizer
	This class performs Dogleg nonlinear optimization. More...
struct	DoglegOptimizerImpl
	This class contains the implementation of the Dogleg algorithm. More...
class	DoglegParams
	Parameters for Levenberg-Marquardt optimization. More...
class	Domain
	Domain restriction constraint. More...
struct	DRollPrior
	Forces the roll to a particular value - useful for flying robots Implied value is zero Dim: 1. More...
class	DSF
class	DSFBase
class	DSFMap
class	DSFVector
struct	Dummy
class	DummyFactor
class	DummyPreconditioner
struct	DummyPreconditionerParameters
class	DynamicValuesMismatched
class	EliminatableClusterTree
	A cluster-tree that eliminates to a Bayes tree. More...
class	EliminateableFactorGraph
	EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms. More...
struct	EliminationData
struct	EliminationTraits
	Traits class for eliminateable factor graphs, specifies the types that result from elimination, etc. More...
struct	EliminationTraits< DiscreteFactorGraph >
struct	EliminationTraits< GaussianFactorGraph >
struct	EliminationTraits< SymbolicFactorGraph >
class	EliminationTree
	An elimination tree is a data structure used intermediately during elimination. More...
class	EqualityFactorGraph
	Collection of all Linear Equality constraints Ax=b of a Programming problem as a Factor Graph. More...
struct	equals
	Template to create a binary predicate. More...
struct	equals_star
	Binary predicate on shared pointers. More...
class	EquivInertialNavFactor_GlobalVel
class	EquivInertialNavFactor_GlobalVel_NoBias
class	Errors
	vector of errors 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...
class	EssentialMatrixConstraint
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	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	Expression
	Expression class that supports automatic differentiation. More...
class	ExpressionFactor
	Factor that supports arbitrary expressions via AD. More...
class	ExpressionFactorGraph
	Factor graph that supports adding ExpressionFactors directly. More...
class	ExpressionFactorN
	N-ary variadic template for ExpressionFactor meant as a base class for N-ary factors. More...
class	ExtendedKalmanFilter
	This is a generic Extended Kalman Filter class implemented using nonlinear factors. More...
class	Factor
	This is the base class for all factor types. More...
class	FactorGraph
	A factor graph is a bipartite graph with factor nodes connected to variable nodes. More...
class	FastList
class	FastMap
class	FastSet
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...
struct	FixedDimension
	Give fixed size dimension of a type, fails at compile time if dynamic. More...
class	FixedLagSmoother
class	FixedVector
	Fixed size vectors - compatible with boost vectors, but with compile-type size checking. More...
class	FourierBasis
	Fourier basis. More...
class	FrobeniusBetweenFactor
	FrobeniusBetweenFactor is a BetweenFactor that evaluates the Frobenius norm of the rotation error between measured and predicted (rather than the Logmap of the error). More...
class	FrobeniusFactor
	FrobeniusFactor calculates the Frobenius norm between rotation matrices. More...
class	FrobeniusPrior
	FrobeniusPrior calculates the Frobenius norm between a given matrix and an element of SO(3) or SO(4). More...
class	FullIMUFactor
	Class that represents integrating IMU measurements over time for dynamic systems This factor has dimension 9, with a built-in constraint for velocity modeling. More...
class	FunctorizedFactor
	Factor which evaluates provided unary functor and uses the result to compute error with respect to the provided measurement. More...
class	FunctorizedFactor2
	Factor which evaluates provided binary functor and uses the result to compute error with respect to the provided measurement. More...
class	G_x1
	Helper class that computes the derivative of f w.r.t. More...
class	GaussianBayesNet
	A Bayes net made from linear-Gaussian densities. More...
class	GaussianBayesTree
	A Bayes tree representing a Gaussian density. More...
class	GaussianBayesTreeClique
	A clique in a GaussianBayesTree. More...
class	GaussianConditional
	A conditional Gaussian functions as the node in a Bayes network It has a set of parents y,z, etc. More...
class	GaussianDensity
	A Gaussian density. More...
class	GaussianEliminationTree
class	GaussianFactor
	An abstract virtual base class for JacobianFactor and HessianFactor. More...
class	GaussianFactorGraph
	A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. More...
class	GaussianFactorGraphSystem
	System class needed for calling preconditionedConjugateGradient. More...
class	GaussianISAM
class	GaussianJunctionTree
class	GaussMarkov1stOrderFactor
class	GaussNewtonOptimizer
	This class performs Gauss-Newton nonlinear optimization. More...
class	GaussNewtonParams
	Parameters for Gauss-Newton optimization, inherits from NonlinearOptimizationParams. More...
class	GeneralSFMFactor
class	GeneralSFMFactor2
	Non-linear factor for a constraint derived from a 2D measurement. More...
class	GenericProjectionFactor
class	GenericStereoFactor
class	GenericValue
	Wraps any type T so it can play as a Value. More...
class	GncOptimizer
class	GncParams
class	GPSFactor
class	GPSFactor2
struct	GraphvizFormatting
	Formatting options when saving in GraphViz format using NonlinearFactorGraph::saveGraph. More...
struct	group_tag
	tag to assert a type is a group More...
struct	HasBearing
struct	HasRange
struct	HasTestablePrereqs
	Requirements on type to pass it to Testable template below. More...
class	HessianFactor
	A Gaussian factor using the canonical parameters (information form) More...
class	IMUFactor
	Class that represents integrating IMU measurements over time for dynamic systems Templated to allow for different key types, but variables all assumed to be PoseRTV. More...
class	ImuFactor
class	ImuFactor2
class	InconsistentEliminationRequested
	An inference algorithm was called with inconsistent arguments. More...
class	IncrementalFixedLagSmoother
	This is a base class for the various HMF2 implementations. More...
class	IndeterminantLinearSystemException
	Thrown when a linear system is ill-posed. More...
class	IndexPair
	Small utility class for representing a wrappable pairs of ints. More...
class	InequalityFactorGraph
	Collection of all Linear Inequality constraints Ax-b <= 0 of a Programming problem as a Factor Graph. More...
class	InertialNavFactor_GlobalVelocity
class	InfeasibleInitialValues
	An exception indicating that the provided initial value is infeasible Also used to inzdicatethat the noise model dimension passed into a JacobianFactor has a different dimensionality than the factor. More...
class	InfeasibleOrUnboundedProblem
struct	InitializePose3
class	InvalidArgumentThreadsafe
	Thread-safe invalid argument exception. More...
class	InvalidDenseElimination
class	InvalidMatrixBlock
	An exception indicating that a matrix block passed into a JacobianFactor has a different dimensionality than the factor. More...
class	InvalidNoiseModel
	An exception indicating that the noise model dimension passed into a JacobianFactor has a different dimensionality than the factor. 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	ISAM
	A Bayes tree with an update methods that implements the iSAM algorithm. More...
class	ISAM2
class	ISAM2BayesTree
class	ISAM2Clique
	Specialized Clique structure for ISAM2, incorporating caching and gradient contribution TODO: more documentation. More...
struct	ISAM2DoglegParams
struct	ISAM2GaussNewtonParams
class	ISAM2JunctionTree
struct	ISAM2Params
struct	ISAM2Result
struct	ISAM2UpdateParams
class	IsGroup
	Group Concept. More...
class	IsLieGroup
	Lie Group Concept. More...
class	IsTestable
class	IsVectorSpace
	Vector Space concept. 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...
class	JacobianFactorQ
	JacobianFactor for Schur complement that uses Q noise model. More...
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	JointMarginal
	A class to store and access a joint marginal, returned from Marginals::jointMarginalCovariance and Marginals::jointMarginalInformation. More...
class	JunctionTree
class	KalmanFilter
	Kalman Filter class. 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	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...
class	KeyInfo
	Handy data structure for iterative solvers. More...
struct	KeyInfoEntry
	Handy data structure for iterative solvers key to (index, dimension, start) More...
class	LabeledSymbol
	Customized version of gtsam::Symbol for multi-robot use. More...
class	LevenbergMarquardtOptimizer
	This class performs Levenberg-Marquardt nonlinear optimization. More...
class	LevenbergMarquardtParams
	Parameters for Levenberg-Marquardt optimization. More...
struct	lie_group_tag
	tag to assert a type is a Lie group 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...
class	Line3
class	LinearContainerFactor
	Dummy version of a generic linear factor to be injected into a nonlinear factor graph. More...
class	LinearCost
	This class defines a linear cost function c'x which is a JacobianFactor with only one row. More...
class	LinearEquality
	This class defines a linear equality constraints, inheriting JacobianFactor with the special Constrained noise model. More...
class	LinearInequality
	This class defines a linear inequality constraint Ax-b <= 0, inheriting JacobianFactor with the special Constrained noise model. More...
class	LinearizedGaussianFactor
	A base factor class for the Jacobian and Hessian linearized factors. More...
class	LinearizedHessianFactor
	A factor that takes a linear, Hessian factor and inserts it into a nonlinear graph. More...
class	LinearizedJacobianFactor
	A factor that takes a linear, Jacobian factor and inserts it into a nonlinear graph. More...
class	ListOfOneContainer
	A helper class that behaves as a container with one element, and works with boost::range. More...
class	LocalOrientedPlane3Factor
	Factor to measure a planar landmark from a given pose, with a given local linearization point. More...
struct	LP
	Data structure of a Linear Program. More...
class	LPInitSolver
	This LPInitSolver implements the strategy in Matlab: http://www.mathworks.com/help/optim/ug/linear-programming-algorithms.html#brozyzb-9 Solve for x and y: min y st Ax = b Cx - y <= d where y \in R, x \in R^n, and Ax = b and Cx <= d is the constraints of the original problem. More...
struct	LPPolicy
	Policy for ActivetSetSolver to solve Linear Programming. 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...
struct	MakeJacobian
	: meta-function to generate Jacobian More...
struct	MakeOptionalJacobian
	: meta-function to generate JacobianTA optional reference Used mainly by Expressions More...
struct	manifold_tag
	tag to assert a type is a manifold More...
class	ManifoldEvaluationFactor
	For a measurement value of type T i.e. More...
class	ManifoldPreintegration
	IMU pre-integration on NavSatet manifold. More...
class	MarginalizeNonleafException
	Thrown when requesting to marginalize out variables from ISAM2 that are not leaves. More...
class	Marginals
	A class for computing Gaussian marginals of variables in a NonlinearFactorGraph. More...
class	Mechanization_bRn2
class	MetisIndex
	The MetisIndex class converts a factor graph into the Compressed Sparse Row format for use in METIS algorithms. More...
class	MFAS
struct	multiplicative_group_tag
	Group operator syntax flavors. 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...
class	MultiProjectionFactor
class	NavState
	Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make this a Lie group, but it is a 9D manifold. More...
struct	needs_eigen_aligned_allocator
	A SFINAE trait to mark classes that need special alignment. More...
struct	needs_eigen_aligned_allocator< T, void_t< typename T::_eigen_aligned_allocator_trait > >
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	NoiseModelFactor1
	A convenient base class for creating your own NoiseModelFactor with 1 variable. More...
class	NoiseModelFactor2
	A convenient base class for creating your own NoiseModelFactor with 2 variables. More...
class	NoiseModelFactor3
	A convenient base class for creating your own NoiseModelFactor with 3 variables. More...
class	NoiseModelFactor4
	A convenient base class for creating your own NoiseModelFactor with 4 variables. More...
class	NoiseModelFactor5
	A convenient base class for creating your own NoiseModelFactor with 5 variables. More...
class	NoiseModelFactor6
	A convenient base class for creating your own NoiseModelFactor with 6 variables. More...
class	NoMatchFoundForFixed
class	NonlinearClusterTree
class	NonlinearConjugateGradientOptimizer
	An implementation of the nonlinear CG method using the template below. More...
class	NonlinearEquality
	An equality factor that forces either one variable to a constant, or a set of variables to be equal to each other. More...
class	NonlinearEquality1
	Simple unary equality constraint - fixes a value for a variable. More...
class	NonlinearEquality2
	Simple binary equality constraint - this constraint forces two variables to be the same. More...
class	NonlinearFactor
	Nonlinear factor base class. More...
class	NonlinearFactorGraph
	A non-linear factor graph is a graph of non-Gaussian, i.e. More...
class	NonlinearISAM
	Wrapper class to manage ISAM in a nonlinear context. 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	OdometryFactorBase
	OdometryFactorBase: Pose2 odometry, with Basenodes. 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 >
class	Ordering
class	ordering_key_visitor
class	OrientedPlane3
	Represents an infinite plane in 3D, which is composed of a planar normal and its perpendicular distance to the origin. More...
class	OrientedPlane3DirectionPrior
class	OrientedPlane3Factor
	Factor to measure a planar landmark from a given pose. More...
class	OutOfRangeThreadsafe
	Thread-safe out of range exception. More...
class	ParameterMatrix
	A matrix abstraction of MxN values at the Basis points. More...
class	PartialPriorFactor
	A class for a soft partial prior on any Lie type, with a mask over Expmap parameters. More...
class	PCGSolver
	A virtual base class for the preconditioned conjugate gradient solver. More...
struct	PCGSolverParameters
	Parameters for PCG. More...
class	PendulumFactor1
	This class implements the first constraint. More...
class	PendulumFactor2
	This class implements the second constraint the. More...
class	PendulumFactorPk
	This class implements the first position-momentum update rule p_k = -D_1 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)+mgrh(1-\alpha)\,\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) = (1/h)mr^2 (q_{k+1}-q_k) + mgrh(1-alpha) sin ((1-alpha)q_k+\alpha q_{k+1}) More...
class	PendulumFactorPk1
	This class implements the second position-momentum update rule p_k1 = D_2 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)-mgrh\alpha\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) = (1/h)mr^2 (q_{k+1}-q_k) - mgrh alpha sin ((1-alpha)q_k+\alpha q_{k+1}) More...
class	PinholeBase
class	PinholeBaseK
class	PinholeCamera
class	PinholePose
class	PinholeSet
	PinholeSet: triangulates point and keeps an estimate of it around. More...
class	Pose2
class	Pose3
class	Pose3AttitudeFactor
class	PoseBetweenFactor
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	PosePriorFactor
class	PoseRotationPrior
class	PoseRTV
	Robot state for use with IMU measurements. More...
class	PoseToPointFactor
class	PoseTranslationPrior
	A prior on the translation part of a pose. More...
class	Potentials
	A base class for both DiscreteFactor and DiscreteConditional. More...
class	PowerMethod
	Compute maximum Eigenpair with power method. More...
class	Preconditioner
struct	PreconditionerParameters
class	PredecessorMap
	Map from variable key to parent key. More...
class	PreintegratedAhrsMeasurements
	PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates) and the corresponding covariance matrix. More...
class	PreintegratedCombinedMeasurements
class	PreintegratedImuMeasurements
class	PreintegratedRotation
	PreintegratedRotation is the base class for all PreintegratedMeasurements classes (in AHRSFactor, ImuFactor, and CombinedImuFactor). More...
struct	PreintegratedRotationParams
	Parameters for pre-integration: Usage: Create just a single Params and pass a shared pointer to the constructor. More...
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	PriorFactor
class	ProductLieGroup
	Template to construct the product Lie group of two other Lie groups Assumes Lie group structure for G and H. More...
class	ProjectionFactorPPP
class	ProjectionFactorPPPC
class	ProjectionFactorRollingShutter
struct	QP
	Struct contains factor graphs of a Quadratic Programming problem. More...
class	QPInitSolver
	This class finds a feasible solution for a QP problem. More...
struct	QPPolicy
	Policy for ActivetSetSolver to solve Linear Programming. More...
class	QPSParser
class	QPSParserException
struct	Range
struct	Range< CalibratedCamera, T >
struct	Range< PinholeCamera< Calibration >, T >
struct	Range< Point2, Point2 >
struct	Range< Point3, Point3 >
struct	Range< Pose2, T >
struct	Range< Pose3, T >
struct	Range< PoseRTV, PoseRTV >
class	RangeFactor
class	RangeFactorWithTransform
class	Reconstruction
	Implement the Reconstruction equation: \( g_{k+1} = g_k \exp (h\xi_k) \), where \( h \): timestep (parameter) \( g_{k+1}, g_{k} \): poses at the current and the next timestep \( \xi_k \): the body-fixed velocity (Lie algebra) It is somewhat similar to BetweenFactor, but treats the body-fixed velocity \( \xi_k \) as a variable. More...
struct	RedirectCout
	For Python str(). More...
class	RefCallPushBack
	Helper. More...
class	ReferenceFrameFactor
	A constraint between two landmarks in separate maps Templated on: Point : Type of landmark Transform : Transform variable class. More...
class	RegularHessianFactor
class	RegularImplicitSchurFactor
	RegularImplicitSchurFactor. More...
class	RegularJacobianFactor
	JacobianFactor with constant sized blocks Provides raw memory access versions of linear operator. More...
class	RelativeElevationFactor
	Binary factor for a relative elevation. More...
struct	Reshape
	Reshape functor. More...
struct	Reshape< M, M, InOptions, M, M, InOptions >
	Reshape specialization that does nothing as shape stays the same (needed to not be ambiguous for square input equals square output) More...
struct	Reshape< M, N, InOptions, M, N, InOptions >
	Reshape specialization that does nothing as shape stays the same. More...
struct	Reshape< N, M, InOptions, M, N, InOptions >
	Reshape specialization that does transpose. More...
class	Rot2
class	Rot3
class	Rot3AttitudeFactor
class	RotateDirectionsFactor
	Factor on unknown rotation iRc that relates two directions c Directions provide less constraints than a full rotation. More...
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	RuntimeErrorThreadsafe
	Thread-safe runtime error exception. More...
class	Sampler
	Sampling structure that keeps internal random number generators for diagonal distributions specified by NoiseModel. 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	Scatter
	Scatter is an intermediate data structure used when building a HessianFactor incrementally, to get the keys in the right order. More...
class	Scenario
	Simple trajectory simulator. More...
class	ScenarioRunner
class	Scheduler
	Scheduler class Creates one variable for each student, and three variables for each of the student's areas, for a total of 4*nrStudents variables. More...
class	SDGraph
	SDGraph is undirected graph with variable keys and double edge weights. More...
struct	SfmData
	Define the structure for SfM data. More...
struct	SfmTrack
	Define the structure for the 3D points. More...
class	SGraph
class	ShonanAveraging
	Class that implements Shonan Averaging from our ECCV'20 paper. More...
class	ShonanAveraging2
class	ShonanAveraging3
struct	ShonanAveragingParameters
	Parameters governing optimization etc. More...
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	Signature
	Signature for a discrete conditional density, used to construct conditionals. More...
class	Similarity3
	3D similarity transform More...
class	SingleValue
	SingleValue constraint. More...
struct	SlotEntry
	One SlotEntry stores the slot index for a variable, as well its dim. More...
class	SmartFactorBase
	Base class for smart factors. More...
class	SmartProjectionFactor
	SmartProjectionFactor: triangulates point and keeps an estimate of it around. More...
struct	SmartProjectionParams
class	SmartProjectionPoseFactor
class	SmartProjectionPoseFactorRollingShutter
class	SmartProjectionRigFactor
class	SmartRangeFactor
class	SmartStereoProjectionFactor
	SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around. More...
class	SmartStereoProjectionFactorPP
class	SmartStereoProjectionPoseFactor
class	SO
	Manifold of special orthogonal rotation matrices SO<N>. More...
class	StereoCamera
class	StereoCheiralityException
class	StereoPoint2
struct	StreamedKey
	To use the key_formatter on Keys, they must be wrapped in a StreamedKey. More...
class	Subgraph
class	SubgraphBuilder
struct	SubgraphBuilderParameters
class	SubgraphPreconditioner
	Subgraph conditioner class, as explained in the RSS 2010 submission. More...
struct	SubgraphPreconditionerParameters
class	SubgraphSolver
	This class implements the linear SPCG solver presented in Dellaert et al in IROS'10. More...
struct	SubgraphSolverParameters
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...
class	SymbolicBayesNet
	Symbolic Bayes Net. More...
class	SymbolicBayesTree
	A Bayes tree that represents the connectivity between variables but is not associated with any probability functions. More...
class	SymbolicBayesTreeClique
	A clique in a SymbolicBayesTree. More...
class	SymbolicConditional
	SymbolicConditional is a conditional with keys but no probability data, produced by symbolic elimination of SymbolicFactor. More...
class	SymbolicEliminationTree
class	SymbolicFactor
	SymbolicFactor represents a symbolic factor that specifies graph topology but is not associated with any numerical function. More...
class	SymbolicFactorGraph
	Symbolic Factor Graph. More...
class	SymbolicISAM
class	SymbolicJunctionTree
class	SymmetricBlockMatrix
class	System
	Helper class encapsulating the combined system |Ax-b_|^2 Needed to run Conjugate Gradients on matrices. More...
class	TangentPreintegration
	Integrate on the 9D tangent space of the NavState manifold. More...
class	TbbOpenMPMixedScope
	An object whose scope defines a block where TBB and OpenMP parallelism are mixed. 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	TOAFactor
	A "Time of Arrival" factor - so little code seems hardly worth it :-) More...
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...
struct	traits< BearingFactor< A1, A2, T > >
	traits More...
struct	traits< BearingRange< A1, A2 > >
struct	traits< BearingRangeFactor< A1, A2, B, R > >
	traits More...
struct	traits< BetweenConstraint< VALUE > >
	traits More...
struct	traits< BetweenFactor< VALUE > >
	traits More...
struct	traits< BinaryJacobianFactor< M, N1, N2 > >
struct	traits< Cal3_S2 >
struct	traits< Cal3_S2Stereo >
struct	traits< Cal3Bundler >
struct	traits< Cal3DS2 >
struct	traits< Cal3Fisheye >
struct	traits< Cal3Unified >
struct	traits< CalibratedCamera >
struct	traits< CameraSet< CAMERA > >
struct	traits< CombinedImuFactor >
struct	traits< ConcurrentBatchFilter >
	traits More...
struct	traits< ConcurrentBatchSmoother >
	traits More...
struct	traits< ConcurrentIncrementalFilter >
	traits More...
struct	traits< ConcurrentIncrementalSmoother >
	traits More...
struct	traits< const Cal3_S2 >
struct	traits< const Cal3_S2Stereo >
struct	traits< const Cal3Bundler >
struct	traits< const Cal3DS2 >
struct	traits< const Cal3Fisheye >
struct	traits< const Cal3Unified >
struct	traits< const CalibratedCamera >
struct	traits< const CameraSet< CAMERA > >
struct	traits< const EssentialMatrix >
struct	traits< const Line3 >
struct	traits< const OrientedPlane3 >
struct	traits< const PinholeCamera< Calibration > >
struct	traits< const PinholePose< CALIBRATION > >
struct	traits< const PinholeSet< CAMERA > >
struct	traits< const Pose2 >
struct	traits< const Pose3 >
struct	traits< const Rot2 >
struct	traits< const Rot3 >
struct	traits< const Similarity3 >
struct	traits< const SO3 >
struct	traits< const SO4 >
struct	traits< const SO< N > >
struct	traits< const StereoCamera >
struct	traits< const StereoPoint2 >
struct	traits< const Unit3 >
struct	traits< Cyclic< N > >
	Define cyclic group to be a model of the Additive Group concept. More...
struct	traits< DecisionTreeFactor >
struct	traits< DirectProduct< G, H > >
struct	traits< DirectSum< G, H > >
struct	traits< DiscreteBayesNet >
struct	traits< DiscreteConditional >
struct	traits< DiscreteFactor >
struct	traits< DiscreteFactor::Values >
struct	traits< DiscreteFactorGraph >
	traits More...
struct	traits< double >
	double More...
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 > >
struct	traits< Eigen::Matrix< double, M, N, Options, MaxRows, MaxCols > >
struct	traits< EqualityFactorGraph >
	traits More...
struct	traits< Errors >
	traits More...
struct	traits< EssentialMatrix >
struct	traits< ExpressionFactor< T > >
	traits More...
struct	traits< ExpressionFactorN< T, Args... > >
	traits More...
struct	traits< float >
	float More...
struct	traits< FunctorizedFactor2< R, T1, T2 > >
	traits More...
struct	traits< FunctorizedFactor< R, T > >
	traits More...
struct	traits< GaussianBayesNet >
	traits More...
struct	traits< GaussianBayesTree >
	traits More...
struct	traits< GaussianConditional >
	traits More...
struct	traits< GaussianFactor >
	traits More...
struct	traits< GaussianFactorGraph >
	traits More...
struct	traits< GaussianISAM >
	traits More...
struct	traits< GaussMarkov1stOrderFactor< VALUE > >
	traits More...
struct	traits< GeneralSFMFactor2< CALIBRATION > >
struct	traits< GeneralSFMFactor< CAMERA, LANDMARK > >
struct	traits< GenericProjectionFactor< POSE, LANDMARK, CALIBRATION > >
	traits More...
struct	traits< GenericStereoFactor< T1, T2 > >
	traits More...
struct	traits< GenericValue< ValueType > >
struct	traits< HessianFactor >
	traits More...
struct	traits< imuBias::ConstantBias >
struct	traits< ImuFactor >
struct	traits< ImuFactor2 >
struct	traits< InequalityFactorGraph >
	traits More...
struct	traits< InertialNavFactor_GlobalVelocity< POSE, VELOCITY, IMUBIAS > >
	traits More...
struct	traits< ISAM2 >
	traits More...
struct	traits< JacobianFactor >
	traits More...
struct	traits< JacobianFactorQ< D, ZDim > >
struct	traits< Key >
struct	traits< LabeledSymbol >
	traits More...
struct	traits< Line3 >
struct	traits< LinearContainerFactor >
struct	traits< LinearCost >
	traits More...
struct	traits< LinearEquality >
	traits More...
struct	traits< LinearInequality >
	traits More...
struct	traits< LinearizedHessianFactor >
	traits More...
struct	traits< LinearizedJacobianFactor >
	traits More...
struct	traits< LP >
	traits More...
struct	traits< NavState >
struct	traits< noiseModel::Constrained >
struct	traits< noiseModel::Diagonal >
struct	traits< noiseModel::Gaussian >
	traits More...
struct	traits< noiseModel::Isotropic >
struct	traits< noiseModel::Unit >
struct	traits< NonlinearEquality1< VALUE > >
struct	traits< NonlinearEquality2< VALUE > >
struct	traits< NonlinearEquality< VALUE > >
struct	traits< NonlinearFactor >
	traits More...
struct	traits< NonlinearFactorGraph >
	traits More...
struct	traits< Ordering >
	traits More...
struct	traits< OrientedPlane3 >
struct	traits< ParameterMatrix< M > >
struct	traits< PinholeCamera< Calibration > >
struct	traits< PinholePose< CALIBRATION > >
struct	traits< PinholeSet< CAMERA > >
struct	traits< Pose2 >
struct	traits< Pose3 >
struct	traits< Pose3AttitudeFactor >
	traits More...
struct	traits< PoseRTV >
struct	traits< Potentials >
struct	traits< Potentials::ADT >
struct	traits< PreintegratedCombinedMeasurements >
struct	traits< PreintegratedImuMeasurements >
struct	traits< PreintegratedRotation >
struct	traits< PreintegrationCombinedParams >
struct	traits< PriorFactor< VALUE > >
	traits More...
struct	traits< ProductLieGroup< G, H > >
struct	traits< ProjectionFactorPPP< POSE, LANDMARK, CALIBRATION > >
	traits More...
struct	traits< ProjectionFactorPPPC< POSE, LANDMARK, CALIBRATION > >
	traits More...
struct	traits< ProjectionFactorRollingShutter >
	traits More...
struct	traits< QUATERNION_TYPE >
struct	traits< RangeFactor< A1, A2, T > >
	traits More...
struct	traits< RangeFactorWithTransform< A1, A2, T > >
	traits More...
struct	traits< ReferenceFrameFactor< T1, T2 > >
	traits More...
struct	traits< RegularHessianFactor< D > >
struct	traits< RegularImplicitSchurFactor< CAMERA > >
struct	traits< Rot2 >
struct	traits< Rot3 >
struct	traits< Rot3AttitudeFactor >
	traits More...
struct	traits< SfmData >
	traits More...
struct	traits< SfmTrack >
	traits More...
struct	traits< Similarity3 >
struct	traits< SmartProjectionFactor< CAMERA > >
	traits More...
struct	traits< SmartProjectionPoseFactor< CALIBRATION > >
	traits More...
struct	traits< SmartProjectionPoseFactorRollingShutter< CAMERA > >
	traits More...
struct	traits< SmartProjectionRigFactor< CAMERA > >
	traits More...
struct	traits< SmartStereoProjectionFactor >
	traits More...
struct	traits< SmartStereoProjectionFactorPP >
	traits More...
struct	traits< SmartStereoProjectionPoseFactor >
	traits More...
struct	traits< SO3 >
struct	traits< SO4 >
struct	traits< SO< N > >
struct	traits< StereoCamera >
struct	traits< StereoPoint2 >
struct	traits< Symbol >
	traits More...
struct	traits< SymbolicBayesNet >
	traits More...
struct	traits< SymbolicBayesTree >
struct	traits< SymbolicBayesTreeClique >
	traits More...
struct	traits< SymbolicConditional >
	traits More...
struct	traits< SymbolicEliminationTree >
	traits More...
struct	traits< SymbolicFactor >
	traits More...
struct	traits< SymbolicFactorGraph >
	traits More...
struct	traits< TransformBtwRobotsUnaryFactor< VALUE > >
	traits More...
struct	traits< TransformBtwRobotsUnaryFactorEM< VALUE > >
	traits More...
struct	traits< Unit3 >
struct	traits< Values >
	traits More...
struct	traits< VariableIndex >
	traits More...
struct	traits< VariableSlots >
	traits More...
struct	traits< VectorValues >
	traits More...
class	TransformBtwRobotsUnaryFactor
class	TransformBtwRobotsUnaryFactorEM
class	TransformCovariance
	Functor for transforming covariance of T. More...
class	TranslationFactor
class	TranslationRecovery
class	TriangulationCheiralityException
	Exception thrown by triangulateDLT when landmark is behind one or more of the cameras. More...
class	TriangulationFactor
struct	TriangulationParameters
class	TriangulationResult
	TriangulationResult is an optional point, along with the reasons why it is invalid. More...
class	TriangulationUnderconstrainedException
	Exception thrown by triangulateDLT when SVD returns rank < 3. More...
class	Unit3
	Represents a 3D point on a unit sphere. More...
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	Value
	This is the base class for any type to be stored in Values. More...
class	ValueCloneAllocator
class	Values
	A non-templated config holding any types of Manifold-group elements. More...
struct	ValuesCastHelper
struct	ValuesCastHelper< const Value, CastedKeyValuePairType, KeyValuePairType >
struct	ValuesCastHelper< Value, CastedKeyValuePairType, KeyValuePairType >
class	ValuesIncorrectType
class	ValuesKeyAlreadyExists
class	ValuesKeyDoesNotExist
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	VariableIndex
	The VariableIndex class computes and stores the block column structure of a factor graph. More...
class	VariableSlots
	A combined factor is assembled as one block of rows for each component factor. More...
struct	vector_space_tag
	tag to assert a type is a vector space More...
class	VectorComponentFactor
	Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix of size (P, N) is equal to specified measurement at the same point, when using a pseudo-spectral parameterization. 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	VectorEvaluationFactor
	Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix of size (M, N) is equal to a vector-valued measurement at the same point, when using a pseudo-spectral parameterization. More...
class	VectorValues
	This class represents a collection of vector-valued variables associated each with a unique integer index. More...
class	VelocityConstraint
	Constraint to enforce dynamics between the velocities and poses, using a prediction based on a numerical integration flag. More...
class	VelocityConstraint3
struct	VelocityPrior
	Constrains the full velocity of a state to a particular value Useful for enforcing a stationary state Dim: 3. More...
class	VerticalBlockMatrix
class	WeightedSampler
class	WhiteNoiseFactor
	Binary factor to estimate parameters of zero-mean Gaussian white noise. 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 = std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type >
template<bool B, class T = void>
using	enable_if_t = typename std::enable_if< B, T >::type
	An shorthand alias for accessing the ::type inside std::enable_if that can be used in a template directly.
typedef Eigen::MatrixXd	Matrix
typedef Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor >	MatrixRowMajor
typedef Eigen::Block< Matrix >	SubMatrix
typedef Eigen::Block< const Matrix >	ConstSubMatrix
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<typename ... >
using	void_t = void
	Convenience void_t as we assume C++11, it will not conflict the std one in C++17 as this is in gtsam::
typedef Eigen::VectorXd	Vector
typedef Eigen::Matrix< double, 1, 1 >	Vector1
typedef Eigen::Vector2d	Vector2
typedef Eigen::Vector3d	Vector3
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.
typedef std::pair< Key, size_t >	DiscreteKey
	Key type for discrete conditionals Includes name and cardinality.
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 >
	Calculate pose between a vector of 2D point correspondences (p,q) where q = Pose2::transformFrom(p) = t + R*p.
using	Point2Pairs = std::vector< Point2Pair >
typedef std::vector< Point2, Eigen::aligned_allocator< Point2 > >	Point2Vector
typedef Vector3	Point3
	As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3.
using	Point3Pair = std::pair< Point3, Point3 >
using	Point3Pairs = std::vector< Point3Pair >
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	PinholeCameraCal3_S2 = gtsam::PinholeCamera< gtsam::Cal3_S2 >
	Convenient aliases for Pinhole camera classes with different calibrations. More...
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	CameraSetCal3Bundler = CameraSet< PinholeCamera< Cal3Bundler > >
using	CameraSetCal3_S2 = CameraSet< PinholeCamera< Cal3_S2 > >
using	CameraSetCal3Fisheye = CameraSet< PinholeCamera< Cal3Fisheye > >
using	CameraSetCal3Unified = CameraSet< PinholeCamera< Cal3Unified > >
typedef FastVector< FactorIndex >	FactorIndices
	Define collection types: More...
typedef FastSet< FactorIndex >	FactorIndexSet
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	SparseTriplets = std::vector< std::tuple< int, int, double > >
typedef noiseModel::Base::shared_ptr	SharedNoiseModel
	Note, deliberately not in noiseModel namespace. More...
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. More...
typedef ManifoldPreintegration	PreintegrationType
typedef Expression< NavState >	NavState_
typedef Expression< Velocity3 >	Velocity3_
typedef Vector3	Velocity3
	Velocity is currently typedef'd to Vector3. More...
using	JacobianVector = std::vector< Matrix >
using	CustomErrorFunction = std::function< Vector(const CustomFactor &, const Values &, const JacobianVector *)>
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 FastMap< char, Vector >	ISAM2ThresholdMap
typedef ISAM2ThresholdMap::value_type	ISAM2ThresholdMapValue
typedef NonlinearOptimizerParams	SuccessiveLinearizationParams
typedef std::map< std::pair< Key, Key >, double >	KeyPairDoubleMap
using	ShonanAveragingParameters2 = ShonanAveragingParameters< 2 >
using	ShonanAveragingParameters3 = ShonanAveragingParameters< 3 >
using	ShonanFactor2 = ShonanFactor< 2 >
using	ShonanFactor3 = ShonanFactor< 3 >
template<typename T >
using	Parser = std::function< boost::optional< T >(istream &is, const string &tag)>
using	BearingRange2D = BearingRange< Pose2, Point2 >
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 = std::pair< NonlinearFactorGraph::shared_ptr, Values::shared_ptr >
	Return type for load functions, which return a graph and initial values. More...
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.
typedef PinholeCamera< Cal3Bundler >	SfmCamera
	Define the structure for the camera poses.
using	BetweenFactorPose2s = std::vector< BetweenFactor< Pose2 >::shared_ptr >
using	BetweenFactorPose3s = std::vector< BetweenFactor< Pose3 >::shared_ptr >
using	BinaryMeasurementsUnit3 = std::vector< BinaryMeasurement< Unit3 > >
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< Cal3_S2 >	Cal3_S2_
typedef Expression< Cal3Bundler >	Cal3Bundler_
typedef std::map< Key, std::vector< size_t > >	KeyVectorMap
typedef std::map< Key, Rot3 >	KeyRotMap
typedef DSF< int >	DSFInt
typedef Eigen::RowVectorXd	RowVector
using	KeyDimMap = std::map< Key, size_t >
	Mapping between variable's key and its corresponding dimensionality.
using	LPSolver = ActiveSetSolver< LP, LPPolicy, LPInitSolver >
using	QPSolver = ActiveSetSolver< QP, QPPolicy, QPInitSolver >
typedef qi::grammar< boost::spirit::basic_istream_iterator< char > >	base_grammar
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 FixedLagSmoother::KeyTimestampMap	FixedLagSmootherKeyTimestampMap
	Typedef for matlab wrapping.
typedef FixedLagSmootherKeyTimestampMap::value_type	FixedLagSmootherKeyTimestampMapValue
typedef FixedLagSmoother::Result	FixedLagSmootherResult
typedef SmartProjectionParams	SmartStereoProjectionParams

Enumerations
enum	GncLossType { GM , TLS }
	Choice of robust loss function for GNC.
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. More...
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]. More...
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 >
	BOOST_CONCEPT_REQUIRES (((IsGroup< G >)),(bool)) check_group_invariants(const G &a
	Check invariants.
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. More...
template<class Class >
Vector	logmap_default (const Class &l0, const Class &lp)
	Log map centered at l0, s.t. More...
template<class Class >
Class	expmap_default (const Class &t, const Vector &d)
	Exponential map centered at l0, s.t. More...
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. More...
template<class T >
Matrix	wedge (const Vector &x)
	Declaration of wedge (see Murray94book) used to convert from n exponential coordinates to n*n element of the Lie algebra.
template<class T >
T	expm (const Vector &x, int K=7)
	Exponential map given exponential coordinates class T needs a wedge<> function and a constructor from Matrix. More...
template<typename T >
T	interpolate (const T &X, const T &Y, double t, typename MakeOptionalJacobian< T, T >::type Hx=boost::none, typename MakeOptionalJacobian< T, T >::type Hy=boost::none)
	Linear interpolation between X and Y by coefficient t. More...
template<typename T , typename ... Args>
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > >	make_shared (Args &&... args)
	Add our own make_shared as a layer of wrapping on boost::make_shared This solves the problem with the stock make_shared that custom alignment is not respected, causing SEGFAULTs at runtime, which is notoriously hard to debug. More...
template<typename T , typename ... Args>
gtsam::enable_if_t<!needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > >	make_shared (Args &&... args)
	Fall back to the boost version if no need for alignment.
template<typename T >
	BOOST_CONCEPT_REQUIRES (((IsTestable< T >)),(bool)) check_manifold_invariants(const T &a
	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
Vector	operator^ (const Matrix &A, const Vector &v)
	overload ^ for trans(A)*v We transpose the vectors for speed.
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. More...
Matrix	diag (const std::vector< Matrix > &Hs)
	Create a matrix with submatrices along its diagonal.
Vector	columnNormSquare (const Matrix &A)
pair< Matrix, Matrix >	qr (const Matrix &A)
	Householder QR factorization, Golub & Van Loan p 224, explicit version More...
list< boost::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. More...
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. More...
void	householder (Matrix &A, size_t k)
	Householder tranformation, zeros below diagonal. More...
Vector	backSubstituteLower (const Matrix &L, const Vector &b, bool unit=false)
	backSubstitute L*x=b More...
Vector	backSubstituteUpper (const Matrix &U, const Vector &b, bool unit=false)
	backSubstitute U*x=b More...
Vector	backSubstituteUpper (const Vector &b, const Matrix &U, bool unit=false)
	backSubstitute x'*U=b' More...
Matrix	stack (size_t nrMatrices,...)
	create a matrix by stacking other matrices Given a set of matrices: A1, A2, A3... More...
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 More...
Matrix	collect (size_t nrMatrices,...)
void	vector_scale_inplace (const Vector &v, Matrix &A, bool inf_mask=false)
	scales a matrix row or column by the values in a vector Arguments (Matrix, Vector) scales the columns, (Vector, Matrix) scales the rows More...
Matrix	vector_scale (const Vector &v, const Matrix &A, bool inf_mask)
Matrix	vector_scale (const Matrix &A, const Vector &v, bool inf_mask)
Matrix	LLt (const Matrix &A)
Matrix	RtR (const Matrix &A)
Matrix	cholesky_inverse (const Matrix &A)
	Return the inverse of a S.P.D. More...
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'. More...
boost::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. More...
Matrix	expm (const Matrix &A, size_t K=7)
	Numerical exponential map, naive approach, not industrial strength !!! More...
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. More...
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
template<class MATRIX >
MATRIX	prod (const MATRIX &A, const MATRIX &B)
	products using old-style format to improve compatibility
template<class MATRIX >
Eigen::Block< const MATRIX >	sub (const MATRIX &A, size_t i1, size_t i2, size_t j1, size_t j2)
	extract submatrix, slice semantics, i.e. More...
template<typename Derived1 , typename Derived2 >
void	insertSub (Eigen::MatrixBase< Derived1 > &fullMatrix, const Eigen::MatrixBase< Derived2 > &subMatrix, size_t i, size_t j)
	insert a submatrix IN PLACE at a specified location in a larger matrix NOTE: there is no size checking More...
template<class MATRIX >
const MATRIX::ConstColXpr	column (const MATRIX &A, size_t j)
	Extracts a column view from a matrix that avoids a copy. More...
template<class MATRIX >
const MATRIX::ConstRowXpr	row (const MATRIX &A, size_t j)
	Extracts a row view from a matrix that avoids a copy. More...
template<class MATRIX >
void	zeroBelowDiagonal (MATRIX &A, size_t cols=0)
	Zeros all of the elements below the diagonal of a matrix, in place. More...
Matrix	trans (const Matrix &A)
	static transpose function, just calls Eigen transpose member function
template<int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions>
Reshape< OutM, OutN, OutOptions, InM, InN, InOptions >::ReshapedType	reshape (const Eigen::Matrix< double, InM, InN, InOptions > &m)
Matrix3	skewSymmetric (double wx, double wy, double wz)
	skew symmetric matrix returns this: 0 -wz wy wz 0 -wx -wy wx 0 More...
template<class Derived >
Matrix3	skewSymmetric (const Eigen::MatrixBase< Derived > &w)
template<class X , int N = traits<X>::dimension>
Eigen::Matrix< double, N, 1 >	numericalGradient (std::function< double(const X &)> h, const X &x, double delta=1e-5)
	Numerically compute gradient of scalar function. More...
template<class Y , class X , int N = traits<X>::dimension>
internal::FixedSizeMatrix< Y, X >::type	numericalDerivative11 (std::function< Y(const X &)> h, const X &x, double delta=1e-5)
	New-style numerical derivatives using manifold_traits. More...
template<class Y , class X >
internal::FixedSizeMatrix< Y, X >::type	numericalDerivative11 (Y(*h)(const X &), const X &x, double delta=1e-5)
	use a raw C++ function pointer
template<class Y , class X1 , class X2 , int N = traits<X1>::dimension>
internal::FixedSizeMatrix< Y, X1 >::type	numericalDerivative21 (const std::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
	Compute numerical derivative in argument 1 of binary function. More...
template<class Y , class X1 , class X2 >
internal::FixedSizeMatrix< Y, X1 >::type	numericalDerivative21 (Y(*h)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
	use a raw C++ function pointer
template<class Y , class X1 , class X2 , int N = traits<X2>::dimension>
internal::FixedSizeMatrix< Y, X2 >::type	numericalDerivative22 (std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
	Compute numerical derivative in argument 2 of binary function. More...
template<class Y , class X1 , class X2 >
internal::FixedSizeMatrix< Y, X2 >::type	numericalDerivative22 (Y(*h)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
	use a raw C++ function pointer
template<class Y , class X1 , class X2 , class X3 , int N = traits<X1>::dimension>
internal::FixedSizeMatrix< Y, X1 >::type	numericalDerivative31 (std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Compute numerical derivative in argument 1 of ternary function. More...
template<class Y , class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< Y, X1 >::type	numericalDerivative31 (Y(*h)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
template<class Y , class X1 , class X2 , class X3 , int N = traits<X2>::dimension>
internal::FixedSizeMatrix< Y, X2 >::type	numericalDerivative32 (std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Compute numerical derivative in argument 2 of ternary function. More...
template<class Y , class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< Y, X2 >::type	numericalDerivative32 (Y(*h)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
template<class Y , class X1 , class X2 , class X3 , int N = traits<X3>::dimension>
internal::FixedSizeMatrix< Y, X3 >::type	numericalDerivative33 (std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Compute numerical derivative in argument 3 of ternary function. More...
template<class Y , class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< Y, X3 >::type	numericalDerivative33 (Y(*h)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
template<class Y , class X1 , class X2 , class X3 , class X4 , int N = traits<X1>::dimension>
internal::FixedSizeMatrix< Y, X1 >::type	numericalDerivative41 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> 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. More...
template<class Y , class X1 , class X2 , class X3 , class X4 >
internal::FixedSizeMatrix< Y, X1 >::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)
template<class Y , class X1 , class X2 , class X3 , class X4 , int N = traits<X2>::dimension>
internal::FixedSizeMatrix< Y, X2 >::type	numericalDerivative42 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> 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. More...
template<class Y , class X1 , class X2 , class X3 , class X4 >
internal::FixedSizeMatrix< Y, X2 >::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)
template<class Y , class X1 , class X2 , class X3 , class X4 , int N = traits<X3>::dimension>
internal::FixedSizeMatrix< Y, X3 >::type	numericalDerivative43 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> 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. More...
template<class Y , class X1 , class X2 , class X3 , class X4 >
internal::FixedSizeMatrix< Y, X3 >::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)
template<class Y , class X1 , class X2 , class X3 , class X4 , int N = traits<X4>::dimension>
internal::FixedSizeMatrix< Y, X4 >::type	numericalDerivative44 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> 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. More...
template<class Y , class X1 , class X2 , class X3 , class X4 >
internal::FixedSizeMatrix< Y, X4 >::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)
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , int N = traits<X1>::dimension>
internal::FixedSizeMatrix< Y, X1 >::type	numericalDerivative51 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> 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. More...
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 >
internal::FixedSizeMatrix< Y, X1 >::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)
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , int N = traits<X2>::dimension>
internal::FixedSizeMatrix< Y, X2 >::type	numericalDerivative52 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> 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. More...
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 >
internal::FixedSizeMatrix< Y, X2 >::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)
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , int N = traits<X3>::dimension>
internal::FixedSizeMatrix< Y, X3 >::type	numericalDerivative53 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> 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. More...
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 >
internal::FixedSizeMatrix< Y, X3 >::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)
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , int N = traits<X4>::dimension>
internal::FixedSizeMatrix< Y, X4 >::type	numericalDerivative54 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> 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. More...
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 >
internal::FixedSizeMatrix< Y, X4 >::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)
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , int N = traits<X5>::dimension>
internal::FixedSizeMatrix< Y, X5 >::type	numericalDerivative55 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> 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. More...
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 >
internal::FixedSizeMatrix< Y, X5 >::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)
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X1>::dimension>
internal::FixedSizeMatrix< Y, X1 >::type	numericalDerivative61 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> 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. More...
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 >
internal::FixedSizeMatrix< Y, X1 >::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)
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X2>::dimension>
internal::FixedSizeMatrix< Y, X2 >::type	numericalDerivative62 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> 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. More...
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 >
internal::FixedSizeMatrix< Y, X2 >::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)
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X3>::dimension>
internal::FixedSizeMatrix< Y, X3 >::type	numericalDerivative63 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> 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. More...
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 >
internal::FixedSizeMatrix< Y, X3 >::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)
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X4>::dimension>
internal::FixedSizeMatrix< Y, X4 >::type	numericalDerivative64 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> 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. More...
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 >
internal::FixedSizeMatrix< Y, X4 >::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)
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X5>::dimension>
internal::FixedSizeMatrix< Y, X5 >::type	numericalDerivative65 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> 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. More...
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 >
internal::FixedSizeMatrix< Y, X5 >::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)
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X6>::dimension>
internal::FixedSizeMatrix< Y, X6 >::type	numericalDerivative66 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> 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. More...
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 >
internal::FixedSizeMatrix< Y, X6 >::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)
template<class X >
internal::FixedSizeMatrix< X, X >::type	numericalHessian (std::function< double(const X &)> f, const X &x, double delta=1e-5)
	Compute numerical Hessian matrix. More...
template<class X >
internal::FixedSizeMatrix< X, X >::type	numericalHessian (double(*f)(const X &), const X &x, double delta=1e-5)
template<class X1 , class X2 >
internal::FixedSizeMatrix< X1, X2 >::type	numericalHessian212 (std::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5)
template<class X1 , class X2 >
internal::FixedSizeMatrix< X1, X2 >::type	numericalHessian212 (double(*f)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
template<class X1 , class X2 >
internal::FixedSizeMatrix< X1, X1 >::type	numericalHessian211 (std::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5)
template<class X1 , class X2 >
internal::FixedSizeMatrix< X1, X1 >::type	numericalHessian211 (double(*f)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
template<class X1 , class X2 >
internal::FixedSizeMatrix< X2, X2 >::type	numericalHessian222 (std::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5)
template<class X1 , class X2 >
internal::FixedSizeMatrix< X2, X2 >::type	numericalHessian222 (double(*f)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X1, X1 >::type	numericalHessian311 (std::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	Numerical Hessian for tenary functions.
template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X1, X1 >::type	numericalHessian311 (double(*f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X2, X2 >::type	numericalHessian322 (std::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X2, X2 >::type	numericalHessian322 (double(*f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X3, X3 >::type	numericalHessian333 (std::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X3, X3 >::type	numericalHessian333 (double(*f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X1, X2 >::type	numericalHessian312 (std::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X1, X3 >::type	numericalHessian313 (std::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X2, X3 >::type	numericalHessian323 (std::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X1, X2 >::type	numericalHessian312 (double(*f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X1, X3 >::type	numericalHessian313 (double(*f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
template<class X1 , class X2 , class X3 >
internal::FixedSizeMatrix< X2, X3 >::type	numericalHessian323 (double(*f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
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, double tol=0.0)
	Equals testing for basic types.
template<class V >
bool	assert_equal (const boost::optional< V > &expected, const boost::optional< V > &actual, double tol=1e-9)
	Comparisons for boost.optional objects that checks whether objects exist before comparing their values. More...
template<class V >
bool	assert_equal (const V &expected, const boost::optional< V > &actual, double tol=1e-9)
template<class V >
bool	assert_equal (const V &expected, const boost::optional< const V & > &actual, double tol=1e-9)
template<class V >
bool GTSAM_DEPRECATED	assert_equal (const std::vector< V > &expected, const std::vector< V > &actual, double tol=1e-9)
	Version of assert_equals to work with vectors. More...
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. More...
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_print2_ ()
void	tictoc_reset_ ()
std::string	demangle (const char *name)
	Pretty print Value type name. More...
	BOOST_CONCEPT_ASSERT ((boost::RandomAccessRangeConcept< ListOfOneContainer< int > >))
template<typename T >
ListOfOneContainer< T >	ListOfOne (const T &element)
	Factory function for ListOfOneContainer to enable ListOfOne(e) syntax.
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. More...
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	operator== (const Vector &vec1, const Vector &vec2)
	operator==()
bool	greaterThanOrEqual (const Vector &v1, const Vector &v2)
	Greater than or equal to operation returns true if all elements in v1 are greater than corresponding elements in v2.
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. More...
bool	assert_inequal (const Vector &vec1, const Vector &vec2, double tol=1e-9)
	Not the same, prints if error. More...
bool	assert_equal (const SubVector &vec1, const SubVector &vec2, double tol=1e-9)
	Same, prints if error. More...
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 More...
Vector	ediv_ (const Vector &a, const Vector &b)
	elementwise division, but 0/0 = 0, not inf More...
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. More...
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. More...
Vector	concatVectors (const std::list< Vector > &vs)
	concatenate Vectors
Vector	concatVectors (size_t nrVectors,...)
	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.
template<class V1 , class V2 >
double	inner_prod (const V1 &a, const V2 &b)
	compatibility version for ublas' inner_prod()
void GTSAM_DEPRECATED	scal (double alpha, Vector &x)
	BLAS Level 1 scal: x <- alpha*x. More...
template<class V1 , class V2 >
void GTSAM_DEPRECATED	axpy (double alpha, const V1 &x, V2 &y)
	BLAS Level 1 axpy: y <- alpha*x + y. More...
void	axpy (double alpha, const Vector &x, SubVector y)
template<size_t M>
Matrix	kroneckerProductIdentity (const Weights &w)
	Function for computing the kronecker product of the 1*N Weight vector w with the MxM identity matrix I efficiently. More...
template<int M>
std::ostream &	operator<< (std::ostream &os, const ParameterMatrix< M > &parameterMatrix)
template<typename L >
std::vector< Assignment< L > >	cartesianProduct (const std::vector< std::pair< L, size_t > > &keys)
	Get Cartesian product consisting all possible configurations. More...
template<typename Y , typename L >
DecisionTree< L, Y >	apply (const DecisionTree< L, Y > &f, const typename DecisionTree< L, Y >::Unary &op)
	free versions of apply
template<typename Y , typename L >
DecisionTree< L, Y >	apply (const DecisionTree< L, Y > &f, const DecisionTree< L, Y > &g, const typename DecisionTree< L, Y >::Binary &op)
std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr >	EliminateDiscrete (const DiscreteFactorGraph &factors, const Ordering &keys)
	Main elimination function for DiscreteFactorGraph.
DiscreteKeys	operator& (const DiscreteKey &key1, const DiscreteKey &key2)
	Create a list from two keys.
ostream &	operator<< (ostream &os, const Signature::Row &row)
ostream &	operator<< (ostream &os, const Signature::Table &table)
ostream &	operator<< (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"); Uses string parser, which requires BOOST 1.42 or higher.
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=boost::none, OptionalJacobian< 2, 2 > Dp=boost::none)
	Function which makes use of the Implicit Function Theorem to compute the Jacobians of calibrate using uncalibrate. More...
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 Cal3Fisheye &cal)
std::ostream &	operator<< (std::ostream &os, const Cal3Unified &cal)
ostream &	operator<< (ostream &os, const EssentialMatrix &E)
istream &	operator>> (istream &is, EssentialMatrix &E)
Line3	transformTo (const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose=boost::none, OptionalJacobian< 4, 4 > Dline=boost::none)
	Transform a line from world to camera frame. More...
double	norm2 (const Point2 &p, OptionalJacobian< 1, 2 > H=boost::none)
	Distance of the point from the origin, with Jacobian.
double	distance2 (const Point2 &p1, const Point2 &q, OptionalJacobian< 1, 2 > H1=boost::none, OptionalJacobian< 1, 2 > H2=boost::none)
	distance between two points
boost::optional< Point2 >	circleCircleIntersection (double R_d, double r_d, double tol)
list< Point2 >	circleCircleIntersection (Point2 c1, Point2 c2, boost::optional< Point2 > fh)
list< Point2 >	circleCircleIntersection (Point2 c1, double r1, Point2 c2, double r2, double tol=1e-9)
	Intersect 2 circles. More...
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=boost::none, OptionalJacobian< 1, 3 > H2=boost::none)
	distance between two points
double	norm3 (const Point3 &p, OptionalJacobian< 1, 3 > H=boost::none)
	Distance of the point from the origin, with Jacobian.
Point3	normalize (const Point3 &p, OptionalJacobian< 3, 3 > H=boost::none)
	normalize, with optional Jacobian
Point3	cross (const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H_p=boost::none, OptionalJacobian< 3, 3 > H_q=boost::none)
	cross product More...
double	dot (const Point3 &p, const Point3 &q, OptionalJacobian< 1, 3 > H_p=boost::none, OptionalJacobian< 1, 3 > H_q=boost::none)
	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)
boost::optional< Pose2 >	align (const vector< Point2Pair > &pairs)
template<>
Matrix	wedge< Pose2 > (const Vector &xi)
	specialization for pose2 wedge function (generic template in Lie.h)
boost::optional< Pose3 >	align (const Point3Pairs &baPointPairs)
std::ostream &	operator<< (std::ostream &os, const Pose3 &pose)
template<>
Matrix	wedge< Pose3 > (const Vector &xi)
	wedge for Pose3: More...
pair< Matrix3, Vector3 >	RQ (const Matrix3 &A, OptionalJacobian< 3, 9 > H=boost::none)
	[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'. More...
ostream &	operator<< (ostream &os, const Rot3 &R)
std::ostream &	operator<< (std::ostream &os, const Similarity3 &p)
template<>
Matrix	wedge< Similarity3 > (const Vector &xi)
template<class Archive >
void	serialize (Archive &ar, SO3 &R, const unsigned int)
	Serialization function.
GTSAM_EXPORT Matrix3	topLeft (const SO4 &Q, OptionalJacobian< 9, 6 > H=boost::none)
	Project to top-left 3*3 matrix. More...
GTSAM_EXPORT Matrix43	stiefel (const SO4 &Q, OptionalJacobian< 12, 6 > H=boost::none)
	Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) -> S \in St(3,4).
template<class Archive >
void	serialize (Archive &ar, SO4 &Q, const unsigned int)
	Serialization function.
template<class Archive >
void	serialize (Archive &ar, SOn &Q, const unsigned int file_version)
	Serialization function.
ostream &	operator<< (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. More...
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. More...
Point3	optimize (const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
	Optimize for triangulation. More...
template<class CALIBRATION >
std::pair< NonlinearFactorGraph, Values >	triangulationGraph (const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, Key landmarkKey, const Point3 &initialEstimate)
	Create a factor graph with projection factors from poses and one calibration. More...
template<class CAMERA >
std::pair< NonlinearFactorGraph, Values >	triangulationGraph (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements, Key landmarkKey, const Point3 &initialEstimate)
	Create a factor graph with projection factors from pinhole cameras (each camera has a pose and calibration) More...
template<class CALIBRATION >
Point3	triangulateNonlinear (const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, const Point3 &initialEstimate)
	Given an initial estimate , refine a point using measurements in several cameras. More...
template<class CAMERA >
Point3	triangulateNonlinear (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements, const Point3 &initialEstimate)
	Given an initial estimate , refine a point using measurements in several cameras. More...
template<class CALIBRATION >
Point3	triangulatePoint3 (const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, double rank_tol=1e-9, bool optimize=false)
	Function to triangulate 3D landmark point from an arbitrary number of poses (at least 2) using the DLT. More...
template<class CAMERA >
Point3	triangulatePoint3 (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements, double rank_tol=1e-9, bool optimize=false)
	Function to triangulate 3D landmark point from an arbitrary number of poses (at least 2) using the DLT. More...
template<class CALIBRATION >
Point3	triangulatePoint3 (const CameraSet< PinholeCamera< CALIBRATION > > &cameras, const Point2Vector &measurements, double rank_tol=1e-9, bool optimize=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
std::ostream &	operator<< (std::ostream &os, const Unit3 &pair)
template<class CLIQUE >
bool	check_sharedCliques (const std::pair< Key, typename BayesTree< CLIQUE >::sharedClique > &v1, const std::pair< Key, typename BayesTree< CLIQUE >::sharedClique > &v2)
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 >
boost::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. More...
template<class G , class Factor , class POSE , class KEY >
boost::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<< (ostream &os, const key_formatter &m)
ostream &	operator<< (ostream &os, const StreamedKey &streamedKey)
GTSAM_EXPORT std::ostream &	operator<< (std::ostream &os, const LabeledSymbol &symbol)
Key	mrsymbol (unsigned char c, unsigned char label, size_t j)
	Create a symbol key from a character, label and index, i.e. More...
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.
size_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. More...
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 >
V	preconditionedConjugateGradient (const S &system, const V &initial, const ConjugateGradientParameters &parameters)
double	dot (const Errors &a, const Errors &b)
	dot product
template<>
void	axpy (double alpha, const Errors &x, Errors &y)
	BLAS level 2 style.
void	print (const Errors &a, const std::string &s="Error")
	print with optional string
template<>
GTSAM_EXPORT void	axpy (double alpha, const Errors &x, Errors &y)
	BLAS level 2 style.
bool	hasConstraints (const GaussianFactorGraph &factors)
	Evaluates whether linear factors have any constrained noise models. More...
std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< HessianFactor > >	EliminateCholesky (const GaussianFactorGraph &factors, const Ordering &keys)
std::pair< boost::shared_ptr< GaussianConditional >, boost::shared_ptr< GaussianFactor > >	EliminatePreferCholesky (const GaussianFactorGraph &factors, const Ordering &keys)
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) More...
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: More...
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<< (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). More...
ostream &	operator<< (ostream &os, const PreconditionerParameters &p)
boost::shared_ptr< Preconditioner >	createPreconditioner (const boost::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<< (ostream &os, const Subgraph::Edge &edge)
ostream &	operator<< (ostream &os, const Subgraph &subgraph)
ostream &	operator<< (ostream &os, const SubgraphBuilderParameters &p)
GaussianFactorGraph::shared_ptr	buildFactorSubgraph (const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone)
	Select the factors in a factor graph according to the subgraph.
std::pair< GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr >	splitFactorGraph (const GaussianFactorGraph &factorGraph, const Subgraph &subgraph)
	Split the graph into a subgraph and the remaining edges. More...
GTSAM_EXPORT ostream &	operator<< (ostream &os, const VectorValues &v)
VectorValues	operator* (const double a, const VectorValues &v)
std::ostream &	operator<< (std::ostream &os, const CombinedImuFactor &f)
Rot3_	attitude (const NavState_ &X)
Point3_	position (const NavState_ &X)
Velocity3_	velocity (const NavState_ &X)
std::ostream &	operator<< (std::ostream &os, const ImuFactor &f)
std::ostream &	operator<< (std::ostream &os, const ImuFactor2 &f)
ostream &	operator<< (ostream &os, const NavState &state)
ostream &	operator<< (ostream &os, const PreintegrationBase &pim)
template<typename T >
Expression< T >	operator* (const Expression< T > &expression1, const Expression< T > &expression2)
	Construct a product expression, assumes T::compose(T) -> T. More...
template<typename T >
std::vector< Expression< T > >	createUnknowns (size_t n, char c, size_t start)
	Construct an array of leaves. More...
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. More...
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)
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. More...
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. More...
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. More...
size_t	optimizeWildfire (const ISAM2Clique::shared_ptr &root, double threshold, const KeySet &replaced, VectorValues *delta)
	Optimize the BayesTree, starting from the root. More...
size_t	optimizeWildfireNonRecursive (const ISAM2Clique::shared_ptr &root, double threshold, const KeySet &keys, VectorValues *delta)
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 >
boost::tuple< V, int >	nonlinearConjugateGradient (const S &system, const V &initial, const NonlinearOptimizerParams &params, const bool singleIteration, 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. More...
bool	checkConvergence (double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity=NonlinearOptimizerParams::SILENT)
	Check whether the relative error decrease is less than relativeErrorTreshold, the absolute error decrease is less than absoluteErrorTreshold, or the error itself is less than errorThreshold.
GTSAM_EXPORT bool	checkConvergence (const NonlinearOptimizerParams &params, double currentError, double newError)
string	findExampleDataFile (const std::string &name)
	Find the full path to an example dataset distributed with gtsam. More...
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 >
map< size_t, T >	parseToMap (const string &filename, Parser< pair< size_t, T > > parse, size_t maxIndex)
boost::optional< IndexedPose >	parseVertexPose (std::istream &is, const std::string &tag)
	Parse TORO/G2O vertex "id x y yaw". More...
template<>
std::map< size_t, Pose2 >	parseVariables< Pose2 > (const std::string &filename, size_t maxIndex)
boost::optional< IndexedLandmark >	parseVertexLandmark (std::istream &is, const std::string &tag)
	Parse G2O landmark vertex "id x y". More...
template<>
std::map< size_t, Point2 >	parseVariables< Point2 > (const std::string &filename, size_t maxIndex)
boost::optional< IndexedEdge >	parseEdge (std::istream &is, const std::string &tag)
	Parse TORO/G2O edge "id1 id2 x y yaw". More...
boost::shared_ptr< Sampler >	createSampler (const SharedNoiseModel &model)
template<>
std::vector< BinaryMeasurement< Pose2 > >	parseMeasurements (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
template<>
std::vector< BinaryMeasurement< Rot2 > >	parseMeasurements (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
template<>
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. More...
GraphAndValues	load2D (pair< string, SharedNoiseModel > dataset, size_t maxIndex, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType)
	Load TORO 2D Graph. More...
GraphAndValues	load2D_robust (const 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. More...
void	writeG2o (const NonlinearFactorGraph &graph, const Values &estimate, const std::string &filename)
	This function writes a g2o file from NonlinearFactorGraph and a Values structure. More...
istream &	operator>> (istream &is, Quaternion &q)
istream &	operator>> (istream &is, Rot3 &R)
boost::optional< pair< size_t, Pose3 > >	parseVertexPose3 (istream &is, const string &tag)
template<>
std::map< size_t, Pose3 >	parseVariables< Pose3 > (const std::string &filename, size_t maxIndex)
boost::optional< pair< size_t, Point3 > >	parseVertexPoint3 (istream &is, const string &tag)
template<>
std::map< size_t, Point3 >	parseVariables< Point3 > (const std::string &filename, size_t maxIndex)
istream &	operator>> (istream &is, Matrix6 &m)
template<>
std::vector< BinaryMeasurement< Pose3 > >	parseMeasurements (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
template<>
std::vector< BinaryMeasurement< Rot3 > >	parseMeasurements (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex)
template<>
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 TORO 3D Graph.
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. More...
Pose3	gtsam2openGL (const Rot3 &R, double tx, double ty, double tz)
	This function converts a GTSAM camera pose to an openGL camera pose. More...
Pose3	gtsam2openGL (const Pose3 &PoseGTSAM)
	This function converts a GTSAM camera pose to an openGL camera pose. More...
bool	readBundler (const std::string &filename, SfmData &data)
	This function parses a bundler output file and stores the data into a SfmData structure. More...
bool	readBAL (const std::string &filename, SfmData &data)
	This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a SfmData structure. More...
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. More...
bool	writeBAL (const std::string &filename, SfmData &data)
	This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure. More...
bool	writeBALfromValues (const std::string &filename, const SfmData &data, 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) More...
Values	initialCamerasEstimate (const SfmData &db)
	This function creates initial values for cameras from db. More...
Values	initialCamerasAndPointsEstimate (const SfmData &db)
	This function creates initial values for cameras and points from db. More...
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. More...
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. More...
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. More...
GTSAM_EXPORT 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. More...
Point2_	transformTo (const Pose2_ &x, const Point2_ &p)
Point3_	transformTo (const Pose3_ &x, const Point3_ &p)
Point3_	transformFrom (const Pose3_ &x, const Point3_ &p)
Line3_	transformTo (const Pose3_ &wTc, const Line3_ &wL)
Point3_	cross (const Point3_ &a, const Point3_ &b)
Double_	dot (const Point3_ &a, const Point3_ &b)
Rot3_	rotation (const Pose3_ &pose)
Point3_	rotate (const Rot3_ &x, const Point3_ &p)
Unit3_	rotate (const Rot3_ &x, const Unit3_ &p)
Point3_	unrotate (const Rot3_ &x, const Point3_ &p)
Unit3_	unrotate (const Rot3_ &x, const Unit3_ &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<typename T >
gtsam::Expression< typename gtsam::traits< T >::TangentVector >	logmap (const gtsam::Expression< T > &x1, const gtsam::Expression< T > &x2)
	logmap
SharedNoiseModel	ConvertNoiseModel (const SharedNoiseModel &model, size_t n, bool defaultToUnit=true)
	When creating (any) FrobeniusFactor we can convert a Rot/Pose BetweenFactor noise model into a n-dimensional isotropic noise model used to weight the Frobenius norm. More...
template<class T , class ALLOC >
T	FindKarcherMeanImpl (const vector< T, ALLOC > &rotations)
template<class T , typename = typename std::enable_if< std::is_same<gtsam::Rot3, T>::value >::type>
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 rotations, by constructing a factor graph out of simple PriorFactors.
template<class T >
T	FindKarcherMean (std::initializer_list< T > &&rotations)
template<class T , class P >
P	transform_point (const T &trans, const P &global, boost::optional< Matrix & > Dtrans, boost::optional< Matrix & > Dglobal)
	Transform function that must be specialized specific domains. More...
std::pair< boost::shared_ptr< SymbolicConditional >, boost::shared_ptr< SymbolicFactor > >	EliminateSymbolic (const SymbolicFactorGraph &factors, const Ordering &keys)
	Dense elimination function for symbolic factors. More...
template<class PROBLEM >
Key	maxKey (const PROBLEM &problem)
	Find the max key in a problem. More...
template<class LinearGraph >
KeyDimMap	collectKeyDim (const LinearGraph &linearGraph)
void	synchronize (ConcurrentFilter &filter, ConcurrentSmoother &smoother)
void	recursiveMarkAffectedKeys (const Key &key, const ISAM2Clique::shared_ptr &clique, std::set< Key > &additionalKeys)
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")
double	bound (double a, double min, double max)
Standard serialization
Serialization in default compressed format
template<class T >
void	serializeToStream (const T &input, std::ostream &out_archive_stream)
template<class T >
void	deserializeFromStream (std::istream &in_archive_stream, T &output)
	deserializes from a stream
template<class T >
std::string	serializeToString (const T &input)
	serializes to a string
template<class T >
void	deserializeFromString (const std::string &serialized, T &output)
	deserializes from a string
template<class T >
bool	serializeToFile (const T &input, const std::string &filename)
	serializes to a file
template<class T >
bool	deserializeFromFile (const std::string &filename, T &output)
	deserializes from a file
template<class T >
std::string	serialize (const T &input)
	serializes to a string
template<class T >
void	deserialize (const std::string &serialized, T &output)
	deserializes from a string
XML Serialization
Serialization to XML format with named structures
template<class T >
void	serializeToXMLStream (const T &input, std::ostream &out_archive_stream, const std::string &name="data")
template<class T >
void	deserializeFromXMLStream (std::istream &in_archive_stream, T &output, const std::string &name="data")
	deserializes from a stream in XML
template<class T >
std::string	serializeToXMLString (const T &input, const std::string &name="data")
	serializes to a string in XML
template<class T >
void	deserializeFromXMLString (const std::string &serialized, T &output, const std::string &name="data")
	deserializes from a string in XML
template<class T >
bool	serializeToXMLFile (const T &input, const std::string &filename, const std::string &name="data")
	serializes to an XML file
template<class T >
bool	deserializeFromXMLFile (const std::string &filename, T &output, const std::string &name="data")
	deserializes from an XML file
template<class T >
std::string	serializeXML (const T &input, const std::string &name="data")
	serializes to a string in XML
template<class T >
void	deserializeXML (const std::string &serialized, T &output, const std::string &name="data")
	deserializes from a string in XML
Binary Serialization
Serialization to binary format with named structures
template<class T >
void	serializeToBinaryStream (const T &input, std::ostream &out_archive_stream, const std::string &name="data")
template<class T >
void	deserializeFromBinaryStream (std::istream &in_archive_stream, T &output, const std::string &name="data")
	deserializes from a stream in binary
template<class T >
std::string	serializeToBinaryString (const T &input, const std::string &name="data")
	serializes to a string in binary
template<class T >
void	deserializeFromBinaryString (const std::string &serialized, T &output, const std::string &name="data")
	deserializes from a string in binary
template<class T >
bool	serializeToBinaryFile (const T &input, const std::string &filename, const std::string &name="data")
	serializes to a binary file
template<class T >
bool	deserializeFromBinaryFile (const std::string &filename, T &output, const std::string &name="data")
	deserializes from a binary file
template<class T >
std::string	serializeBinary (const T &input, const std::string &name="data")
	serializes to a string in binary
template<class T >
void	deserializeBinary (const std::string &serialized, T &output, const std::string &name="data")
	deserializes from a string in binary
utility functions
VectorValues	buildVectorValues (const Vector &v, const Ordering &ordering, const std::map< Key, size_t > &dimensions)
	Create VectorValues from a Vector.
VectorValues	buildVectorValues (const Vector &v, const KeyInfo &keyInfo)
	Create VectorValues from a Vector and a KeyInfo class.

Variables
GTSAM_EXPORT FastMap< std::string, ValueWithDefault< bool, false > >	debugFlags
const G &	b
const G double	tol
template class GTSAM_EXPORT	Conditional< DecisionTreeFactor, DiscreteConditional >
const double	logSqrt2PI = log(std::sqrt(2.0 * M_PI))
	constant needed below

Detailed Description

Global functions in a separate testing namespace.

triangulationFactor.h

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

March 2, 2014

Author

Frank Dellaert

Typedef Documentation

FactorIndices

typedef FastVector< FactorIndex > gtsam::FactorIndices

Define collection types:

Define collection type:

GraphAndValues

using gtsam::GraphAndValues = typedef 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).

PinholeCameraCal3_S2

using gtsam::PinholeCameraCal3_S2 = typedef gtsam::PinholeCamera<gtsam::Cal3_S2>

Convenient aliases for Pinhole camera classes with different calibrations.

Also needed as forward declarations in the wrapper.

SharedNoiseModel

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

Note, deliberately not in noiseModel namespace.

Deprecated. Only for compatibility with previous version.

SparseEigen

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

Eigen-format sparse matrix.

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

Velocity3

typedef Vector3 gtsam::Velocity3

Velocity is currently typedef'd to Vector3.

Syntactic sugar to clarify components.

Enumeration Type Documentation

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.

Function Documentation

assert_equal() [1/4]

template<class V >

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

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

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

Concept requirement: V is testable

assert_equal() [2/4]

template<class V >

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

Version of assert_equals to work with vectors.

Deprecated:

: use container equals instead

assert_equal() [3/4]

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() [4/4]

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.

axpy()

template<class V1 , class V2 >

void GTSAM_DEPRECATED gtsam::axpy ( double  alpha, const V1 &  x, V2 &  y  )

inline

BLAS Level 1 axpy: y <- alpha*x + y.

Deprecated:

: use operators instead

backSubstituteLower()

GTSAM_EXPORT Vector gtsam::backSubstituteLower	(	const Matrix &	L,
		const Vector &	b,
		bool	unit = false
	)

backSubstitute L*x=b

Parameters

L	an lower triangular matrix
b	an RHS vector
unit,set	true if unit triangular

Returns

the solution x of L*x=b

backSubstituteUpper() [1/2]

GTSAM_EXPORT Vector gtsam::backSubstituteUpper	(	const Matrix &	U,
		const Vector &	b,
		bool	unit = false
	)

backSubstitute U*x=b

Parameters

U	an upper triangular matrix
b	an RHS vector
unit,set	true if unit triangular

Returns

the solution x of U*x=b

backSubstituteUpper() [2/2]

GTSAM_EXPORT Vector gtsam::backSubstituteUpper	(	const Vector &	b,
		const Matrix &	U,
		bool	unit = false
	)

backSubstitute x'*U=b'

Parameters

U	an upper triangular matrix
b	an RHS vector
unit,set	true if unit triangular

Returns

the solution x of x'*U=b'

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

calibrateJacobians()

template<typename Cal , size_t Dim>

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

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.

cartesianProduct()

template<typename L >

std::vector< Assignment< L > > gtsam::cartesianProduct

(

const std::vector< std::pair< L, size_t > > &

keys

)

Get Cartesian product consisting all possible configurations.

Parameters

vector

list of keys (label,cardinality) pairs.

Returns

vector list of all possible value assignments

This function returns a vector of Assignment values for all possible (Cartesian product) configurations of set of Keys which are nothing but (Label,cardinality) pairs. This function should NOT be called for more than a small number of variables and cardinalities. E.g. For 6 variables with each having cardinalities 4, we get 4096 possible configurations!!

cholesky_inverse()

GTSAM_EXPORT Matrix gtsam::cholesky_inverse

(

const Matrix &

A

)

Return the inverse of a S.P.D.

matrix. Inversion is done via Cholesky decomposition.

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.

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]

column()

template<class MATRIX >

const MATRIX::ConstColXpr gtsam::column	(	const MATRIX &	A,
		size_t	j
	)

Extracts a column view from a matrix that avoids a copy.

Parameters

A	matrix to extract column from
j	index of the column

Returns

a const view of the matrix

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

ConvertNoiseModel()

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

When creating (any) FrobeniusFactor we can convert a Rot/Pose BetweenFactor noise model into a n-dimensional isotropic noise model used to weight the Frobenius norm.

If the noise model passed is null we return a n-dimensional isotropic noise model with sigma=1.0. If not, we we check if the d-dimensional noise model on rotations is isotropic. If it is, we extend to 'n' dimensions, otherwise we throw an error. If the noise model is a robust error model, we use the sigmas of the underlying noise model.

If defaultToUnit == false throws an exception on unexepcted input.

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 = boost::none,
		OptionalJacobian< 3, 3 >	H_q = boost::none
	)

cross product

Returns

this 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())

DLT()

GTSAM_EXPORT boost::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')

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< boost::shared_ptr< GaussianConditional >, boost::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).

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

EliminateSymbolic()

GTSAM_EXPORT std::pair< boost::shared_ptr< SymbolicConditional >, boost::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.

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 wedge<> function and 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)

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 !!!

initialCamerasAndPointsEstimate()

GTSAM_EXPORT Values gtsam::initialCamerasAndPointsEstimate

(

const SfmData &

db

)

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

Parameters

SfmData

Returns

Values

initialCamerasEstimate()

GTSAM_EXPORT Values gtsam::initialCamerasEstimate

(

const SfmData &

db

)

This function creates initial values for cameras from db.

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

insertSub()

template<typename Derived1 , typename Derived2 >

void gtsam::insertSub	(	Eigen::MatrixBase< Derived1 > &	fullMatrix,
		const Eigen::MatrixBase< Derived2 > &	subMatrix,
		size_t	i,
		size_t	j
	)

insert a submatrix IN PLACE at a specified location in a larger matrix NOTE: there is no size checking

Parameters

fullMatrix	matrix to be updated
subMatrix	matrix to be inserted
i	is the row of the upper left corner insert location
j	is the column of the upper left corner insert location

interpolate()

template<typename T >

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

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.

kroneckerProductIdentity()

template<size_t M>

Matrix gtsam::kroneckerProductIdentity

(

const Weights &

w

)

Function for computing the kronecker product of the 1*N Weight vector w with the MxM identity matrix I efficiently.

The main reason for this is so we don't need to use Eigen's Unsupported library.

Template Parameters

M	Size of the identity matrix.

Parameters

w	The weights of the polynomial.

Returns

Mx(M*N) kronecker product [w(0)*I, w(1)*I, ..., w(N-1)*I]

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
	)

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/3]

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/3]

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

load2D() [3/3]

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

load2D_robust()

GTSAM_EXPORT GraphAndValues gtsam::load2D_robust	(	const std::string &	filename,
		const noiseModel::Base::shared_ptr &	model,
		size_t	maxIndex = 0
	)

Deprecated:

load2D now allows for arbitrary models and wrapping a robust kernel

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

make_shared()

template<typename T , typename ... Args>

gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > gtsam::make_shared

(

Args &&...

args

)

Add our own make_shared as a layer of wrapping on boost::make_shared This solves the problem with the stock make_shared that custom alignment is not respected, causing SEGFAULTs at runtime, which is notoriously hard to debug.

Explanation

The template needs_eigen_aligned_allocator<T>::value will evaluate to std::true_type if the type alias _eigen_aligned_allocator_trait = void is present in a class, which is automatically added by the GTSAM_MAKE_ALIGNED_OPERATOR_NEW macro.

This function declaration will only be taken when the above condition is true, so if some object does not need to be aligned, gtsam::make_shared will fall back to the next definition, which is a simple wrapper for boost::make_shared.

Template Parameters

T	The type of object being constructed
Args	Type of the arguments of the constructor

Parameters

args	Arguments of the constructor

Returns

The object created as a boost::shared_ptr<T>

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.

maxKey()

template<class PROBLEM >

Key gtsam::maxKey

(

const PROBLEM &

problem

)

Find the max key in a problem.

Useful to determine unique keys for additional slack variables

mrsymbol()

Key gtsam::mrsymbol ( unsigned char  c, unsigned char  label, size_t  j  )

inline

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

xA5.

nonlinearConjugateGradient()

template<class S , class V >

boost::tuple< V, int > gtsam::nonlinearConjugateGradient	(	const S &	system,
		const V &	initial,
		const NonlinearOptimizerParams &	params,
		const bool	singleIteration,
		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

numericalDerivative11()

template<class Y , class X , int N = traits<X>::dimension>

internal::FixedSizeMatrix< Y, X >::type gtsam::numericalDerivative11	(	std::function< Y(const X &)>	h,
		const X &	x,
		double	delta = 1e-5
	)

New-style numerical derivatives using manifold_traits.

Computes numerical derivative in argument 1 of unary function

Parameters

h	unary function yielding m-vector
x	n-dimensional value at which to evaluate h
delta	increment for numerical derivative Class Y is the output argument Class X is the input argument

Template Parameters

int	N is the dimension of the X input value if variable dimension type but known at test time

Returns

m*n Jacobian computed via central differencing

numericalDerivative21()

template<class Y , class X1 , class X2 , int N = traits<X1>::dimension>

internal::FixedSizeMatrix< Y, X1 >::type gtsam::numericalDerivative21	(	const std::function< Y(const X1 &, const X2 &)> &	h,
		const X1 &	x1,
		const X2 &	x2,
		double	delta = 1e-5
	)

Compute numerical derivative in argument 1 of binary function.

Parameters

h	binary function yielding m-vector
x1	n-dimensional first argument value
x2	second argument value
delta	increment for numerical derivative

Returns

m*n Jacobian computed via central differencing

Template Parameters

int	N is the dimension of the X1 input value if variable dimension type but known at test time

numericalDerivative22()

template<class Y , class X1 , class X2 , int N = traits<X2>::dimension>

internal::FixedSizeMatrix< Y, X2 >::type gtsam::numericalDerivative22	(	std::function< Y(const X1 &, const X2 &)>	h,
		const X1 &	x1,
		const X2 &	x2,
		double	delta = 1e-5
	)

Compute numerical derivative in argument 2 of binary function.

Parameters

h	binary function yielding m-vector
x1	first argument value
x2	n-dimensional second argument value
delta	increment for numerical derivative

Returns

m*n Jacobian computed via central differencing

Template Parameters

int	N is the dimension of the X2 input value if variable dimension type but known at test time

numericalDerivative31()

template<class Y , class X1 , class X2 , class X3 , int N = traits<X1>::dimension>

internal::FixedSizeMatrix< Y, X1 >::type gtsam::numericalDerivative31	(	std::function< Y(const X1 &, const X2 &, const X3 &)>	h,
		const X1 &	x1,
		const X2 &	x2,
		const X3 &	x3,
		double	delta = 1e-5
	)

Compute numerical derivative in argument 1 of ternary function.

Parameters

h	ternary function yielding m-vector
x1	n-dimensional first argument value
x2	second argument value
x3	third argument value
delta	increment for numerical derivative

Returns

m*n Jacobian computed via central differencing All classes Y,X1,X2,X3 need dim, expmap, logmap

Template Parameters

int	N is the dimension of the X1 input value if variable dimension type but known at test time

numericalDerivative32()

template<class Y , class X1 , class X2 , class X3 , int N = traits<X2>::dimension>

internal::FixedSizeMatrix< Y, X2 >::type gtsam::numericalDerivative32	(	std::function< Y(const X1 &, const X2 &, const X3 &)>	h,
		const X1 &	x1,
		const X2 &	x2,
		const X3 &	x3,
		double	delta = 1e-5
	)

Compute numerical derivative in argument 2 of ternary function.

Parameters

h	ternary function yielding m-vector
x1	n-dimensional first argument value
x2	second argument value
x3	third argument value
delta	increment for numerical derivative

Returns

m*n Jacobian computed via central differencing All classes Y,X1,X2,X3 need dim, expmap, logmap

Template Parameters

int	N is the dimension of the X2 input value if variable dimension type but known at test time

numericalDerivative33()

template<class Y , class X1 , class X2 , class X3 , int N = traits<X3>::dimension>

internal::FixedSizeMatrix< Y, X3 >::type gtsam::numericalDerivative33	(	std::function< Y(const X1 &, const X2 &, const X3 &)>	h,
		const X1 &	x1,
		const X2 &	x2,
		const X3 &	x3,
		double	delta = 1e-5
	)

Compute numerical derivative in argument 3 of ternary function.

Parameters

h	ternary function yielding m-vector
x1	n-dimensional first argument value
x2	second argument value
x3	third argument value
delta	increment for numerical derivative

Returns

m*n Jacobian computed via central differencing All classes Y,X1,X2,X3 need dim, expmap, logmap

Template Parameters

int	N is the dimension of the X3 input value if variable dimension type but known at test time

numericalDerivative41()

template<class Y , class X1 , class X2 , class X3 , class X4 , int N = traits<X1>::dimension>

internal::FixedSizeMatrix< Y, X1 >::type gtsam::numericalDerivative41	(	std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)>	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.

Parameters

h	quartic function yielding m-vector
x1	n-dimensional first argument value
x2	second argument value
x3	third argument value
x4	fourth argument value
delta	increment for numerical derivative

Returns

m*n Jacobian computed via central differencing

Template Parameters

int	N is the dimension of the X1 input value if variable dimension type but known at test time

numericalDerivative42()

template<class Y , class X1 , class X2 , class X3 , class X4 , int N = traits<X2>::dimension>

internal::FixedSizeMatrix< Y, X2 >::type gtsam::numericalDerivative42	(	std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)>	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.

Parameters

h	quartic function yielding m-vector
x1	first argument value
x2	n-dimensional second argument value
x3	third argument value
x4	fourth argument value
delta	increment for numerical derivative

Returns

m*n Jacobian computed via central differencing

Template Parameters

int	N is the dimension of the X2 input value if variable dimension type but known at test time

numericalDerivative43()

template<class Y , class X1 , class X2 , class X3 , class X4 , int N = traits<X3>::dimension>

internal::FixedSizeMatrix< Y, X3 >::type gtsam::numericalDerivative43	(	std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)>	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.

Parameters

h	quartic function yielding m-vector
x1	first argument value
x2	second argument value
x3	n-dimensional third argument value
x4	fourth argument value
delta	increment for numerical derivative

Returns

m*n Jacobian computed via central differencing

Template Parameters

int	N is the dimension of the X3 input value if variable dimension type but known at test time

numericalDerivative44()

template<class Y , class X1 , class X2 , class X3 , class X4 , int N = traits<X4>::dimension>

internal::FixedSizeMatrix< Y, X4 >::type gtsam::numericalDerivative44	(	std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)>	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.

Parameters

h	quartic function yielding m-vector
x1	first argument value
x2	second argument value
x3	third argument value
x4	n-dimensional fourth argument value
delta	increment for numerical derivative

Returns

m*n Jacobian computed via central differencing

Template Parameters

int	N is the dimension of the X4 input value if variable dimension type but known at test time

numericalDerivative51()

template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , int N = traits<X1>::dimension>

internal::FixedSizeMatrix< Y, X1 >::type gtsam::numericalDerivative51	(	std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)>	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.

Parameters

h	quintic function yielding m-vector
x1	n-dimensional first argument value
x2	second argument value
x3	third argument value
x4	fourth argument value
x5	fifth argument value
delta	increment for numerical derivative

Returns

m*n Jacobian computed via central differencing

Template Parameters

int	N is the dimension of the X1 input value if variable dimension type but known at test time

numericalDerivative52()

template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , int N = traits<X2>::dimension>

internal::FixedSizeMatrix< Y, X2 >::type gtsam::numericalDerivative52	(	std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)>	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.

Parameters

h	quintic function yielding m-vector
x1	n-dimensional first argument value
x2	second argument value
x3	third argument value
x4	fourth argument value
x5	fifth argument value
delta	increment for numerical derivative

Returns

m*n Jacobian computed via central differencing

Template Parameters

int	N is the dimension of the X2 input value if variable dimension type but known at test time

numericalDerivative53()

template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , int N = traits<X3>::dimension>

internal::FixedSizeMatrix< Y, X3 >::type gtsam::numericalDerivative53	(	std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)>	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.

Parameters

h	quintic function yielding m-vector
x1	n-dimensional first argument value
x2	second argument value
x3	third argument value
x4	fourth argument value
x5	fifth argument value
delta	increment for numerical derivative

Returns

m*n Jacobian computed via central differencing

Template Parameters

int	N is the dimension of the X3 input value if variable dimension type but known at test time

numericalDerivative54()

template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , int N = traits<X4>::dimension>

internal::FixedSizeMatrix< Y, X4 >::type gtsam::numericalDerivative54	(	std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)>	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.

Parameters

h	quintic function yielding m-vector
x1	n-dimensional first argument value
x2	second argument value
x3	third argument value
x4	fourth argument value
x5	fifth argument value
delta	increment for numerical derivative

Returns

m*n Jacobian computed via central differencing

Template Parameters

int	N is the dimension of the X4 input value if variable dimension type but known at test time

numericalDerivative55()

template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , int N = traits<X5>::dimension>

internal::FixedSizeMatrix< Y, X5 >::type gtsam::numericalDerivative55	(	std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)>	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.

Parameters

h	quintic function yielding m-vector
x1	n-dimensional first argument value
x2	second argument value
x3	third argument value
x4	fourth argument value
x5	fifth argument value
delta	increment for numerical derivative

Returns

m*n Jacobian computed via central differencing

Template Parameters

int	N is the dimension of the X5 input value if variable dimension type but known at test time

numericalDerivative61()

template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X1>::dimension>

internal::FixedSizeMatrix< Y, X1 >::type gtsam::numericalDerivative61	(	std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)>	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.

Parameters

h	quintic function yielding m-vector
x1	n-dimensional first argument value
x2	second argument value
x3	third argument value
x4	fourth argument value
x5	fifth argument value
x6	sixth argument value
delta	increment for numerical derivative

Returns

m*n Jacobian computed via central differencing

Template Parameters

int	N is the dimension of the X1 input value if variable dimension type but known at test time

numericalDerivative62()

template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X2>::dimension>

internal::FixedSizeMatrix< Y, X2 >::type gtsam::numericalDerivative62	(	std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)>	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.

Parameters

h	quintic function yielding m-vector
x1	n-dimensional first argument value
x2	second argument value
x3	third argument value
x4	fourth argument value
x5	fifth argument value
x6	sixth argument value
delta	increment for numerical derivative

Returns

m*n Jacobian computed via central differencing

Template Parameters

int	N is the dimension of the X2 input value if variable dimension type but known at test time

numericalDerivative63()

template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X3>::dimension>

internal::FixedSizeMatrix< Y, X3 >::type gtsam::numericalDerivative63	(	std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)>	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.

Parameters

h	quintic function yielding m-vector
x1	n-dimensional first argument value
x2	second argument value
x3	third argument value
x4	fourth argument value
x5	fifth argument value
x6	sixth argument value
delta	increment for numerical derivative

Returns

m*n Jacobian computed via central differencing

Template Parameters

int	N is the dimension of the X3 input value if variable dimension type but known at test time

numericalDerivative64()

template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X4>::dimension>

internal::FixedSizeMatrix< Y, X4 >::type gtsam::numericalDerivative64	(	std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)>	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.

Parameters

h	quintic function yielding m-vector
x1	n-dimensional first argument value
x2	second argument value
x3	third argument value
x4	fourth argument value
x5	fifth argument value
x6	sixth argument value
delta	increment for numerical derivative

Returns

m*n Jacobian computed via central differencing

Template Parameters

int	N is the dimension of the X4 input value if variable dimension type but known at test time

numericalDerivative65()

template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X5>::dimension>

internal::FixedSizeMatrix< Y, X5 >::type gtsam::numericalDerivative65	(	std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)>	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.

Parameters

h	quintic function yielding m-vector
x1	n-dimensional first argument value
x2	second argument value
x3	third argument value
x4	fourth argument value
x5	fifth argument value
x6	sixth argument value
delta	increment for numerical derivative

Returns

m*n Jacobian computed via central differencing

Template Parameters

int	N is the dimension of the X5 input value if variable dimension type but known at test time

numericalDerivative66()

template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X6>::dimension>

internal::FixedSizeMatrix< Y, X6 >::type gtsam::numericalDerivative66	(	std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)>	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.

Parameters

h	quintic function yielding m-vector
x1	n-dimensional first argument value
x2	second argument value
x3	third argument value
x4	fourth argument value
x5	fifth argument value
x6	sixth argument value
delta	increment for numerical derivative

Returns

m*n Jacobian computed via central differencing

Template Parameters

int	N is the dimension of the X6 input value if variable dimension type but known at test time

numericalGradient()

template<class X , int N = traits<X>::dimension>

Eigen::Matrix< double, N, 1 > gtsam::numericalGradient	(	std::function< double(const X &)>	h,
		const X &	x,
		double	delta = 1e-5
	)

Numerically compute gradient of scalar function.

Returns

n-dimensional gradient computed via central differencing Class X is the input argument The class X needs to have dim, expmap, logmap int N is the dimension of the X input value if variable dimension type but known at test time

numericalHessian()

template<class X >

internal::FixedSizeMatrix< X, X >::type gtsam::numericalHessian ( std::function< double(const X &)>  f, const X &  x, double  delta = 1e-5  )

inline

Compute numerical Hessian matrix.

Requires a single-argument Lie->scalar function. This is implemented simply as the derivative of the gradient.

Parameters

f	A function taking a Lie object as input and returning a scalar
x	The center point for computing the Hessian
delta	The numerical derivative step size

Returns

n*n Hessian matrix computed via central differencing

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>>()

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.

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 boost::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 boost::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 boost::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

predecessorMap2Graph()

template<class G , class V , class KEY >

boost::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.

readBAL()

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

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

Parameters

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

Returns

true if the parsing was successful, false otherwise

readBundler()

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

This function parses a bundler output file and stores the data into a SfmData structure.

Parameters

filename	The name of the bundler file
data	SfM structure where the data is stored

Returns

true if the parsing was successful, false otherwise

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

row()

template<class MATRIX >

const MATRIX::ConstRowXpr gtsam::row	(	const MATRIX &	A,
		size_t	j
	)

Extracts a row view from a matrix that avoids a copy.

Parameters

A	matrix to extract row from
j	index of the row

Returns

a const view of the matrix

RQ()

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

[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.

scal()

void GTSAM_DEPRECATED gtsam::scal ( double  alpha, Vector &  x  )

inline

BLAS Level 1 scal: x <- alpha*x.

Deprecated:

: use operators instead

serializeToBinaryStream()

template<class T >

void gtsam::serializeToBinaryStream	(	const T &	input,
		std::ostream &	out_archive_stream,
		const std::string &	name = "data"
	)

serializes to a stream in binary

serializeToStream()

template<class T >

void gtsam::serializeToStream	(	const T &	input,
		std::ostream &	out_archive_stream
	)

serializes to a stream

serializeToXMLStream()

template<class T >

void gtsam::serializeToXMLStream	(	const T &	input,
		std::ostream &	out_archive_stream,
		const std::string &	name = "data"
	)

serializes to a stream in XML

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< boost::shared_ptr< GaussianFactorGraph >, boost::shared_ptr< GaussianFactorGraph > > 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.

stack()

GTSAM_EXPORT Matrix gtsam::stack	(	size_t	nrMatrices,
			...
	)

create a matrix by stacking other matrices Given a set of matrices: A1, A2, A3...

Parameters

...	pointers to matrices to be stacked

Returns

combined matrix [A1; A2; A3]

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

sub()

template<class MATRIX >

Eigen::Block< const MATRIX > gtsam::sub	(	const MATRIX &	A,
		size_t	i1,
		size_t	i2,
		size_t	j1,
		size_t	j2
	)

extract submatrix, slice semantics, i.e.

range = [i1,i2[ excluding i2

Parameters

A	matrix
i1	first row index
i2	last row index + 1
j1	first col index
j2	last col index + 1

Returns

submatrix A(i1:i2-1,j1:j2-1)

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 = boost::none
	)

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,
		boost::optional< Matrix & >	Dtrans,
		boost::optional< Matrix & >	Dglobal
	)

Transform function that must be specialized specific domains.

Template Parameters

T	is a Transform type
P	is a point type

transformTo()

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

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()

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

triangulateNonlinear() [1/2]

template<class CAMERA >

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

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,
		boost::shared_ptr< CALIBRATION >	sharedCal,
		const Point2Vector &	measurements,
		const Point3 &	initialEstimate
	)

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
	)

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

Returns

Returns a Point3

triangulatePoint3() [2/2]

template<class CALIBRATION >

Point3 gtsam::triangulatePoint3	(	const std::vector< Pose3 > &	poses,
		boost::shared_ptr< CALIBRATION >	sharedCal,
		const Point2Vector &	measurements,
		double	rank_tol = 1e-9,
		bool	optimize = 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

Returns

Returns a Point3

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
	)

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,
		boost::shared_ptr< CALIBRATION >	sharedCal,
		const Point2Vector &	measurements,
		Key	landmarkKey,
		const Point3 &	initialEstimate
	)

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

vector_scale_inplace()

GTSAM_EXPORT void gtsam::vector_scale_inplace	(	const Vector &	v,
		Matrix &	A,
		bool	inf_mask = false
	)

scales a matrix row or column by the values in a vector Arguments (Matrix, Vector) scales the columns, (Vector, Matrix) scales the rows

Parameters

inf_mask

when true, will not scale with a NaN or inf value.

wedge< Pose3 >()

template<>

Matrix gtsam::wedge< Pose3 > ( const Vector &  xi )

inline

wedge for Pose3:

Parameters

xi	6-dim twist (omega,v) where omega = 3D angular velocity v = 3D velocity

Returns

xihat, 4*4 element of Lie algebra that can be exponentiated

weighted_eliminate()

GTSAM_EXPORT std::list< boost::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,
		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,
		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 that the current version assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1

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).

zeroBelowDiagonal()

template<class MATRIX >

void gtsam::zeroBelowDiagonal	(	MATRIX &	A,
		size_t	cols = 0
	)

Zeros all of the elements below the diagonal of a matrix, in place.

Parameters

A	is a matrix, to be modified in place
cols	is the number of columns to zero, use zero for all columns