gtsam  3.2.0
gtsam
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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, Point2Point2Pair
 Calculate pose between a vector of 2D point correspondences (p,q) where q = Pose2::transform_from(p) = t + R*p.
 
typedef std::pair< Point3, Point3Point3Pair
 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_S2SimpleCamera
 A simple camera class with a Cal3_S2 calibration.
 
typedef FastList< KeyKeyList
 Useful typedefs for operations with Values - allow for matlab interfaces.
 
typedef FastVector< KeyKeyVector
 
typedef FastSet< KeyKeySet
 
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, Point2SfM_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, Rot3KeyRotMap
 

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 >
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 >
expmap_default (const T &t, const Vector &d)
 Exponential map centered at l0, s.t. More...
 
template<class 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 >
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< Pose2align (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< Pose3align (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< ValuescomposePoses (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 >
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 >
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< PreconditionercreatePreconditioner (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 >
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

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
vis 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
vec1Vector
vec2Vector
tol1e-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
vec1Vector
vec2Vector
tol1e-9
Returns
bool
GTSAM_EXPORT bool gtsam::assert_equal ( const SubVector &  vec1,
const SubVector &  vec2,
double  tol = 1e-9 
)

Same, prints if error.

Parameters
vec1Vector
vec2Vector
tol1e-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
vec1Vector
vec2Vector
tol1e-9
Returns
bool
bool gtsam::assert_inequal ( const Vector &  vec1,
const Vector &  vec2,
double  tol = 1e-9 
)

Not the same, prints if error.

Parameters
vec1Vector
vec2Vector
tol1e-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
Lan lower triangular matrix
ban RHS vector
unit,settrue 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
Uan upper triangular matrix
ban RHS vector
unit,settrue if unit triangular
Returns
the solution x of U*x=b
Parameters
Uan upper triangular matrix
ban RHS vector
unit,settrue 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
Uan upper triangular matrix
ban RHS vector
unit,settrue 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
nis the size of the vector
iindex 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
vectorlist 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
matricesis a vector of matrices in the order to be collected
mis the number of rows of a single matrix
nis 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
Amatrix to extract column from
jindex of the column
Returns
a const view of the matrix
template<class S , class V , class E >
V gtsam::conjugateGradients ( const S &  Ab,
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
xis the initial estimate
steepestflag, 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
nis the size of the vector
iindex of the one
valueis 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
Aof 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
afirst vector
bsecond 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
afirst vector
bsecond vector
Returns
vector [a(i)/b(i)]
GTSAM_EXPORT Vector gtsam::emul ( const Vector &  a,
const Vector &  b 
)

elementwise multiplication

Parameters
afirst vector
bsecond vector
Returns
vector [a(i)*b(i)]
GTSAM_EXPORT Vector gtsam::esqrt ( const Vector &  v)

Elementwise sqrt of vector elements.

Parameters
vis 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
xexponential 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
Amatrix to exponentiate
Knumber 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_argumentif 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
Rrotation in GTSAM
txx component of the translation in GTSAM
tyy component of the translation in GTSAM
tzz 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
PoseGTSAMpose 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
knumber of columns to zero out below diagonal
Amatrix
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
knumber of columns to zero out below diagonal
Amatrix
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
Ais the input matrix, and is the output
clear_below_diagonalenables 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
Amatrix to be modified in place
colis the vector to be inserted
jis 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
fullMatrixmatrix to be updated
subMatrixmatrix to be inserted
iis the row of the upper left corner insert location
jis 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
vec1Vector
vec2Vector
tol1e-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
vec1Vector
vec2Vector
tol1e-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/modelpair as constructed by [dataset]
maxIDif non-zero cut out vertices >= maxID
addNoiseadd noise to the edges
smarttry 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
modeloptional noise model to use instead of one specified by file
maxIDif non-zero cut out vertices >= maxID
addNoiseadd noise to the edges
smarttry to reduce complexity of covariance to cheapest model
noiseFormathow noise parameters are stored
kernelFunctionTypewhether 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
ais 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
vvector
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
hunary function yielding m-vector
xn-dimensional value at which to evaluate h
deltaincrement 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
hbinary function yielding m-vector
x1n-dimensional first argument value
x2second argument value
deltaincrement 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
hbinary function yielding m-vector
x1first argument value
x2n-dimensional second argument value
deltaincrement 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
hternary function yielding m-vector
x1n-dimensional first argument value
x2second argument value
x3third argument value
deltaincrement 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
hternary function yielding m-vector
x1n-dimensional first argument value
x2second argument value
x3third argument value
deltaincrement 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
hternary function yielding m-vector
x1n-dimensional first argument value
x2second argument value
x3third argument value
deltaincrement 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
fA function taking a Lie object as input and returning a scalar
xThe center point for computing the Hessian
deltaThe 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
nsize
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
Rrotation in openGL
txx component of the translation in openGL
tyy component of the translation in openGL
tzz 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
graphnonlinear factors for projection
valuesinitial values
landmarkKeyto 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
replacedNeeds to contain all variables that are contained in the top of the Bayes tree that has been redone.
deltaThe current solution, an offset from the linearization point.
thresholdThe 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
Aa 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
filenameThe name of the BAL file
dataSfM 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
filenameThe name of the bundler file
dataSfM 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
filenameThe name of the g2o file\
is3Dindicates if the file describes a 2D or 3D problem
kernelFunctionTypewhether 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
avector
Returns
[1/a(i)]
GTSAM_EXPORT Vector gtsam::repeat ( size_t  n,
double  value 
)

Create vector initialized to a constant value.

Parameters
nis the size of the vector
valueis 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
Amatrix to extract row from
jindex 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
A3 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
wx3 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
vVector
i1first row index
i2last 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
Amatrix
i1first row index
i2last row index + 1
j1first col index
j2last 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
fullVectoris the vector to be changed
subVectoris the vector to insert
iis the index where the subvector should be inserted
GTSAM_EXPORT double gtsam::sum ( const Vector &  a)

sum vector elements

Parameters
avector
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
Aan m*n matrix
Uoutput argument: rotation matrix
Soutput argument: sorted vector of singular values
Voutput 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
Tis a Transform type
Pis 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_matricesProjection matrices (K*P^-1)
measurements2D measurements
rank_tolSVD 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
posesCamera poses
sharedCalshared pointer to single calibration object
measurements2D 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
cameraspinhole cameras
measurements2D 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
posesA vector of camera poses
sharedCalshared pointer to single calibration object
measurementsA vector of camera measurements
rank_tolrank tolerance, default 1e-9
optimizeFlag 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
cameraspinhole cameras
measurementsA vector of camera measurements
rank_tolrank tolerance, default 1e-9
optimizeFlag 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
posesCamera poses
sharedCalshared pointer to single calibration object
measurements2D measurements
landmarkKeyto 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
cameraspinhole cameras
measurements2D measurements
landmarkKeyto 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_maskwhen true, will not scale with a NaN or inf value.
template<>
Matrix gtsam::wedge< Pose3 > ( const Vector &  xi)
inline

wedge for Pose3:

Parameters
xi6-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
Ais a matrix to eliminate
bis the rhs
sigmasis 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
vis the first column of the matrix to solve
weightsis 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
filenameThe name of the BAL file to write
dataSfM 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
filenameThe name of the BAL file to write
dataSfM structure where the data is stored
valuesstructure 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
filenameThe name of the g2o file to write
graphNonlinearFactor graph storing the measurements
estimateValues
Vector gtsam::zero ( size_t  n)
inline

Create zero vector.

Parameters
nsize
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
Ais a matrix, to be modified in place
colsis 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.