gtsam Namespace Reference

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

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

Classes
class	ConcurrentMap
class	DerivedValue
class	DSFBase
class	DSFVector
class	FastList
class	FastMap
struct	FastSetTestableHelper
class	FastSet
struct	FastSetTestableHelper< VALUE, typename boost::enable_if< has_print< VALUE > >::type >
class	FastVector
class	GroupConcept
	This concept check enforces a Group structure on a variable type, in which we require the existence of basic algebraic operations. More...
class	LieConcept
	Concept check class for Lie group type. More...
struct	LieMatrix
	LieVector is a wrapper around vector to allow it to be a Lie type. More...
struct	LieScalar
	LieScalar is a wrapper around double to allow it to be a Lie type. More...
struct	LieVector
	LieVector is a wrapper around vector to allow it to be a Lie type. More...
class	ManifoldConcept
	Concept check class for Manifold types Requires a mapping between a linear tangent space and the underlying manifold, of which Lie is a specialization. More...
class	G_x1
	Helper class that computes the derivative of f w.r.t. More...
class	SymmetricBlockMatrix
class	CholeskyFailed
class	SymmetricBlockMatrixBlockExpr
	A matrix expression that references a single block of a SymmetricBlockMatrix. More...
class	TestableConcept
struct	equals
	Template to create a binary predicate. More...
struct	equals_star
	Binary predicate on shared pointers. 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...
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	ListOfOneContainer
	A helper class that behaves as a container with one element, and works with boost::range. More...
class	ThreadsafeException
	Base exception type that uses tbb_exception if GTSAM is compiled with TBB. More...
class	RuntimeErrorThreadsafe
	Threadsafe runtime error exception. More...
class	OutOfRangeThreadsafe
	Threadsafe runtime error exception. More...
class	InvalidArgumentThreadsafe
	Threadsafe invalid argument exception. More...
class	TbbOpenMPMixedScope
	An object whose scope defines a block where TBB and OpenMP parallelism are mixed. More...
class	Value
	This is the interface class for any value that may be used as a variable assignment in a factor graph, and which you must derive to create new variable types to use with gtsam. More...
class	VerticalBlockMatrix
class	Cal3_S2
class	Cal3_S2Stereo
class	Cal3Bundler
class	Cal3DS2
class	Cal3DS2_Base
class	Cal3Unified
class	CheiralityException
class	CalibratedCamera
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	RangeMeasurementConcept
	Range measurement concept Given a pair of Lie variables, there must exist a function to calculate range with derivatives. 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	PinholeCamera
class	Point2
class	Point3
class	Pose2
class	Pose3
class	Rot2
class	Rot3
class	StereoCheiralityException
class	StereoCamera
class	StereoPoint2
class	TriangulationUnderconstrainedException
	Exception thrown by triangulateDLT when SVD returns rank < 3. More...
class	TriangulationCheiralityException
	Exception thrown by triangulateDLT when landmark is behind one or more of the cameras. More...
class	TriangulationFactor
class	Unit3
	Represents a 3D point on a unit sphere. More...
class	BayesNet
	A BayesNet is a tree of conditionals, stored in elimination order. More...
class	FactorGraph
	A factor graph is a bipartite graph with factor nodes connected to variable nodes. More...
class	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...
struct	BayesTreeCliqueStats
	clique statistics More...
struct	BayesTreeCliqueData
	store all the sizes More...
class	BayesTree
class	BayesTreeOrphanWrapper
struct	EliminationTraits
	Traits class for eliminateable factor graphs, specifies the types that result from elimination, etc. More...
class	BayesTreeCliqueBase
	This is the base class for BayesTree cliques. More...
class	Conditional
	TODO: Update comments. More...
class	EliminateableFactorGraph
	EliminateableFactorGraph is a base class for factor graphs that contains elimination algorithms. More...
class	EliminationTree
	An elimination tree is a data structure used intermediately during elimination. More...
class	Factor
	This is the base class for all factor types. More...
class	CRefCallPushBack
	Helper. More...
class	RefCallPushBack
	Helper. More...
class	CRefCallAddCopy
	Helper. More...
class	ordering_key_visitor
class	compose_key_visitor
class	SDGraph
	SDGraph is undirected graph with variable keys and double edge weights. More...
class	SGraph
class	PredecessorMap
	Map from variable key to parent key. More...
class	InconsistentEliminationRequested
	An inference algorithm was called with inconsistent arguments. More...
class	ISAM
	A Bayes tree with an update methods that implements the iSAM algorithm. More...
class	JunctionTree
class	LabeledSymbol
	Customized version of gtsam::Symbol for multi-robot use. More...
class	Ordering
class	Symbol
	Character and index key used in VectorValues, GaussianFactorGraph, GaussianFactor, etc. 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...
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	Assignment
	An assignment from labels to value index (size_t). 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	DiscreteBayesNet
	A Bayes net made from linear-Discrete densities. More...
class	DiscreteBayesTreeClique
	A clique in a DiscreteBayesTree. More...
class	DiscreteBayesTree
	A Bayes tree representing a Discrete density. More...
class	DiscreteConditional
	Discrete Conditional Density Derives from DecisionTreeFactor. More...
class	DiscreteEliminationTree
class	DiscreteFactor
	Base class for discrete probabilistic factors The most general one is the derived DecisionTreeFactor. More...
struct	EliminationTraits< DiscreteFactorGraph >
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	Potentials
	A base class for both DiscreteFactor and DiscreteConditional. More...
class	Signature
	Signature for a discrete conditional density, used to construct conditionals. More...
class	ConjugateGradientParameters
	parameters for the conjugate gradient method More...
class	Errors
	vector of errors More...
class	GaussianBayesNet
	A Bayes net made from linear-Gaussian densities. More...
class	GaussianBayesTreeClique
	A clique in a GaussianBayesTree. More...
class	GaussianBayesTree
	A Bayes tree representing a Gaussian density. 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...
struct	EliminationTraits< GaussianFactorGraph >
class	GaussianFactorGraph
	A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e. More...
class	GaussianISAM
class	GaussianJunctionTree
struct	SlotEntry
	One SlotEntry stores the slot index for a variable, as well its dimension. 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	HessianFactor
	A Gaussian factor using the canonical parameters (information form) More...
struct	CGState
class	System
	Helper class encapsulating the combined system |Ax-b_|^2 Needed to run Conjugate Gradients on matrices. More...
class	IterativeOptimizationParameters
	parameters for iterative linear solvers More...
class	IterativeSolver
class	KeyInfoEntry
class	KeyInfo
class	JacobianFactor
	A Gaussian factor in the squared-error form. More...
class	KalmanFilter
	Kalman Filter class. More...
class	IndeterminantLinearSystemException
	Thrown when a linear system is ill-posed. More...
class	InvalidNoiseModel
	An exception indicating that the noise model dimension passed into a JacobianFactor has a different dimensionality than the factor. More...
class	InvalidMatrixBlock
	An exception indicating that a matrix block passed into a JacobianFactor has a different dimensionality than the factor. More...
class	InvalidDenseElimination
struct	PCGSolverParameters
class	PCGSolver
class	GaussianFactorGraphSystem
struct	PreconditionerParameters
class	Preconditioner
struct	DummyPreconditionerParameters
class	DummyPreconditioner
struct	BlockJacobiPreconditionerParameters
class	BlockJacobiPreconditioner
class	Sampler
	Sampling structure that keeps internal random number generators for diagonal distributions specified by NoiseModel. More...
struct	SubgraphEdge
class	Subgraph
struct	SubgraphBuilderParameters
class	SubgraphBuilder
struct	SubgraphPreconditionerParameters
class	SubgraphPreconditioner
	Subgraph conditioner class, as explained in the RSS 2010 submission. More...
class	SubgraphSolverParameters
class	SubgraphSolver
	This class implements the SPCG solver presented in Dellaert et al in IROS'10. More...
class	VectorValues
	This class represents a collection of vector-valued variables associated each with a unique integer index. More...
class	DoglegParams
	Parameters for Levenberg-Marquardt optimization. More...
class	DoglegState
	State for DoglegOptimizer. More...
class	DoglegOptimizer
	This class performs Dogleg nonlinear optimization. More...
struct	DoglegOptimizerImpl
	This class contains the implementation of the Dogleg algorithm. More...
class	ExtendedKalmanFilter
	This is a generic Extended Kalman Filter class implemented using nonlinear factors. More...
class	GaussNewtonParams
	Parameters for Gauss-Newton optimization, inherits from NonlinearOptimizationParams. More...
class	GaussNewtonState
class	GaussNewtonOptimizer
	This class performs Gauss-Newton nonlinear optimization. More...
class	ISAM2
struct	ISAM2GaussNewtonParams
struct	ISAM2DoglegParams
struct	ISAM2Params
struct	ISAM2Result
class	ISAM2Clique
	Specialized Clique structure for ISAM2, incorporating caching and gradient contribution TODO: more documentation. More...
class	LevenbergMarquardtParams
	Parameters for Levenberg-Marquardt optimization. More...
class	LevenbergMarquardtState
	State for LevenbergMarquardtOptimizer. More...
class	LevenbergMarquardtOptimizer
	This class performs Levenberg-Marquardt nonlinear optimization. More...
class	LinearContainerFactor
	Dummy version of a generic linear factor to be injected into a nonlinear factor graph. More...
class	Marginals
	A class for computing Gaussian marginals of variables in a NonlinearFactorGraph. More...
class	JointMarginal
	A class to store and access a joint marginal, returned from Marginals::jointMarginalCovariance and Marginals::jointMarginalInformation. More...
class	NonlinearConjugateGradientState
	An implementation of the nonlinear cg method using the template below. More...
class	NonlinearConjugateGradientOptimizer
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 factors to be the same. More...
class	MarginalizeNonleafException
	Thrown when requesting to marginalize out variables from ISAM2 that are not leaves. More...
class	NonlinearFactor
	Nonlinear factor base class. More...
class	NoiseModelFactor
	A nonlinear sum-of-squares factor with a zero-mean noise model implementing the density \( P(z|x) \propto exp -0.5*|z-h(x)|^2_C \) Templated on the parameter type X and the values structure Values There is no return type specified for h(x). More...
class	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...
struct	GraphvizFormatting
	Formatting options when saving in GraphViz format using NonlinearFactorGraph::saveGraph. 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	NonlinearOptimizerState
	Base class for a nonlinear optimization state, including the current estimate of the variable values, error, and number of iterations. 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...
struct	_ValuesKeyValuePair
struct	_ValuesConstKeyValuePair
class	ValueCloneAllocator
class	Values
	A non-templated config holding any types of Manifold-group elements. More...
class	ValuesKeyAlreadyExists
class	ValuesKeyDoesNotExist
class	ValuesIncorrectType
class	DynamicValuesMismatched
class	WhiteNoiseFactor
	Binary factor to estimate parameters of zero-mean Gaussian white noise. More...
class	AntiFactor
class	BearingFactor
class	BearingRangeFactor
class	BetweenFactor
class	BetweenConstraint
	Binary between constraint - forces between to a given value This constraint requires the underlying type to a Lie type. More...
struct	BoundingConstraint1
struct	BoundingConstraint2
	Binary scalar inequality constraint, with a similar value() function to implement for specific systems. More...
struct	SfM_Track
	Define the structure for the 3D points. More...
struct	SfM_data
	Define the structure for SfM data. 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	GeneralSFMFactor
class	GeneralSFMFactor2
	Non-linear factor for a constraint derived from a 2D measurement. More...
class	ImplicitSchurFactor
	ImplicitSchurFactor. 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 Q noise model. More...
class	JacobianSchurFactor
	JacobianFactor for Schur complement that uses Q noise model. More...
class	PoseRotationPrior
class	PoseTranslationPrior
	A prior on the translation part of a pose. More...
class	PriorFactor
class	GenericProjectionFactor
class	RangeFactor
class	ReferenceFrameFactor
	A constraint between two landmarks in separate maps Templated on: Point : Type of landmark Transform : Transform variable class. More...
class	RegularHessianFactor
class	RotateFactor
	Factor on unknown rotation iRC that relates two incremental rotations c1Rc2 = iRc' * i1Ri2 * iRc Which we can write (see doc/math.lyx) e^[z] = iRc' * e^[p] * iRc = e^([iRc'*p]) with z and p measured and predicted angular velocities, and hence p = iRc * z. More...
class	RotateDirectionsFactor
	Factor on unknown rotation R that relates two directions p_i = iRc * z_c Directions provide less constraints than a full rotation. More...
class	SmartFactorBase
	Base class with no internal point, completely functional. More...
class	SmartProjectionFactorState
class	SmartProjectionFactor
	SmartProjectionFactor: triangulates point TODO: why LANDMARK parameter? More...
class	SmartProjectionPoseFactor
class	GenericStereoFactor

Typedefs
typedef Eigen::MatrixXd	Matrix
typedef Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor >	MatrixRowMajor
typedef Eigen::Matrix3d	Matrix3
typedef Eigen::Matrix4d	Matrix4
typedef Eigen::Matrix< double, 6, 6 >	Matrix6
typedef Eigen::Block< Matrix >	SubMatrix
typedef Eigen::Block< const Matrix >	ConstSubMatrix
typedef size_t	Key
	Integer nonlinear key type.
typedef boost::function < std::string(Key)>	KeyFormatter
	Typedef for a function to format a key, i.e. to convert it to a string.
typedef ptrdiff_t	DenseIndex
	The index type for Eigen objects.
typedef Eigen::VectorXd	Vector
typedef Eigen::Vector2d	Vector2
typedef Eigen::Vector3d	Vector3
typedef Eigen::Matrix< double, 6, 1 >	Vector6
typedef Eigen::VectorBlock < Vector >	SubVector
typedef Eigen::VectorBlock < const Vector >	ConstSubVector
typedef std::pair< Point2, Point2 >	Point2Pair
	Calculate pose between a vector of 2D point correspondences (p,q) where q = Pose2::transform_from(p) = t + R*p.
typedef std::pair< Point3, Point3 >	Point3Pair
	Calculate pose between a vector of 3D point correspondences (p,q) where q = Pose3::transform_from(p) = t + R*p.
typedef Eigen::Quaternion < double, Eigen::DontAlign >	Quaternion
	Typedef to an Eigen Quaternion<double>, we disable alignment because geometry objects are stored in boost pool allocators, in Values containers, and and these pool allocators do not support alignment. More...
typedef PinholeCamera< Cal3_S2 >	SimpleCamera
	A simple camera class with a Cal3_S2 calibration.
typedef FastList< Key >	KeyList
	Useful typedefs for operations with Values - allow for matlab interfaces.
typedef FastVector< Key >	KeyVector
typedef FastSet< Key >	KeySet
typedef FastMap< Key, int >	KeyGroupMap
typedef std::pair< Key, size_t >	DiscreteKey
	Key type for discrete conditionals Includes name and cardinality.
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 FastMap< char, Vector >	ISAM2ThresholdMap
typedef ISAM2ThresholdMap::value_type	ISAM2ThresholdMapValue
typedef NonlinearOptimizerParams	SuccessiveLinearizationParams
typedef std::pair < NonlinearFactorGraph::shared_ptr, Values::shared_ptr >	GraphAndValues
	Return type for load functions.
typedef std::pair< size_t, Point2 >	SfM_Measurement
	A measurement with its camera index.
typedef PinholeCamera < Cal3Bundler >	SfM_Camera
	Define the structure for the camera poses.
typedef std::map< Key, std::vector< size_t > >	KeyVectorMap
typedef std::map< Key, Rot3 >	KeyRotMap

Enumerations
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, JACOBIAN_SVD, JACOBIAN_Q }

Functions
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)
	Partial Cholesky computes a factor [R S such that [R' 0 [R S = [A B 0 L] S' I] 0 L] B' C]. More...
template<class T >
T	between_default (const T &l1, const T &l2)
	These core global functions can be specialized by new Lie types for better performance. More...
template<class T >
Vector	logmap_default (const T &l0, const T &lp)
	Log map centered at l0, s.t. More...
template<class T >
T	expmap_default (const T &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...
Matrix	zeros (size_t m, size_t n)
	Creates an zeros matrix, with matlab-like syntax. More...
Matrix	ones (size_t m, size_t n)
	Creates an ones matrix, with matlab-like syntax.
Matrix	eye (size_t m, size_t n)
	Creates an identity matrix, with matlab-like syntax. More...
Matrix	diag (const Vector &v)
bool	assert_equal (const Matrix &A, const Matrix &B, double tol=1e-9)
	equals with an tolerance, prints out message if unequal More...
bool	assert_inequal (const Matrix &A, const Matrix &B, double tol=1e-9)
	inequals with an tolerance, prints out message if within tolerance More...
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 More...
void	multiplyAdd (double alpha, const Matrix &A, const Vector &x, Vector &e)
	BLAS Level-2 style e <- e + alpha*A*x.
void	multiplyAdd (const Matrix &A, const Vector &x, Vector &e)
	BLAS Level-2 style e <- e + A*x.
Vector	operator^ (const Matrix &A, const Vector &v)
	overload ^ for trans(A)*v We transpose the vectors for speed.
void	transposeMultiplyAdd (double alpha, const Matrix &A, const Vector &e, Vector &x)
	BLAS Level-2 style x <- x + alpha*A'*e.
void	transposeMultiplyAdd (const Matrix &A, const Vector &e, Vector &x)
	BLAS Level-2 style x <- x + A'*e.
void	transposeMultiplyAdd (double alpha, const Matrix &A, const Vector &e, SubVector x)
	BLAS Level-2 style x <- x + alpha*A'*e.
void	print (const Matrix &A, const std::string &s="", std::ostream &stream=std::cout)
	print a matrix More...
void	save (const Matrix &A, const std::string &s, const std::string &filename)
	save a matrix to file, which can be loaded by matlab More...
istream &	operator>> (std::istream &inputStream, Matrix &destinationMatrix)
	Read a matrix from an input stream, such as a file. More...
void	insertSub (Matrix &fullMatrix, const Matrix &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...
Matrix	diag (const std::vector< Matrix > &Hs)
	Create a matrix with submatrices along its diagonal.
void	insertColumn (Matrix &A, const Vector &col, size_t j)
	inserts a column into a matrix IN PLACE NOTE: there is no size checking Alternate form allows for vectors smaller than the whole column to be inserted More...
void	insertColumn (Matrix &A, const Vector &col, size_t i, size_t j)
Vector	columnNormSquare (const Matrix &A)
void	solve (Matrix &A, Matrix &B)
	solve AX=B via in-place Lu factorization and backsubstitution After calling, A contains LU, B the solved RHS vectors
Matrix	inverse (const Matrix &A)
	invert 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... 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)
Matrix3	skewSymmetric (double wx, double wy, double wz)
	skew symmetric matrix returns this: 0 -wz wy wz 0 -wx -wy wx 0 More...
Matrix	LLt (const Matrix &A)
	Calculate the LL^t decomposition of a S.P.D matrix.
Matrix	RtR (const Matrix &A)
	Calculate the R^tR decomposition of a S.P.D matrix.
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...
Matrix	Cayley (const Matrix &A)
	Cayley transform.
std::string	formatMatrixIndented (const std::string &label, const Matrix &matrix, bool makeVectorHorizontal)
Matrix	eye (size_t m)
	Creates a square identity matrix, with matlab-like syntax. 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 an tolerance
bool	operator== (const Matrix &A, const Matrix &B)
	equality is just equal_with_abs_tol 1e-9 More...
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<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<class MATRIX >
void	inplace_QR (MATRIX &A)
	QR factorization using Eigen's internal block QR algorithm. More...
template<class Derived >
Matrix3	skewSymmetric (const Eigen::MatrixBase< Derived > &w)
template<int N>
Eigen::Matrix< double, N, N >	CayleyFixed (const Eigen::Matrix< double, N, N > &A)
	Implementation of Cayley transform using fixed size matrices to let Eigen do more optimization.
LieVector	makeLieVector (const Vector &v)
	global functions for converting to a LieVector for use with numericalDerivative
LieVector	makeLieVectorD (double d)
template<class X >
Vector	numericalGradient (boost::function< double(const X &)> h, const X &x, double delta=1e-5)
	Numerically compute gradient of scalar function Class X is the input argument The class X needs to have dim, expmap, logmap.
template<class X >
Vector	numericalGradient (double(*h)(const X &), const X &x, double delta=1e-5)
template<class Y , class X >
Matrix	numericalDerivative11 (boost::function< Y(const X &)> h, const X &x, double delta=1e-5)
	Compute numerical derivative in argument 1 of unary function. More...
template<class Y , class X >
Matrix	numericalDerivative11 (Y(*h)(const X &), const X &x, double delta=1e-5)
	use a raw C++ function pointer
template<class X >
Matrix	numericalDerivative11 (double(*h)(const X &), const X &x, double delta=1e-5)
	remapping for double valued functions
template<class X >
Matrix	numericalDerivative11 (boost::function< Vector(const X &)> h, const X &x, double delta=1e-5)
	remapping for vector valued functions
template<class X >
Matrix	numericalDerivative11 (Vector(*h)(const X &), const X &x, double delta=1e-5)
template<class Y , class X1 , class X2 >
Matrix	numericalDerivative21 (boost::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 >
Matrix	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 X1 , class X2 >
Matrix	numericalDerivative21 (boost::function< double(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
	pseudo-partial template specialization for double return values
template<class X1 , class X2 >
Matrix	numericalDerivative21 (double(*h)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
template<class X1 , class X2 >
Matrix	numericalDerivative21 (boost::function< Vector(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
	pseudo-partial template specialization for vector return values
template<class X1 , class X2 >
Matrix	numericalDerivative21 (Vector(*h)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
template<class Y , class X1 , class X2 >
Matrix	numericalDerivative22 (boost::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 >
Matrix	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 X1 , class X2 >
Matrix	numericalDerivative22 (boost::function< double(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
	pseudo-partial template specialization for double return values
template<class X1 , class X2 >
Matrix	numericalDerivative22 (double(*h)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
template<class X1 , class X2 >
Matrix	numericalDerivative22 (boost::function< Vector(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
	pseudo-partial template specialization for vector return values
template<class X1 , class X2 >
Matrix	numericalDerivative22 (Vector(*h)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
template<class Y , class X1 , class X2 , class X3 >
Matrix	numericalDerivative31 (boost::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 >
Matrix	numericalDerivative31 (Y(*h)(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 >
Matrix	numericalDerivative31 (boost::function< double(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	pseudo-partial template specialization for double return values
template<class X1 , class X2 , class X3 >
Matrix	numericalDerivative31 (double(*h)(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 >
Matrix	numericalDerivative31 (boost::function< Vector(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	pseudo-partial template specialization for vector return values
template<class X1 , class X2 , class X3 >
Matrix	numericalDerivative31 (Vector(*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 >
Matrix	numericalDerivative32 (boost::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 >
Matrix	numericalDerivative32 (Y(*h)(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 >
Matrix	numericalDerivative32 (boost::function< double(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	pseudo-partial template specialization for double return values
template<class X1 , class X2 , class X3 >
Matrix	numericalDerivative32 (double(*h)(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 >
Matrix	numericalDerivative32 (boost::function< Vector(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	pseudo-partial template specialization for vector return values
template<class X1 , class X2 , class X3 >
Matrix	numericalDerivative32 (Vector(*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 >
Matrix	numericalDerivative33 (boost::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 >
Matrix	numericalDerivative33 (Y(*h)(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 >
Matrix	numericalDerivative33 (boost::function< double(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	pseudo-partial template specialization for double return values
template<class X1 , class X2 , class X3 >
Matrix	numericalDerivative33 (double(*h)(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 >
Matrix	numericalDerivative33 (boost::function< Vector(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
	pseudo-partial template specialization for vector return values
template<class X1 , class X2 , class X3 >
Matrix	numericalDerivative33 (Vector(*h)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
template<class X >
Matrix	numericalHessian (boost::function< double(const X &)> f, const X &x, double delta=1e-5)
	Compute numerical Hessian matrix. More...
template<class X >
Matrix	numericalHessian (double(*f)(const X &), const X &x, double delta=1e-5)
template<class X1 , class X2 >
Matrix	numericalHessian212 (boost::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5)
template<class X1 , class X2 >
Matrix	numericalHessian212 (double(*f)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
template<class X1 , class X2 >
Matrix	numericalHessian211 (boost::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5)
template<class X1 , class X2 >
Matrix	numericalHessian211 (double(*f)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
template<class X1 , class X2 >
Matrix	numericalHessian222 (boost::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5)
template<class X1 , class X2 >
Matrix	numericalHessian222 (double(*f)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5)
template<class X1 , class X2 , class X3 >
Matrix	numericalHessian311 (boost::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 >
Matrix	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 >
Matrix	numericalHessian322 (boost::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 >
Matrix	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 >
Matrix	numericalHessian333 (boost::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 >
Matrix	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 >
Matrix	numericalHessian312 (boost::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 >
Matrix	numericalHessian313 (boost::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 >
Matrix	numericalHessian323 (boost::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 >
Matrix	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 >
Matrix	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 >
Matrix	numericalHessian323 (double(*f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5)
template<class T >
std::string	serialize (const T &input)
template<class T >
void	deserialize (const std::string &serialized, T &output)
template<class T >
bool	serializeToFile (const T &input, const std::string &filename)
template<class T >
bool	deserializeFromFile (const std::string &filename, T &output)
template<class T >
std::string	serializeXML (const T &input, const std::string &name="data")
template<class T >
void	deserializeXML (const std::string &serialized, T &output, const std::string &name="data")
template<class T >
bool	serializeToXMLFile (const T &input, const std::string &filename, const std::string &name="data")
template<class T >
bool	deserializeFromXMLFile (const std::string &filename, T &output, const std::string &name="data")
template<class T >
std::string	serializeBinary (const T &input, const std::string &name="data")
template<class T >
void	deserializeBinary (const std::string &serialized, T &output, const std::string &name="data")
template<class T >
bool	serializeToBinaryFile (const T &input, const std::string &filename, const std::string &name="data")
template<class T >
bool	deserializeFromBinaryFile (const std::string &filename, T &output, const std::string &name="data")
template<class T >
void	print (const T &object, const std::string &s="")
	Call print on the object.
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 on the object 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	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.
void	tictoc_finishedIteration_ ()
void	tictoc_print_ ()
void	tictoc_print2_ ()
void	tictoc_reset_ ()
std::string	_defaultKeyFormatter (Key key)
	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. More...
bool	zero (const Vector &v)
	check if all zero
Vector	repeat (size_t n, double value)
	Create vector initialized to a constant value. More...
Vector	delta (size_t n, size_t i, double value)
	Create basis vector of dimension n, with a constant in spot i. More...
void	print (const Vector &v, const std::string &s="", std::ostream &stream=std::cout)
	print with optional string
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...
ConstSubVector	sub (const Vector &v, size_t i1, size_t i2)
	extract subvector, slice semantics, i.e. More...
void	subInsert (Vector &fullVector, const Vector &subVector, size_t i)
	Inserts a subvector into a vector IN PLACE. More...
Vector	emul (const Vector &a, const Vector &b)
	elementwise multiplication More...
Vector	ediv (const Vector &a, const Vector &b)
	elementwise division More...
Vector	ediv_ (const Vector &a, const Vector &b)
	elementwise division, but 0/0 = 0, not inf More...
double	sum (const Vector &a)
	sum vector elements More...
double	norm_2 (const Vector &v)
	Calculates L2 norm for a vector modeled after boost.ublas for compatibility. More...
Vector	reciprocal (const Vector &a)
	Elementwise reciprocal of vector elements. More...
Vector	esqrt (const Vector &v)
	Elementwise sqrt of vector elements. More...
Vector	abs (const Vector &v)
	Absolute values of vector elements. More...
double	max (const Vector &a)
	Return the max element of a vector. 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
Vector	basis (size_t n, size_t i)
	Create basis vector of dimension n, with one in spot i. More...
Vector	zero (size_t n)
	Create zero vector. More...
Vector	ones (size_t n)
	Create vector initialized to ones. More...
size_t	dim (const Vector &v)
	dimensionality == size
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	scal (double alpha, Vector &x)
	BLAS Level 1 scal: x <- alpha*x. More...
template<class V1 , class V2 >
void	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)
ostream &	operator<< (ostream &os, const EssentialMatrix &E)
istream &	operator>> (istream &is, EssentialMatrix &E)
	INSTANTIATE_LIE (Point2)
	Explicit instantiation of base class to export members.
ostream &	operator<< (ostream &os, const Point2 &p)
Point2	operator* (double s, const Point2 &p)
	multiply with scalar
	INSTANTIATE_LIE (Point3)
	Explicit instantiation of base class to export members.
ostream &	operator<< (ostream &os, const Point3 &p)
Point3	operator* (double s, const Point3 &p)
	Syntactic sugar for multiplying coordinates by a scalar s*p.
	INSTANTIATE_LIE (Pose2)
	Explicit instantiation of base class to export members.
	GTSAM_CONCEPT_POSE_INST (Pose2)
	instantiate concept checks
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)
	INSTANTIATE_LIE (Pose3)
	Explicit instantiation of base class to export members.
	GTSAM_CONCEPT_POSE_INST (Pose3)
	instantiate concept checks
boost::optional< Pose3 >	align (const vector< Point3Pair > &pairs)
std::ostream &	operator<< (std::ostream &os, const Pose3 &pose)
template<>
Matrix	wedge< Pose3 > (const Vector &xi)
	wedge for Pose3: More...
	INSTANTIATE_LIE (Rot2)
	Explicit instantiation of base class to export members.
pair< Matrix3, Vector3 >	RQ (const Matrix3 &A)
	[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)
SimpleCamera	simpleCamera (const Matrix &P)
	Recover camera from 3*4 camera matrix.
Point3	triangulateDLT (const std::vector< Matrix > &projection_matrices, const std::vector< Point2 > &measurements, double rank_tol)
	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 std::vector< Point2 > &measurements, Key landmarkKey, const Point3 &initialEstimate)
	Create a factor graph with projection factors from poses and one calibration. More...
template<class CALIBRATION >
std::pair < NonlinearFactorGraph, Values >	triangulationGraph (const std::vector< PinholeCamera< CALIBRATION > > &cameras, const std::vector< Point2 > &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 std::vector< Point2 > &measurements, const Point3 &initialEstimate)
	Given an initial estimate , refine a point using measurements in several cameras. More...
template<class CALIBRATION >
Point3	triangulateNonlinear (const std::vector< PinholeCamera< CALIBRATION > > &cameras, const std::vector< Point2 > &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 std::vector< Point2 > &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 std::vector< PinholeCamera< CALIBRATION > > &cameras, const std::vector< Point2 > &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 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.
std::string	_multirobotKeyFormatter (Key key)
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.
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. More...
unsigned char	mrsymbolLabel (Key key)
	Return the label portion of a symbol key. More...
size_t	mrsymbolIndex (Key key)
	Return the index portion of a symbol key. More...
Key	symbol (unsigned char c, size_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. More...
size_t	symbolIndex (Key key)
	Return the index portion of a symbol key. More...
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);.
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< Errors, Errors > (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< Errors, Errors > (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)
std::pair< boost::shared_ptr < GaussianConditional > , boost::shared_ptr < JacobianFactor > >	EliminateQR (const GaussianFactorGraph &factors, const Ordering &keys)
VectorValues	buildVectorValues (const Vector &v, const Ordering &ordering, const map< Key, size_t > &dimensions)
VectorValues	buildVectorValues (const Vector &v, const KeyInfo &keyInfo)
ostream &	operator<< (ostream &os, const PreconditionerParameters &p)
boost::shared_ptr< Preconditioner >	createPreconditioner (const boost::shared_ptr< PreconditionerParameters > parameters)
vector< size_t >	uniqueSampler (const vector< double > &weight, const size_t n)
std::ostream &	operator<< (std::ostream &os, const SubgraphEdge &edge)
std::ostream &	operator<< (std::ostream &os, const Subgraph &subgraph)
ostream &	operator<< (ostream &os, const SubgraphBuilderParameters &p)
Vector	getSubvector (const Vector &src, const KeyInfo &keyInfo, const FastVector< Key > &keys)
void	setSubvector (const Vector &src, const KeyInfo &keyInfo, const FastVector< Key > &keys, Vector &dst)
boost::shared_ptr < GaussianFactorGraph >	buildFactorSubgraph (const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone)
template<typename Container >
std::vector< size_t >	sort_idx (const Container &src)
VectorValues	operator* (const double a, const VectorValues &v)
void	CheckRelinearizationRecursiveDouble (FastSet< Key > &relinKeys, double threshold, const VectorValues &delta, const ISAM2Clique::shared_ptr &clique)
void	CheckRelinearizationRecursiveMap (FastSet< Key > &relinKeys, const FastMap< char, Vector > &thresholds, const VectorValues &delta, const ISAM2Clique::shared_ptr &clique)
template<class CLIQUE >
size_t	optimizeWildfire (const boost::shared_ptr< CLIQUE > &root, double threshold, const FastSet< Key > &replaced, VectorValues &delta)
	Optimize the BayesTree, starting from the root. More...
template<class CLIQUE >
size_t	optimizeWildfireNonRecursive (const boost::shared_ptr< CLIQUE > &root, double threshold, const FastSet< Key > &keys, VectorValues &delta)
template<class CLIQUE >
void	nnz_internal (const boost::shared_ptr< CLIQUE > &clique, int &result)
template<class CLIQUE >
int	calculate_nnz (const boost::shared_ptr< CLIQUE > &clique)
	calculate the number of non-zero entries for the tree starting at clique (use root for complete matrix)
VectorValues	gradientInPlace (const NonlinearFactorGraph &nfg, const Values &values)
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-Ribieve formula suggested in http://en.wikipedia.org/wiki/Nonlinear_conjugate_gradient_method. More...
template<class T >
bool	compare (const T &a, const T &b)
	Template default compare function that assumes a testable T.
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.
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.
GraphAndValues	load2D (pair< string, SharedNoiseModel > dataset, int maxID, bool addNoise, bool smart, NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType)
GraphAndValues	load2D (const std::string &filename, SharedNoiseModel model=SharedNoiseModel(), Key maxID=0, bool addNoise=false, bool smart=true, NoiseFormat noiseFormat=NoiseFormatAUTO,KernelFunctionType kernelFunctionType=KernelFunctionTypeNONE)
	Load TORO/G2O style graph files. More...
GraphAndValues	load2D_robust (const string &filename, noiseModel::Base::shared_ptr &model, int maxID)
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...
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, SfM_data &data)
	This function parses a bundler output file and stores the data into a SfM_data structure. More...
bool	readBAL (const std::string &filename, SfM_data &data)
	This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a SfM_data structure. More...
bool	writeBAL (const std::string &filename, SfM_data &data)
	This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfM_data structure. More...
bool	writeBALfromValues (const std::string &filename, const SfM_data &data, Values &values)
	This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfM_data 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 SfM_data &db)
	This function creates initial values for cameras from db. More...
Values	initialCamerasAndPointsEstimate (const SfM_data &db)
	This function creates initial values for cameras and points from db. More...
GTSAM_EXPORT GraphAndValues	load2D (std::pair< std::string, SharedNoiseModel > dataset, int maxID=0, bool addNoise=false, bool smart=true, NoiseFormat noiseFormat=NoiseFormatAUTO, KernelFunctionType kernelFunctionType=KernelFunctionTypeNONE)
	Load TORO 2D Graph. More...
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...

Variables
GTSAM_EXPORT FastMap < std::string, ValueWithDefault< bool, false > >	debugFlags
const double	logSqrt2PI = log(std::sqrt(2.0 * M_PI))
	constant needed below

Detailed Description

Global functions in a separate testing namespace.

testTriangulationFactor.h

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 boost::bind magic or writing a bunch of separate proxy functions.

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

Date

March 2, 2014

Author

Frank Dellaert

Typedef Documentation

typedef Eigen::Quaternion<double, Eigen::DontAlign> gtsam::Quaternion

Typedef to an Eigen Quaternion<double>, we disable alignment because geometry objects are stored in boost pool allocators, in Values containers, and and these pool allocators do not support alignment.

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

Note, deliberately not in noiseModel namespace.

Deprecated. Only for compatibility with previous version.

Enumeration Type Documentation

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

GTSAM_EXPORT Vector gtsam::abs

(

const Vector &

v

)

Absolute values of vector elements.

Parameters

v	is a vector

Returns

[abs(a(i))]

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

GTSAM_EXPORT bool gtsam::assert_equal	(	const Matrix &	expected,
		const Matrix &	actual,
		double	tol
	)

equals with an tolerance, prints out message if unequal

Same, prints if error.

Parameters

vec1	Vector
vec2	Vector
tol	1e-9

Returns

bool

template<class V >

bool 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

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

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

GTSAM_EXPORT bool gtsam::assert_inequal	(	const Matrix &	A,
		const Matrix &	B,
		double	tol
	)

inequals with an tolerance, prints out message if within tolerance

Not the same, prints if error.

Parameters

vec1	Vector
vec2	Vector
tol	1e-9

Returns

bool

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

template<class V1 , class V2 >

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

inline

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

Deprecated:

: use operators instead

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

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

backSubstitute U*x=b

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 U*x=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'

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'

Vector gtsam::basis ( size_t  n, size_t  i  )

inline

Create basis vector of dimension n, with one in spot i.

Parameters

n	is the size of the vector
i	index of the one

Returns

basis vector

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_formulaAGC: bracket() only appears in Rot3 tests, should this be used elsewhere?

template<class T >

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

inline

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

Compute l0 s.t. l2=l1*l0

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

GTSAM_EXPORT Matrix gtsam::cholesky_inverse

(

const Matrix &

A

)

Return the inverse of a S.P.D.

matrix. Inversion is done via Cholesky decomposition.

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.

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

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.

Returns

true if the decomposition is successful, false if A was not positive-definite.

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]

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

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

GTSAM_EXPORT Vector gtsam::delta	(	size_t	n,
		size_t	i,
		double	value
	)

Create basis vector of dimension n, with a constant in spot i.

Parameters

n	is the size of the vector
i	index of the one
value	is the value to insert into the vector

Returns

delta vector

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

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

elementwise division

Parameters

a	first vector
b	second vector

Returns

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

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

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

elementwise multiplication

Parameters

a	first vector
b	second vector

Returns

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

GTSAM_EXPORT Vector gtsam::esqrt

(

const Vector &

v

)

Elementwise sqrt of vector elements.

Parameters

v	is a vector

Returns

[sqrt(a(i))]

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

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

template<class T >

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

inline

Exponential map centered at l0, s.t.

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

GTSAM_EXPORT Matrix gtsam::eye	(	size_t	m,
		size_t	n
	)

Creates an identity matrix, with matlab-like syntax.

Note: if assigning a block (created from an Eigen block() function) of a matrix to identity, don't use this function, instead use ".setIdentity(m,n)" to avoid an Eigen error.

Matrix gtsam::eye ( size_t  m )

inline

Creates a square identity matrix, with matlab-like syntax.

Note: if assigning a block (created from an Eigen block() function) of a matrix to identity, don't use this function, instead use ".setIdentity(m)" to avoid an Eigen error.

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.

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

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

GTSAM_EXPORT bool gtsam::hasConstraints

(

const GaussianFactorGraph &

factors

)

Evaluates whether linear factors have any constrained noise models.

Returns

true if any factor is constrained.

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.

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

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

GTSAM_EXPORT Values gtsam::initialCamerasAndPointsEstimate

(

const SfM_data &

db

)

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

Parameters

SfM_data

Returns

Values

GTSAM_EXPORT Values gtsam::initialCamerasEstimate

(

const SfM_data &

db

)

This function creates initial values for cameras from db.

Parameters

SfM_data

Returns

Values

template<class MATRIX >

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

GTSAM_EXPORT void gtsam::insertColumn	(	Matrix &	A,
		const Vector &	col,
		size_t	j
	)

inserts a column into a matrix IN PLACE NOTE: there is no size checking Alternate form allows for vectors smaller than the whole column to be inserted

Parameters

A	matrix to be modified in place
col	is the vector to be inserted
j	is the index to insert the column

GTSAM_EXPORT void gtsam::insertSub	(	Matrix &	fullMatrix,
		const Matrix &	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

GTSAM_EXPORT bool gtsam::linear_dependent	(	const Matrix &	A,
		const Matrix &	B,
		double	tol
	)

check whether the rows of two matrices are linear dependent

check whether two vectors are linearly dependent

Parameters

vec1	Vector
vec2	Vector
tol	1e-9

Returns

bool

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

template<typename T >

ListOfOneContainer<T> gtsam::ListOfOne

(

const T &

element

)

Factory function for ListOfOneContainer to enable ListOfOne(e) syntax.

GTSAM_EXPORT GraphAndValues gtsam::load2D	(	std::pair< std::string, SharedNoiseModel >	dataset,
		int	maxID = 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]
maxID	if non-zero cut out vertices >= maxID
addNoise	add noise to the edges
smart	try to reduce complexity of covariance to cheapest model

GTSAM_EXPORT GraphAndValues gtsam::load2D	(	const std::string &	filename,
		SharedNoiseModel	model = SharedNoiseModel(),
		Key	maxID = 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
maxID	if non-zero cut out vertices >= maxID
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

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

Deprecated:

load2D now allows for arbitrary models and wrapping a robust kernel

template<class T >

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

inline

Log map centered at l0, s.t.

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

GTSAM_EXPORT double gtsam::max

(

const Vector &

a

)

Return the max element of a vector.

Parameters

a	is a vector

Returns

max(a)

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.

unsigned char gtsam::mrsymbolChr ( Key  key )

inline

Return the character portion of a symbol key.

size_t gtsam::mrsymbolIndex ( Key  key )

inline

Return the index portion of a symbol key.

unsigned char gtsam::mrsymbolLabel ( Key  key )

inline

Return the label portion of a symbol key.

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

GTSAM_EXPORT double gtsam::norm_2

(

const Vector &

v

)

Calculates L2 norm for a vector modeled after boost.ublas for compatibility.

Parameters

v

vector

Returns

the L2 norm

template<class Y , class X >

Matrix gtsam::numericalDerivative11	(	boost::function< Y(const X &)>	h,
		const X &	x,
		double	delta = 1e-5
	)

Compute 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

Returns

m*n Jacobian computed via central differencing Both classes X,Y need dim, expmap, logmap

template<class Y , class X1 , class X2 >

Matrix gtsam::numericalDerivative21	(	boost::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 All classes Y,X1,X2 need dim, expmap, logmap

template<class Y , class X1 , class X2 >

Matrix gtsam::numericalDerivative22	(	boost::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 All classes Y,X1,X2 need dim, expmap, logmap

template<class Y , class X1 , class X2 , class X3 >

Matrix gtsam::numericalDerivative31	(	boost::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<class Y , class X1 , class X2 , class X3 >

Matrix gtsam::numericalDerivative32	(	boost::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<class Y , class X1 , class X2 , class X3 >

Matrix gtsam::numericalDerivative33	(	boost::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<class X >

Matrix gtsam::numericalHessian ( boost::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

Vector gtsam::ones ( size_t  n )

inline

Create vector initialized to ones.

Parameters

n

size

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

VectorValues gtsam::operator*	(	const double	a,
		const VectorValues &	v
	)

GTSAM_EXPORT bool gtsam::operator== ( const Matrix &  A, const Matrix &  B  )

inline

equality is just equal_with_abs_tol 1e-9

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.

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

template<class CLIQUE >

size_t gtsam::optimizeWildfire	(	const boost::shared_ptr< CLIQUE > &	root,
		double	threshold,
		const FastSet< Key > &	replaced,
		VectorValues &	delta
	)

Optimize the BayesTree, starting from the root.

Parameters

replaced	Needs to contain all variables that are contained in the top of the Bayes tree that has been redone.
delta	The current solution, an offset from the linearization point.
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.

Returns

The number of variables that were solved for

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

GTSAM_EXPORT void gtsam::print	(	const Matrix &	A,
		const string &	s,
		ostream &	stream
	)

print a matrix

print with optional string

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

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

This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a SfM_data 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

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

This function parses a bundler output file and stores the data into a SfM_data 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

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

GTSAM_EXPORT Vector gtsam::reciprocal

(

const Vector &

a

)

Elementwise reciprocal of vector elements.

Parameters

a

vector

Returns

[1/a(i)]

GTSAM_EXPORT Vector gtsam::repeat	(	size_t	n,
		double	value
	)

Create vector initialized to a constant value.

Parameters

n	is the size of the vector
value	is a constant value to insert into the vector

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

GTSAM_EXPORT std::pair< Matrix3, Vector3 > gtsam::RQ

(

const Matrix3 &

A

)

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

GTSAM_EXPORT void gtsam::save	(	const Matrix &	A,
		const string &	s,
		const string &	filename
	)

save a matrix to file, which can be loaded by matlab

save a vector to file, which can be loaded by matlab

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

inline

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

Deprecated:

: use operators instead

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

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

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]

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

GTSAM_EXPORT ConstSubVector gtsam::sub	(	const Vector &	v,
		size_t	i1,
		size_t	i2
	)

extract subvector, slice semantics, i.e.

range = [i1,i2[ excluding i2

Parameters

v	Vector
i1	first row index
i2	last row index + 1

Returns

subvector v(i1:i2)

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)

GTSAM_EXPORT void gtsam::subInsert	(	Vector &	fullVector,
		const Vector &	subVector,
		size_t	i
	)

Inserts a subvector into a vector IN PLACE.

Parameters

fullVector	is the vector to be changed
subVector	is the vector to insert
i	is the index where the subvector should be inserted

GTSAM_EXPORT double gtsam::sum

(

const Vector &

a

)

sum vector elements

Parameters

a

vector

Returns

sum_i a(i)

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.

Key gtsam::symbol ( unsigned char  c, size_t  j  )

inline

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

x5.

unsigned char gtsam::symbolChr ( Key  key )

inline

Return the character portion of a symbol key.

size_t gtsam::symbolIndex ( Key  key )

inline

Return the index portion of a symbol key.

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

GTSAM_EXPORT Point3 gtsam::triangulateDLT	(	const std::vector< Matrix > &	projection_matrices,
		const std::vector< Point2 > &	measurements,
		double	rank_tol
	)

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

template<class CALIBRATION >

Point3 gtsam::triangulateNonlinear	(	const std::vector< Pose3 > &	poses,
		boost::shared_ptr< CALIBRATION >	sharedCal,
		const std::vector< Point2 > &	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

template<class CALIBRATION >

Point3 gtsam::triangulateNonlinear	(	const std::vector< PinholeCamera< CALIBRATION > > &	cameras,
		const std::vector< Point2 > &	measurements,
		const Point3 &	initialEstimate
	)

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

Parameters

cameras	pinhole cameras
measurements	2D measurements
initialEstimate

Returns

refined Point3

template<class CALIBRATION >

Point3 gtsam::triangulatePoint3	(	const std::vector< Pose3 > &	poses,
		boost::shared_ptr< CALIBRATION >	sharedCal,
		const std::vector< Point2 > &	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

template<class CALIBRATION >

Point3 gtsam::triangulatePoint3	(	const std::vector< PinholeCamera< CALIBRATION > > &	cameras,
		const std::vector< Point2 > &	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

template<class CALIBRATION >

std::pair<NonlinearFactorGraph, Values> gtsam::triangulationGraph	(	const std::vector< Pose3 > &	poses,
		boost::shared_ptr< CALIBRATION >	sharedCal,
		const std::vector< Point2 > &	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
measurements	2D measurements
landmarkKey	to refer to landmark
initialEstimate

Returns

graph and initial values

template<class CALIBRATION >

std::pair<NonlinearFactorGraph, Values> gtsam::triangulationGraph	(	const std::vector< PinholeCamera< CALIBRATION > > &	cameras,
		const std::vector< Point2 > &	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
measurements	2D measurements
landmarkKey	to refer to landmark
initialEstimate

Returns

graph and initial values

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.

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

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

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

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

This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfM_data 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

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

This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfM_data 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

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

Vector gtsam::zero ( size_t  n )

inline

Create zero vector.

Parameters

n

size

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

GTSAM_EXPORT Matrix gtsam::zeros	(	size_t	m,
		size_t	n
	)

Creates an zeros matrix, with matlab-like syntax.

Note: if assigning a block (created from an Eigen block() function) of a matrix to zeros, don't use this function, instead use ".setZero(m,n)" to avoid an Eigen error.