GTSAM
4.0.2
C++ library for smoothing and mapping (SAM)
|
Namespaces | |
imuBias | |
All bias models live in the imuBias namespace. | |
noiseModel | |
All noise models live in the noiseModel namespace. | |
treeTraversal | |
Typedefs | |
typedef std::vector< IndexPair > | IndexPairVector |
typedef std::set< IndexPair > | IndexPairSet |
typedef std::map< IndexPair, IndexPairSet > | IndexPairSetMap |
typedef DSFMap< IndexPair > | DSFMapIndexPair |
template<typename T > | |
using | FastVector = std::vector< T, typename internal::FastDefaultVectorAllocator< T >::type > |
template<bool B, class T = void> | |
using | enable_if_t = typename std::enable_if< B, T >::type |
An shorthand alias for accessing the ::type inside std::enable_if that can be used in a template directly. | |
typedef Eigen::MatrixXd | Matrix |
typedef Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > | MatrixRowMajor |
typedef Eigen::Block< Matrix > | SubMatrix |
typedef Eigen::Block< const Matrix > | ConstSubMatrix |
typedef std::uint64_t | Key |
Integer nonlinear key type. | |
typedef std::uint64_t | FactorIndex |
Integer nonlinear factor index type. | |
typedef ptrdiff_t | DenseIndex |
The index type for Eigen objects. | |
template<typename ... > | |
using | void_t = void |
Convenience void_t as we assume C++11, it will not conflict the std one in C++17 as this is in gtsam:: | |
template<class... T> | |
using | index_sequence_for = make_index_sequence< sizeof...(T)> |
typedef Eigen::VectorXd | Vector |
typedef Eigen::Matrix< double, 1, 1 > | Vector1 |
typedef Eigen::Vector2d | Vector2 |
typedef Eigen::Vector3d | Vector3 |
typedef Eigen::VectorBlock< Vector > | SubVector |
typedef Eigen::VectorBlock< const Vector > | ConstSubVector |
using | Weights = Eigen::Matrix< double, 1, -1 > |
using | Sequence = std::map< double, double > |
Our sequence representation is a map of {x: y} values where y = f(x) | |
using | Sample = std::pair< double, double > |
A sample is a key-value pair from a sequence. | |
using | DiscreteKey = std::pair< Key, size_t > |
typedef Vector2 | Point2 |
using | Point2Pair = std::pair< Point2, Point2 > |
using | Point2Pairs = std::vector< Point2Pair > |
typedef std::vector< Point2, Eigen::aligned_allocator< Point2 > > | Point2Vector |
typedef Vector3 | Point3 |
typedef std::vector< Point3, Eigen::aligned_allocator< Point3 > > | Point3Vector |
using | Point3Pair = std::pair< Point3, Point3 > |
using | Point3Pairs = std::vector< Point3Pair > |
using | Pose2Pair = std::pair< Pose2, Pose2 > |
using | Pose2Pairs = std::vector< Pose2Pair > |
using | Pose3Pair = std::pair< Pose3, Pose3 > |
using | Pose3Pairs = std::vector< std::pair< Pose3, Pose3 > > |
typedef std::vector< Pose3 > | Pose3Vector |
typedef Eigen::Quaternion< double, Eigen::DontAlign > | Quaternion |
using | Rot3Vector = std::vector< Rot3, Eigen::aligned_allocator< Rot3 > > |
std::vector of Rot3s, mainly for wrapper | |
using | PinholeCameraCal3_S2 = gtsam::PinholeCamera< gtsam::Cal3_S2 > |
using | PinholeCameraCal3Bundler = gtsam::PinholeCamera< gtsam::Cal3Bundler > |
using | PinholeCameraCal3DS2 = gtsam::PinholeCamera< gtsam::Cal3DS2 > |
using | PinholeCameraCal3Unified = gtsam::PinholeCamera< gtsam::Cal3Unified > |
using | PinholeCameraCal3Fisheye = gtsam::PinholeCamera< gtsam::Cal3Fisheye > |
using | SO3 = SO< 3 > |
using | SO4 = SO< 4 > |
using | SOn = SO< Eigen::Dynamic > |
using | DynamicJacobian = OptionalJacobian< Eigen::Dynamic, Eigen::Dynamic > |
typedef std::vector< StereoPoint2 > | StereoPoint2Vector |
using | CameraSetCal3Bundler = CameraSet< PinholeCamera< Cal3Bundler > > |
using | CameraSetCal3_S2 = CameraSet< PinholeCamera< Cal3_S2 > > |
using | CameraSetCal3DS2 = CameraSet< PinholeCamera< Cal3DS2 > > |
using | CameraSetCal3Fisheye = CameraSet< PinholeCamera< Cal3Fisheye > > |
using | CameraSetCal3Unified = CameraSet< PinholeCamera< Cal3Unified > > |
using | CameraSetSpherical = CameraSet< SphericalCamera > |
using | GaussianFactorGraphTree = DecisionTree< Key, GaussianFactorGraph > |
Alias for DecisionTree of GaussianFactorGraphs. | |
using | SharedFactor = std::shared_ptr< Factor > |
using | MotionModel = BetweenFactor< double > |
typedef FastVector< FactorIndex > | FactorIndices |
Define collection types: More... | |
typedef FastSet< FactorIndex > | FactorIndexSet |
using | KeyFormatter = std::function< std::string(Key)> |
Typedef for a function to format a key, i.e. to convert it to a string. | |
using | KeyVector = FastVector< Key > |
Define collection type once and for all - also used in wrappers. | |
using | KeyList = FastList< Key > |
using | KeySet = FastSet< Key > |
using | KeyGroupMap = FastMap< Key, int > |
using | Sparse = Eigen::SparseMatrix< double > |
using | Errors = FastList< Vector > |
Errors is a vector of errors. | |
typedef noiseModel::Base::shared_ptr | SharedNoiseModel |
typedef noiseModel::Gaussian::shared_ptr | SharedGaussian |
typedef noiseModel::Diagonal::shared_ptr | SharedDiagonal |
typedef noiseModel::Constrained::shared_ptr | SharedConstrained |
typedef noiseModel::Isotropic::shared_ptr | SharedIsotropic |
typedef Eigen::SparseMatrix< double, Eigen::ColMajor, int > | SparseEigen |
typedef ManifoldPreintegration | PreintegrationType |
typedef Expression< NavState > | NavState_ |
typedef Expression< Velocity3 > | Velocity3_ |
typedef Vector3 | Velocity3 |
Velocity is currently typedef'd to Vector3. More... | |
using | JacobianVector = std::vector< Matrix > |
using | CustomErrorFunction = std::function< Vector(const CustomFactor &, const Values &, const JacobianVector *)> |
typedef Expression< double > | Double_ |
typedef Expression< Vector1 > | Vector1_ |
typedef Expression< Vector2 > | Vector2_ |
typedef Expression< Vector3 > | Vector3_ |
typedef Expression< Vector4 > | Vector4_ |
typedef Expression< Vector5 > | Vector5_ |
typedef Expression< Vector6 > | Vector6_ |
typedef Expression< Vector7 > | Vector7_ |
typedef Expression< Vector8 > | Vector8_ |
typedef Expression< Vector9 > | Vector9_ |
typedef FastMap< char, Vector > | ISAM2ThresholdMap |
typedef ISAM2ThresholdMap::value_type | ISAM2ThresholdMapValue |
using | OptionalMatrixType = Matrix * |
using | OptionalMatrixVecType = std::vector< Matrix > * |
typedef NonlinearOptimizerParams | SuccessiveLinearizationParams |
typedef std::map< std::pair< Key, Key >, double > | KeyPairDoubleMap |
typedef PinholeCamera< Cal3Bundler > | SfmCamera |
Define the structure for the camera poses. | |
typedef std::pair< size_t, Point2 > | SfmMeasurement |
A measurement with its camera index. | |
typedef std::pair< size_t, size_t > | SiftIndex |
Sift index for SfmTrack. | |
using | SfmTrack2dVector = std::vector< SfmTrack2d > |
using | ShonanAveragingParameters2 = ShonanAveragingParameters< 2 > |
using | ShonanAveragingParameters3 = ShonanAveragingParameters< 3 > |
using | ShonanFactor2 = ShonanFactor< 2 > |
using | ShonanFactor3 = ShonanFactor< 3 > |
typedef std::pair< size_t, Pose2 > | IndexedPose |
Return type for auxiliary functions. | |
typedef std::pair< size_t, Point2 > | IndexedLandmark |
typedef std::pair< std::pair< size_t, size_t >, Pose2 > | IndexedEdge |
using | GraphAndValues = std::pair< NonlinearFactorGraph::shared_ptr, Values::shared_ptr > |
using | BetweenFactorPose2s = std::vector< BetweenFactor< Pose2 >::shared_ptr > |
using | BetweenFactorPose3s = std::vector< BetweenFactor< Pose3 >::shared_ptr > |
using | BinaryMeasurementsUnit3 = std::vector< BinaryMeasurement< Unit3 > > |
using | BinaryMeasurementsPoint3 = std::vector< BinaryMeasurement< Point3 > > |
using | BinaryMeasurementsRot3 = std::vector< BinaryMeasurement< Rot3 > > |
typedef Expression< Point2 > | Point2_ |
typedef Expression< Rot2 > | Rot2_ |
typedef Expression< Pose2 > | Pose2_ |
typedef Expression< Point3 > | Point3_ |
typedef Expression< Unit3 > | Unit3_ |
typedef Expression< Rot3 > | Rot3_ |
typedef Expression< Pose3 > | Pose3_ |
typedef Expression< Line3 > | Line3_ |
typedef Expression< OrientedPlane3 > | OrientedPlane3_ |
typedef Expression< Cal3_S2 > | Cal3_S2_ |
typedef Expression< Cal3Bundler > | Cal3Bundler_ |
typedef std::map< Key, std::vector< size_t > > | KeyVectorMap |
typedef std::map< Key, Rot3 > | KeyRotMap |
typedef DSF< int > | DSFInt |
using | Domains = std::map< Key, Domain > |
typedef std::vector< SimPolygon2D > | SimPolygon2DVector |
typedef std::vector< SimWall2D > | SimWall2DVector |
typedef Eigen::RowVectorXd | RowVector |
using | KeyDimMap = std::map< Key, size_t > |
Mapping between variable's key and its corresponding dimensionality. | |
using | LPSolver = ActiveSetSolver< LP, LPPolicy, LPInitSolver > |
using | QPSolver = ActiveSetSolver< QP, QPPolicy, QPInitSolver > |
typedef ConcurrentBatchFilter::Result | ConcurrentBatchFilterResult |
Typedef for Matlab wrapping. | |
typedef ConcurrentBatchSmoother::Result | ConcurrentBatchSmootherResult |
Typedef for Matlab wrapping. | |
typedef ConcurrentIncrementalFilter::Result | ConcurrentIncrementalFilterResult |
Typedef for Matlab wrapping. | |
typedef ConcurrentIncrementalSmoother::Result | ConcurrentIncrementalSmootherResult |
Typedef for Matlab wrapping. | |
typedef FixedLagSmoother::KeyTimestampMap | FixedLagSmootherKeyTimestampMap |
Typedef for matlab wrapping. | |
typedef FixedLagSmootherKeyTimestampMap::value_type | FixedLagSmootherKeyTimestampMapValue |
typedef FixedLagSmoother::Result | FixedLagSmootherResult |
typedef SmartProjectionParams | SmartStereoProjectionParams |
Enumerations | |
enum | GncLossType { GM, TLS } |
Choice of robust loss function for GNC. | |
enum | NoiseFormat { NoiseFormatG2O, NoiseFormatTORO, NoiseFormatGRAPH, NoiseFormatCOV, NoiseFormatAUTO } |
Indicates how noise parameters are stored in file. More... | |
enum | KernelFunctionType { KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY } |
Robust kernel type to wrap around quadratic noise model. | |
enum | LinearizationMode { HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD } |
Linearization mode: what factor to linearize to. More... | |
enum | DegeneracyMode { IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY } |
How to manage degeneracy. | |
Functions | |
template<typename T > | |
void | testDefaultChart (TestResult &result_, const std::string &name_, const T &value) |
GTSAM_EXPORT std::pair< size_t, bool > | choleskyCareful (Matrix &ATA, int order=-1) |
GTSAM_EXPORT bool | choleskyPartial (Matrix &ABC, size_t nFrontal, size_t topleft=0) |
bool GTSAM_EXPORT | guardedIsDebug (const std::string &s) |
void GTSAM_EXPORT | guardedSetDebug (const std::string &s, const bool v) |
bool GTSAM_EXPORT | isDebugVersion () |
IndexPairVector | IndexPairSetAsArray (IndexPairSet &set) |
template<class T > | |
GenericValue< T > | genericValue (const T &v) |
template<typename G > | |
GTSAM_CONCEPT_REQUIRES (IsGroup< G >, bool) check_group_invariants(const G &a | |
Check invariants. | |
template<class Class > | |
Class | between_default (const Class &l1, const Class &l2) |
template<class Class > | |
Vector | logmap_default (const Class &l0, const Class &lp) |
template<class Class > | |
Class | expmap_default (const Class &t, const Vector &d) |
template<class T > | |
T | BCH (const T &X, const T &Y) |
AGC: bracket() only appears in Rot3 tests, should this be used elsewhere? More... | |
template<class T > | |
Matrix | wedge (const Vector &x) |
template<class T > | |
T | expm (const Vector &x, int K=7) |
template<typename T > | |
T | interpolate (const T &X, const T &Y, double t, typename MakeOptionalJacobian< T, T >::type Hx={}, typename MakeOptionalJacobian< T, T >::type Hy={}) |
template<typename T , typename ... Args> | |
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, std::shared_ptr< T > > | make_shared (Args &&... args) |
template<typename T , typename ... Args> | |
gtsam::enable_if_t<!needs_eigen_aligned_allocator< T >::value, std::shared_ptr< T > > | make_shared (Args &&... args) |
Fall back to the boost version if no need for alignment. | |
template<typename T > | |
GTSAM_CONCEPT_REQUIRES (IsTestable< T >, bool) check_manifold_invariants(const T &a | |
Check invariants for Manifold type. | |
const Eigen::IOFormat & | matlabFormat () |
template<class MATRIX > | |
bool | equal_with_abs_tol (const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9) |
bool | operator== (const Matrix &A, const Matrix &B) |
bool | operator!= (const Matrix &A, const Matrix &B) |
GTSAM_EXPORT bool | assert_equal (const Matrix &A, const Matrix &B, double tol=1e-9) |
GTSAM_EXPORT bool | assert_inequal (const Matrix &A, const Matrix &B, double tol=1e-9) |
GTSAM_EXPORT bool | assert_equal (const std::list< Matrix > &As, const std::list< Matrix > &Bs, double tol=1e-9) |
GTSAM_EXPORT bool | linear_independent (const Matrix &A, const Matrix &B, double tol=1e-9) |
GTSAM_EXPORT bool | linear_dependent (const Matrix &A, const Matrix &B, double tol=1e-9) |
GTSAM_EXPORT Vector | operator^ (const Matrix &A, const Vector &v) |
template<class MATRIX > | |
MATRIX | prod (const MATRIX &A, const MATRIX &B) |
GTSAM_EXPORT void | print (const Matrix &A, const std::string &s, std::ostream &stream) |
GTSAM_EXPORT void | print (const Matrix &A, const std::string &s="") |
GTSAM_EXPORT void | save (const Matrix &A, const std::string &s, const std::string &filename) |
GTSAM_EXPORT std::istream & | operator>> (std::istream &inputStream, Matrix &destinationMatrix) |
template<class MATRIX > | |
Eigen::Block< const MATRIX > | sub (const MATRIX &A, size_t i1, size_t i2, size_t j1, size_t j2) |
template<typename Derived1 , typename Derived2 > | |
void | insertSub (Eigen::MatrixBase< Derived1 > &fullMatrix, const Eigen::MatrixBase< Derived2 > &subMatrix, size_t i, size_t j) |
GTSAM_EXPORT Matrix | diag (const std::vector< Matrix > &Hs) |
template<class MATRIX > | |
const MATRIX::ConstColXpr | column (const MATRIX &A, size_t j) |
template<class MATRIX > | |
const MATRIX::ConstRowXpr | row (const MATRIX &A, size_t j) |
template<class MATRIX > | |
void | zeroBelowDiagonal (MATRIX &A, size_t cols=0) |
Matrix | trans (const Matrix &A) |
template<int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions> | |
Reshape< OutM, OutN, OutOptions, InM, InN, InOptions >::ReshapedType | reshape (const Eigen::Matrix< double, InM, InN, InOptions > &m) |
GTSAM_EXPORT std::pair< Matrix, Matrix > | qr (const Matrix &A) |
GTSAM_EXPORT void | inplace_QR (Matrix &A) |
GTSAM_EXPORT std::list< std::tuple< Vector, double, double > > | weighted_eliminate (Matrix &A, Vector &b, const Vector &sigmas) |
GTSAM_EXPORT void | householder_ (Matrix &A, size_t k, bool copy_vectors=true) |
GTSAM_EXPORT void | householder (Matrix &A, size_t k) |
GTSAM_EXPORT Vector | backSubstituteUpper (const Matrix &U, const Vector &b, bool unit=false) |
GTSAM_EXPORT Vector | backSubstituteUpper (const Vector &b, const Matrix &U, bool unit=false) |
GTSAM_EXPORT Vector | backSubstituteLower (const Matrix &L, const Vector &b, bool unit=false) |
GTSAM_EXPORT Matrix | stack (size_t nrMatrices,...) |
GTSAM_EXPORT Matrix | stack (const std::vector< Matrix > &blocks) |
GTSAM_EXPORT Matrix | collect (const std::vector< const Matrix *> &matrices, size_t m=0, size_t n=0) |
GTSAM_EXPORT Matrix | collect (size_t nrMatrices,...) |
GTSAM_EXPORT void | vector_scale_inplace (const Vector &v, Matrix &A, bool inf_mask=false) |
GTSAM_EXPORT Matrix | vector_scale (const Vector &v, const Matrix &A, bool inf_mask=false) |
GTSAM_EXPORT Matrix | vector_scale (const Matrix &A, const Vector &v, bool inf_mask=false) |
Matrix3 | skewSymmetric (double wx, double wy, double wz) |
template<class Derived > | |
Matrix3 | skewSymmetric (const Eigen::MatrixBase< Derived > &w) |
GTSAM_EXPORT Matrix | inverse_square_root (const Matrix &A) |
GTSAM_EXPORT Matrix | cholesky_inverse (const Matrix &A) |
GTSAM_EXPORT void | svd (const Matrix &A, Matrix &U, Vector &S, Matrix &V) |
GTSAM_EXPORT std::tuple< int, double, Vector > | DLT (const Matrix &A, double rank_tol=1e-9) |
GTSAM_EXPORT Matrix | expm (const Matrix &A, size_t K=7) |
std::string | formatMatrixIndented (const std::string &label, const Matrix &matrix, bool makeVectorHorizontal=false) |
GTSAM_EXPORT Matrix | LLt (const Matrix &A) |
GTSAM_EXPORT Matrix | RtR (const Matrix &A) |
GTSAM_EXPORT Vector | columnNormSquare (const Matrix &A) |
template<class X , int N = traits<X>::dimension> | |
Eigen::Matrix< double, N, 1 > | numericalGradient (std::function< double(const X &)> h, const X &x, double delta=1e-5) |
Numerically compute gradient of scalar function. More... | |
template<class Y , class X , int N = traits<X>::dimension> | |
internal::FixedSizeMatrix< Y, X >::type | numericalDerivative11 (std::function< Y(const X &)> h, const X &x, double delta=1e-5) |
New-style numerical derivatives using manifold_traits. More... | |
template<class Y , class X > | |
internal::FixedSizeMatrix< Y, X >::type | numericalDerivative11 (Y(*h)(const X &), const X &x, double delta=1e-5) |
template<class Y , class X1 , class X2 , int N = traits<X1>::dimension> | |
internal::FixedSizeMatrix< Y, X1 >::type | numericalDerivative21 (const std::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5) |
template<class Y , class X1 , class X2 > | |
internal::FixedSizeMatrix< Y, X1 >::type | numericalDerivative21 (Y(*h)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5) |
template<class Y , class X1 , class X2 , int N = traits<X2>::dimension> | |
internal::FixedSizeMatrix< Y, X2 >::type | numericalDerivative22 (std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5) |
template<class Y , class X1 , class X2 > | |
internal::FixedSizeMatrix< Y, X2 >::type | numericalDerivative22 (Y(*h)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , int N = traits<X1>::dimension> | |
internal::FixedSizeMatrix< Y, X1 >::type | numericalDerivative31 (std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 > | |
internal::FixedSizeMatrix< Y, X1 >::type | numericalDerivative31 (Y(*h)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , int N = traits<X2>::dimension> | |
internal::FixedSizeMatrix< Y, X2 >::type | numericalDerivative32 (std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 > | |
internal::FixedSizeMatrix< Y, X2 >::type | numericalDerivative32 (Y(*h)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , int N = traits<X3>::dimension> | |
internal::FixedSizeMatrix< Y, X3 >::type | numericalDerivative33 (std::function< Y(const X1 &, const X2 &, const X3 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 > | |
internal::FixedSizeMatrix< Y, X3 >::type | numericalDerivative33 (Y(*h)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , int N = traits<X1>::dimension> | |
internal::FixedSizeMatrix< Y, X1 >::type | numericalDerivative41 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 > | |
internal::FixedSizeMatrix< Y, X1 >::type | numericalDerivative41 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , int N = traits<X2>::dimension> | |
internal::FixedSizeMatrix< Y, X2 >::type | numericalDerivative42 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 > | |
internal::FixedSizeMatrix< Y, X2 >::type | numericalDerivative42 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , int N = traits<X3>::dimension> | |
internal::FixedSizeMatrix< Y, X3 >::type | numericalDerivative43 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 > | |
internal::FixedSizeMatrix< Y, X3 >::type | numericalDerivative43 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , int N = traits<X4>::dimension> | |
internal::FixedSizeMatrix< Y, X4 >::type | numericalDerivative44 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 > | |
internal::FixedSizeMatrix< Y, X4 >::type | numericalDerivative44 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , int N = traits<X1>::dimension> | |
internal::FixedSizeMatrix< Y, X1 >::type | numericalDerivative51 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 > | |
internal::FixedSizeMatrix< Y, X1 >::type | numericalDerivative51 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , int N = traits<X2>::dimension> | |
internal::FixedSizeMatrix< Y, X2 >::type | numericalDerivative52 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 > | |
internal::FixedSizeMatrix< Y, X2 >::type | numericalDerivative52 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , int N = traits<X3>::dimension> | |
internal::FixedSizeMatrix< Y, X3 >::type | numericalDerivative53 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 > | |
internal::FixedSizeMatrix< Y, X3 >::type | numericalDerivative53 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , int N = traits<X4>::dimension> | |
internal::FixedSizeMatrix< Y, X4 >::type | numericalDerivative54 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 > | |
internal::FixedSizeMatrix< Y, X4 >::type | numericalDerivative54 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , int N = traits<X5>::dimension> | |
internal::FixedSizeMatrix< Y, X5 >::type | numericalDerivative55 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 > | |
internal::FixedSizeMatrix< Y, X5 >::type | numericalDerivative55 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X1>::dimension> | |
internal::FixedSizeMatrix< Y, X1 >::type | numericalDerivative61 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 > | |
internal::FixedSizeMatrix< Y, X1 >::type | numericalDerivative61 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X2>::dimension> | |
internal::FixedSizeMatrix< Y, X2 >::type | numericalDerivative62 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 > | |
internal::FixedSizeMatrix< Y, X2 >::type | numericalDerivative62 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X3>::dimension> | |
internal::FixedSizeMatrix< Y, X3 >::type | numericalDerivative63 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 > | |
internal::FixedSizeMatrix< Y, X3 >::type | numericalDerivative63 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X4>::dimension> | |
internal::FixedSizeMatrix< Y, X4 >::type | numericalDerivative64 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 > | |
internal::FixedSizeMatrix< Y, X4 >::type | numericalDerivative64 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X5>::dimension> | |
internal::FixedSizeMatrix< Y, X5 >::type | numericalDerivative65 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 > | |
internal::FixedSizeMatrix< Y, X5 >::type | numericalDerivative65 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 , int N = traits<X6>::dimension> | |
internal::FixedSizeMatrix< Y, X6 >::type | numericalDerivative66 (std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> h, const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5) |
template<class Y , class X1 , class X2 , class X3 , class X4 , class X5 , class X6 > | |
internal::FixedSizeMatrix< Y, X6 >::type | numericalDerivative66 (Y(*h)(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &), const X1 &x1, const X2 &x2, const X3 &x3, const X4 &x4, const X5 &x5, const X6 &x6, double delta=1e-5) |
template<class X > | |
internal::FixedSizeMatrix< X, X >::type | numericalHessian (std::function< double(const X &)> f, const X &x, double delta=1e-5) |
template<class X > | |
internal::FixedSizeMatrix< X, X >::type | numericalHessian (double(*f)(const X &), const X &x, double delta=1e-5) |
template<class X1 , class X2 > | |
internal::FixedSizeMatrix< X1, X2 >::type | numericalHessian212 (std::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5) |
template<class X1 , class X2 > | |
internal::FixedSizeMatrix< X1, X2 >::type | numericalHessian212 (double(*f)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5) |
template<class X1 , class X2 > | |
internal::FixedSizeMatrix< X1, X1 >::type | numericalHessian211 (std::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5) |
template<class X1 , class X2 > | |
internal::FixedSizeMatrix< X1, X1 >::type | numericalHessian211 (double(*f)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5) |
template<class X1 , class X2 > | |
internal::FixedSizeMatrix< X2, X2 >::type | numericalHessian222 (std::function< double(const X1 &, const X2 &)> f, const X1 &x1, const X2 &x2, double delta=1e-5) |
template<class X1 , class X2 > | |
internal::FixedSizeMatrix< X2, X2 >::type | numericalHessian222 (double(*f)(const X1 &, const X2 &), const X1 &x1, const X2 &x2, double delta=1e-5) |
template<class X1 , class X2 , class X3 > | |
internal::FixedSizeMatrix< X1, X1 >::type | numericalHessian311 (std::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5) |
template<class X1 , class X2 , class X3 > | |
internal::FixedSizeMatrix< X1, X1 >::type | numericalHessian311 (double(*f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5) |
template<class X1 , class X2 , class X3 > | |
internal::FixedSizeMatrix< X2, X2 >::type | numericalHessian322 (std::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5) |
template<class X1 , class X2 , class X3 > | |
internal::FixedSizeMatrix< X2, X2 >::type | numericalHessian322 (double(*f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5) |
template<class X1 , class X2 , class X3 > | |
internal::FixedSizeMatrix< X3, X3 >::type | numericalHessian333 (std::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5) |
template<class X1 , class X2 , class X3 > | |
internal::FixedSizeMatrix< X3, X3 >::type | numericalHessian333 (double(*f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5) |
template<class X1 , class X2 , class X3 > | |
internal::FixedSizeMatrix< X1, X2 >::type | numericalHessian312 (std::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5) |
template<class X1 , class X2 , class X3 > | |
internal::FixedSizeMatrix< X1, X3 >::type | numericalHessian313 (std::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5) |
template<class X1 , class X2 , class X3 > | |
internal::FixedSizeMatrix< X2, X3 >::type | numericalHessian323 (std::function< double(const X1 &, const X2 &, const X3 &)> f, const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5) |
template<class X1 , class X2 , class X3 > | |
internal::FixedSizeMatrix< X1, X2 >::type | numericalHessian312 (double(*f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5) |
template<class X1 , class X2 , class X3 > | |
internal::FixedSizeMatrix< X1, X3 >::type | numericalHessian313 (double(*f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5) |
template<class X1 , class X2 , class X3 > | |
internal::FixedSizeMatrix< X2, X3 >::type | numericalHessian323 (double(*f)(const X1 &, const X2 &, const X3 &), const X1 &x1, const X2 &x2, const X3 &x3, double delta=1e-5) |
void | print (float v, const std::string &s="") |
void | print (double v, const std::string &s="") |
template<class T > | |
bool | equal (const T &obj1, const T &obj2, double tol) |
template<class T > | |
bool | equal (const T &obj1, const T &obj2) |
template<class V > | |
bool | assert_equal (const V &expected, const V &actual, double tol=1e-9) |
bool | assert_equal (const Key &expected, const Key &actual, double tol=0.0) |
template<class V > | |
bool | assert_equal (const std::optional< V > &expected, const std::optional< V > &actual, double tol=1e-9) |
template<class V > | |
bool | assert_equal (const V &expected, const std::optional< V > &actual, double tol=1e-9) |
template<class V > | |
bool | assert_equal (const V &expected, const std::optional< std::reference_wrapper< const V >> &actual, double tol=1e-9) |
template<class V1 , class V2 > | |
bool | assert_container_equal (const std::map< V1, V2 > &expected, const std::map< V1, V2 > &actual, double tol=1e-9) |
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) |
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) |
template<class V > | |
bool | assert_container_equal (const V &expected, const V &actual, double tol=1e-9) |
template<class V2 > | |
bool | assert_container_equality (const std::map< size_t, V2 > &expected, const std::map< size_t, V2 > &actual) |
template<class V > | |
bool | assert_container_equality (const V &expected, const V &actual) |
bool | assert_equal (const std::string &expected, const std::string &actual) |
template<class V > | |
bool | assert_inequal (const V &expected, const V &actual, double tol=1e-9) |
template<class V > | |
bool | assert_stdout_equal (const std::string &expected, const V &actual) |
template<class V > | |
bool | assert_print_equal (const std::string &expected, const V &actual, const std::string &s="") |
template<typename G > | |
void | testLieGroupDerivatives (TestResult &result_, const std::string &name_, const G &t1, const G &t2) |
template<typename G > | |
void | testChartDerivatives (TestResult &result_, const std::string &name_, const G &t1, const G &t2) |
void | tictoc_finishedIteration_ () |
void | tictoc_print_ () |
void | tictoc_print2_ () |
void | tictoc_reset_ () |
std::string GTSAM_EXPORT | demangle (const char *name) |
Function to demangle type name of variable, e.g. demangle(typeid(x).name()) | |
GTSAM_EXPORT bool | fpEqual (double a, double b, double tol, bool check_relative_also=true) |
GTSAM_EXPORT void | print (const Vector &v, const std::string &s, std::ostream &stream) |
GTSAM_EXPORT void | print (const Vector &v, const std::string &s="") |
GTSAM_EXPORT void | save (const Vector &A, const std::string &s, const std::string &filename) |
GTSAM_EXPORT bool | operator== (const Vector &vec1, const Vector &vec2) |
GTSAM_EXPORT bool | greaterThanOrEqual (const Vector &v1, const Vector &v2) |
GTSAM_EXPORT bool | equal_with_abs_tol (const Vector &vec1, const Vector &vec2, double tol=1e-9) |
GTSAM_EXPORT bool | equal_with_abs_tol (const SubVector &vec1, const SubVector &vec2, double tol=1e-9) |
bool | equal (const Vector &vec1, const Vector &vec2, double tol) |
bool | equal (const Vector &vec1, const Vector &vec2) |
GTSAM_EXPORT bool | assert_equal (const Vector &vec1, const Vector &vec2, double tol=1e-9) |
GTSAM_EXPORT bool | assert_inequal (const Vector &vec1, const Vector &vec2, double tol=1e-9) |
GTSAM_EXPORT bool | assert_equal (const SubVector &vec1, const SubVector &vec2, double tol=1e-9) |
GTSAM_EXPORT bool | assert_equal (const ConstSubVector &vec1, const ConstSubVector &vec2, double tol=1e-9) |
GTSAM_EXPORT bool | linear_dependent (const Vector &vec1, const Vector &vec2, double tol=1e-9) |
GTSAM_EXPORT Vector | ediv_ (const Vector &a, const Vector &b) |
template<class V1 , class V2 > | |
double | dot (const V1 &a, const V2 &b) |
template<class V1 , class V2 > | |
double | inner_prod (const V1 &a, const V2 &b) |
GTSAM_EXPORT std::pair< double, Vector > | house (const Vector &x) |
GTSAM_EXPORT double | houseInPlace (Vector &x) |
GTSAM_EXPORT std::pair< Vector, double > | weightedPseudoinverse (const Vector &v, const Vector &weights) |
GTSAM_EXPORT double | weightedPseudoinverse (const Vector &a, const Vector &weights, Vector &pseudo) |
GTSAM_EXPORT Vector | concatVectors (const std::list< Vector > &vs) |
GTSAM_EXPORT Vector | concatVectors (size_t nrVectors,...) |
template<size_t M> | |
Matrix | kroneckerProductIdentity (const Weights &w) |
Function for computing the kronecker product of the 1*N Weight vector w with the MxM identity matrix I efficiently. The main reason for this is so we don't need to use Eigen's Unsupported library. More... | |
template<int M> | |
std::ostream & | operator<< (std::ostream &os, const ParameterMatrix< M > ¶meterMatrix) |
template<typename L , typename Y > | |
DecisionTree< L, Y > | apply (const DecisionTree< L, Y > &f, const typename DecisionTree< L, Y >::Unary &op) |
Apply unary operator op to DecisionTree f . More... | |
template<typename L , typename Y > | |
DecisionTree< L, Y > | apply (const DecisionTree< L, Y > &f, const typename DecisionTree< L, Y >::UnaryAssignment &op) |
Apply unary operator op with Assignment to DecisionTree f . | |
template<typename L , typename Y > | |
DecisionTree< L, Y > | apply (const DecisionTree< L, Y > &f, const DecisionTree< L, Y > &g, const typename DecisionTree< L, Y >::Binary &op) |
Apply binary operator op to DecisionTree f . | |
template<typename L , typename T1 , typename T2 > | |
std::pair< DecisionTree< L, T1 >, DecisionTree< L, T2 > > | unzip (const DecisionTree< L, std::pair< T1, T2 > > &input) |
unzip a DecisionTree with std::pair values. More... | |
std::vector< double > | expNormalize (const std::vector< double > &logProbs) |
Normalize a set of log probabilities. More... | |
GTSAM_EXPORT std::pair< std::shared_ptr< DiscreteConditional >, DecisionTreeFactor::shared_ptr > | EliminateDiscrete (const DiscreteFactorGraph &factors, const Ordering &keys) |
Main elimination function for DiscreteFactorGraph. More... | |
std::pair< DiscreteConditional::shared_ptr, DecisionTreeFactor::shared_ptr > | EliminateForMPE (const DiscreteFactorGraph &factors, const Ordering &frontalKeys) |
GTSAM_EXPORT DiscreteKeys | operator & (const DiscreteKey &key1, const DiscreteKey &key2) |
Create a list from two keys. | |
std::string | markdown (const DiscreteValues &values, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DiscreteValues::Names &names={}) |
Free version of markdown. | |
std::string | html (const DiscreteValues &values, const KeyFormatter &keyFormatter=DefaultKeyFormatter, const DiscreteValues::Names &names={}) |
Free version of html. | |
GTSAM_EXPORT Signature | operator| (const DiscreteKey &key, const DiscreteKey &parent) |
GTSAM_EXPORT Signature | operator% (const DiscreteKey &key, const std::string &parent) |
GTSAM_EXPORT Signature | operator% (const DiscreteKey &key, const Signature::Table &parent) |
template<typename Cal , size_t Dim> | |
void | calibrateJacobians (const Cal &calibration, const Point2 &pn, OptionalJacobian< 2, Dim > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) |
GTSAM_EXPORT Line3 | transformTo (const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose={}, OptionalJacobian< 4, 4 > Dline={}) |
GTSAM_EXPORT std::ostream & | operator<< (std::ostream &os, const gtsam::Point2Pair &p) |
GTSAM_EXPORT double | norm2 (const Point2 &p, OptionalJacobian< 1, 2 > H={}) |
Distance of the point from the origin, with Jacobian. | |
GTSAM_EXPORT double | distance2 (const Point2 &p1, const Point2 &q, OptionalJacobian< 1, 2 > H1={}, OptionalJacobian< 1, 2 > H2={}) |
distance between two points | |
Point2 | operator* (double s, const Point2 &p) |
multiply with scalar | |
GTSAM_EXPORT std::optional< Point2 > | circleCircleIntersection (double R_d, double r_d, double tol=1e-9) |
GTSAM_EXPORT std::list< Point2 > | circleCircleIntersection (Point2 c1, Point2 c2, std::optional< Point2 > fh) |
GTSAM_EXPORT Point2Pair | means (const std::vector< Point2Pair > &abPointPairs) |
Calculate the two means of a set of Point2 pairs. | |
GTSAM_EXPORT std::list< Point2 > | circleCircleIntersection (Point2 c1, double r1, Point2 c2, double r2, double tol=1e-9) |
Intersect 2 circles. More... | |
GTSAM_EXPORT std::ostream & | operator<< (std::ostream &os, const gtsam::Point3Pair &p) |
GTSAM_EXPORT double | distance3 (const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1={}, OptionalJacobian< 1, 3 > H2={}) |
distance between two points | |
GTSAM_EXPORT double | norm3 (const Point3 &p, OptionalJacobian< 1, 3 > H={}) |
Distance of the point from the origin, with Jacobian. | |
GTSAM_EXPORT Point3 | normalize (const Point3 &p, OptionalJacobian< 3, 3 > H={}) |
normalize, with optional Jacobian | |
GTSAM_EXPORT Point3 | cross (const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H_p={}, OptionalJacobian< 3, 3 > H_q={}) |
cross product More... | |
GTSAM_EXPORT double | dot (const Point3 &p, const Point3 &q, OptionalJacobian< 1, 3 > H_p={}, OptionalJacobian< 1, 3 > H_q={}) |
dot product | |
template<class CONTAINER > | |
Point3 | mean (const CONTAINER &points) |
mean | |
GTSAM_EXPORT Point3Pair | means (const std::vector< Point3Pair > &abPointPairs) |
Calculate the two means of a set of Point3 pairs. | |
template<> | |
Matrix | wedge< Pose2 > (const Vector &xi) |
template<> | |
Matrix | wedge< Pose3 > (const Vector &xi) |
GTSAM_EXPORT std::pair< Matrix3, Vector3 > | RQ (const Matrix3 &A, OptionalJacobian< 3, 9 > H={}) |
template<> | |
Matrix | wedge< Similarity3 > (const Vector &xi) |
GTSAM_EXPORT Matrix3 | topLeft (const SO4 &Q, OptionalJacobian< 9, 6 > H={}) |
GTSAM_EXPORT Matrix43 | stiefel (const SO4 &Q, OptionalJacobian< 12, 6 > H={}) |
GTSAM_EXPORT Vector4 | triangulateHomogeneousDLT (const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 >> &projection_matrices, const Point2Vector &measurements, double rank_tol=1e-9) |
GTSAM_EXPORT Vector4 | triangulateHomogeneousDLT (const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 >> &projection_matrices, const std::vector< Unit3 > &measurements, double rank_tol=1e-9) |
GTSAM_EXPORT Point3 | triangulateDLT (const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 >> &projection_matrices, const Point2Vector &measurements, double rank_tol=1e-9) |
GTSAM_EXPORT Point3 | triangulateDLT (const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 >> &projection_matrices, const std::vector< Unit3 > &measurements, double rank_tol=1e-9) |
GTSAM_EXPORT Point3 | triangulateLOST (const std::vector< Pose3 > &poses, const Point3Vector &calibratedMeasurements, const SharedIsotropic &measurementNoise) |
Triangulation using the LOST (Linear Optimal Sine Triangulation) algorithm proposed in https://arxiv.org/pdf/2205.12197.pdf by Sebastien Henry and John Christian. More... | |
template<class CALIBRATION > | |
std::pair< NonlinearFactorGraph, Values > | triangulationGraph (const std::vector< Pose3 > &poses, std::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, Key landmarkKey, const Point3 &initialEstimate, const SharedNoiseModel &model=noiseModel::Unit::Create(2)) |
template<class CAMERA > | |
std::pair< NonlinearFactorGraph, Values > | triangulationGraph (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements, Key landmarkKey, const Point3 &initialEstimate, const SharedNoiseModel &model=nullptr) |
GTSAM_EXPORT Point3 | optimize (const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey) |
template<class CALIBRATION > | |
Point3 | triangulateNonlinear (const std::vector< Pose3 > &poses, std::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, const Point3 &initialEstimate, const SharedNoiseModel &model=nullptr) |
template<class CAMERA > | |
Point3 | triangulateNonlinear (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements, const Point3 &initialEstimate, const SharedNoiseModel &model=nullptr) |
template<class CAMERA > | |
std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > | projectionMatricesFromCameras (const CameraSet< CAMERA > &cameras) |
template<class CALIBRATION > | |
std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > | projectionMatricesFromPoses (const std::vector< Pose3 > &poses, std::shared_ptr< CALIBRATION > sharedCal) |
template<class CALIBRATION > | |
Cal3_S2 | createPinholeCalibration (const CALIBRATION &cal) |
template<class CALIBRATION , class MEASUREMENT > | |
MEASUREMENT | undistortMeasurementInternal (const CALIBRATION &cal, const MEASUREMENT &measurement, std::optional< Cal3_S2 > pinholeCal={}) |
template<class CALIBRATION > | |
Point2Vector | undistortMeasurements (const CALIBRATION &cal, const Point2Vector &measurements) |
template<> | |
Point2Vector | undistortMeasurements (const Cal3_S2 &cal, const Point2Vector &measurements) |
template<class CAMERA > | |
CAMERA::MeasurementVector | undistortMeasurements (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements) |
template<class CAMERA = PinholeCamera<Cal3_S2>> | |
PinholeCamera< Cal3_S2 >::MeasurementVector | undistortMeasurements (const CameraSet< PinholeCamera< Cal3_S2 >> &cameras, const PinholeCamera< Cal3_S2 >::MeasurementVector &measurements) |
template<class CAMERA = SphericalCamera> | |
SphericalCamera::MeasurementVector | undistortMeasurements (const CameraSet< SphericalCamera > &cameras, const SphericalCamera::MeasurementVector &measurements) |
template<class CALIBRATION > | |
Point3Vector | calibrateMeasurementsShared (const CALIBRATION &cal, const Point2Vector &measurements) |
template<class CAMERA > | |
Point3Vector | calibrateMeasurements (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements) |
template<class CAMERA = SphericalCamera> | |
Point3Vector | calibrateMeasurements (const CameraSet< SphericalCamera > &cameras, const SphericalCamera::MeasurementVector &measurements) |
template<class CALIBRATION > | |
Point3 | triangulatePoint3 (const std::vector< Pose3 > &poses, std::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, double rank_tol=1e-9, bool optimize=false, const SharedNoiseModel &model=nullptr, const bool useLOST=false) |
template<class CAMERA > | |
Point3 | triangulatePoint3 (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements, double rank_tol=1e-9, bool optimize=false, const SharedNoiseModel &model=nullptr, const bool useLOST=false) |
template<class CALIBRATION > | |
Point3 | triangulatePoint3 (const CameraSet< PinholeCamera< CALIBRATION >> &cameras, const Point2Vector &measurements, double rank_tol=1e-9, bool optimize=false, const SharedNoiseModel &model=nullptr, const bool useLOST=false) |
Pinhole-specific version. | |
template<class CAMERA > | |
TriangulationResult | triangulateSafe (const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters ¶ms) |
triangulateSafe: extensive checking of the outcome | |
std::set< DiscreteKey > | DiscreteKeysAsSet (const DiscreteKeys &discreteKeys) |
Return the DiscreteKey vector as a set. | |
KeyVector | CollectKeys (const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys) |
KeyVector | CollectKeys (const KeyVector &keys1, const KeyVector &keys2) |
DiscreteKeys | CollectDiscreteKeys (const DiscreteKeys &key1, const DiscreteKeys &key2) |
GTSAM_EXPORT std::pair< std::shared_ptr< HybridConditional >, std::shared_ptr< Factor > > | EliminateHybrid (const HybridGaussianFactorGraph &factors, const Ordering &keys) |
Main elimination function for HybridGaussianFactorGraph. More... | |
GTSAM_EXPORT const Ordering | HybridOrdering (const HybridGaussianFactorGraph &graph) |
Return a Colamd constrained ordering where the discrete keys are eliminated after the continuous keys. More... | |
HybridGaussianFactorGraph::shared_ptr | makeSwitchingChain (size_t n, std::function< Key(int)> keyFunc=X, std::function< Key(int)> dKeyFunc=M) |
Create a switching system chain. A switching system is a continuous system which depends on a discrete mode at each time step of the chain. More... | |
std::pair< KeyVector, std::vector< int > > | makeBinaryOrdering (std::vector< Key > &input) |
Return the ordering as a binary tree such that all parent nodes are above their children. 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) |
template<class G , class F , class KEY > | |
SDGraph< KEY > | toBoostGraph (const G &graph) |
template<class G , class V , class KEY > | |
std::tuple< G, V, std::map< KEY, V > > | predecessorMap2Graph (const PredecessorMap< KEY > &p_map) |
template<class G , class Factor , class POSE , class KEY > | |
std::shared_ptr< Values > | composePoses (const G &graph, const PredecessorMap< KEY > &tree, const POSE &rootPose) |
template<class G , class KEY , class FACTOR2 > | |
PredecessorMap< KEY > | findMinimumSpanningTree (const G &fg) |
template<class G , class KEY , class FACTOR2 > | |
void | split (const G &g, const PredecessorMap< KEY > &tree, G &Ab1, G &Ab2) |
GTSAM_EXPORT std::string | _defaultKeyFormatter (Key key) |
GTSAM_EXPORT std::string | _multirobotKeyFormatter (gtsam::Key key) |
GTSAM_EXPORT void | PrintKey (Key key, const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) |
Utility function to print one key with optional prefix. | |
GTSAM_EXPORT void | PrintKeyList (const KeyList &keys, const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) |
Utility function to print sets of keys with optional prefix. | |
GTSAM_EXPORT void | PrintKeyVector (const KeyVector &keys, const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) |
Utility function to print sets of keys with optional prefix. | |
GTSAM_EXPORT 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) |
unsigned char | mrsymbolChr (Key key) |
unsigned char | mrsymbolLabel (Key key) |
size_t | mrsymbolIndex (Key key) |
Key | symbol (unsigned char c, std::uint64_t j) |
unsigned char | symbolChr (Key key) |
std::uint64_t | symbolIndex (Key key) |
template<class S , class V > | |
V | preconditionedConjugateGradient (const S &system, const V &initial, const ConjugateGradientParameters ¶meters) |
GTSAM_EXPORT Errors | createErrors (const VectorValues &V) |
Break V into pieces according to its start indices. | |
GTSAM_EXPORT void | print (const Errors &e, const std::string &s="Errors") |
Print an Errors instance. | |
GTSAM_EXPORT bool | equality (const Errors &actual, const Errors &expected, double tol=1e-9) |
GTSAM_EXPORT Errors | operator+ (const Errors &a, const Errors &b) |
Addition. | |
GTSAM_EXPORT Errors | operator- (const Errors &a, const Errors &b) |
Subtraction. | |
GTSAM_EXPORT Errors | operator- (const Errors &a) |
Negation. | |
GTSAM_EXPORT double | dot (const Errors &a, const Errors &b) |
Dot product. | |
GTSAM_EXPORT void | axpy (double alpha, const Errors &x, Errors &y) |
BLAS level 2 style AXPY, y := alpha*x + y | |
GTSAM_EXPORT bool | hasConstraints (const GaussianFactorGraph &factors) |
GTSAM_EXPORT std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< HessianFactor > > | EliminateCholesky (const GaussianFactorGraph &factors, const Ordering &keys) |
GTSAM_EXPORT std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< GaussianFactor > > | EliminatePreferCholesky (const GaussianFactorGraph &factors, const Ordering &keys) |
template<class S , class V , class E > | |
V | conjugateGradients (const S &Ab, V x, const ConjugateGradientParameters ¶meters, bool steepest) |
GTSAM_EXPORT Vector | steepestDescent (const System &Ab, const Vector &x, const IterativeOptimizationParameters ¶meters) |
GTSAM_EXPORT Vector | conjugateGradientDescent (const System &Ab, const Vector &x, const ConjugateGradientParameters ¶meters) |
GTSAM_EXPORT Vector | steepestDescent (const Matrix &A, const Vector &b, const Vector &x, const ConjugateGradientParameters ¶meters) |
GTSAM_EXPORT Vector | conjugateGradientDescent (const Matrix &A, const Vector &b, const Vector &x, const ConjugateGradientParameters ¶meters) |
GTSAM_EXPORT VectorValues | steepestDescent (const GaussianFactorGraph &fg, const VectorValues &x, const ConjugateGradientParameters ¶meters) |
GTSAM_EXPORT VectorValues | conjugateGradientDescent (const GaussianFactorGraph &fg, const VectorValues &x, const ConjugateGradientParameters ¶meters) |
GTSAM_EXPORT std::pair< std::shared_ptr< GaussianConditional >, std::shared_ptr< JacobianFactor > > | EliminateQR (const GaussianFactorGraph &factors, const Ordering &keys) |
std::shared_ptr< Preconditioner > | createPreconditioner (const std::shared_ptr< PreconditionerParameters > parameters) |
SparseEigen | sparseJacobianEigen (const GaussianFactorGraph &gfg, const Ordering &ordering) |
Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph. | |
SparseEigen | sparseJacobianEigen (const GaussianFactorGraph &gfg) |
GaussianFactorGraph | buildFactorSubgraph (const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone) |
std::pair< GaussianFactorGraph, GaussianFactorGraph > | splitFactorGraph (const GaussianFactorGraph &factorGraph, const Subgraph &subgraph) |
Rot3_ | attitude (const NavState_ &X) |
Point3_ | position (const NavState_ &X) |
Velocity3_ | velocity (const NavState_ &X) |
std::unique_ptr< internal::ExecutionTraceStorage[]> | allocAligned (size_t size) |
template<typename T > | |
Expression< T > | operator* (const Expression< T > &expression1, const Expression< T > &expression2) |
Construct a product expression, assumes T::compose(T) -> T. More... | |
template<typename T > | |
std::vector< Expression< T > > | createUnknowns (size_t n, char c, size_t start) |
Construct an array of leaves. More... | |
template<typename T , typename A > | |
Expression< T > | linearExpression (const std::function< T(A)> &f, const Expression< A > &expression, const Eigen::Matrix< double, traits< T >::dimension, traits< A >::dimension > &dTdA) |
template<typename T > | |
ScalarMultiplyExpression< T > | operator* (double s, const Expression< T > &e) |
template<typename T > | |
BinarySumExpression< T > | operator+ (const Expression< T > &e1, const Expression< T > &e2) |
template<typename T > | |
BinarySumExpression< T > | operator- (const Expression< T > &e1, const Expression< T > &e2) |
Construct an expression that subtracts one expression from another. | |
template<typename T > | |
Expression< T > | between (const Expression< T > &t1, const Expression< T > &t2) |
template<typename T > | |
Expression< T > | compose (const Expression< T > &t1, const Expression< T > &t2) |
JacobianFactor | linearizeNumerically (const NoiseModelFactor &factor, const Values &values, double delta=1e-5) |
template<typename T , typename R , typename FUNC > | |
FunctorizedFactor< R, T > | MakeFunctorizedFactor (Key key, const R &z, const SharedNoiseModel &model, const FUNC func) |
template<typename T1 , typename T2 , typename R , typename FUNC > | |
FunctorizedFactor2< R, T1, T2 > | MakeFunctorizedFactor2 (Key key1, Key key2, const R &z, const SharedNoiseModel &model, const FUNC func) |
size_t | optimizeWildfire (const ISAM2Clique::shared_ptr &root, double threshold, const KeySet &replaced, VectorValues *delta) |
size_t | optimizeWildfireNonRecursive (const ISAM2Clique::shared_ptr &root, double threshold, const KeySet &replaced, VectorValues *delta) |
template<class S , class V , class W > | |
double | lineSearch (const S &system, const V currentValues, const W &gradient) |
template<class S , class V > | |
std::tuple< V, int > | nonlinearConjugateGradient (const S &system, const V &initial, const NonlinearOptimizerParams ¶ms, const bool singleIteration, const bool gradientDescent=false) |
GTSAM_EXPORT bool | checkConvergence (double relativeErrorTreshold, double absoluteErrorTreshold, double errorThreshold, double currentError, double newError, NonlinearOptimizerParams::Verbosity verbosity=NonlinearOptimizerParams::SILENT) |
GTSAM_EXPORT bool | checkConvergence (const NonlinearOptimizerParams ¶ms, double currentError, double newError) |
GTSAM_EXPORT SfmData | readBal (const std::string &filename) |
This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data as a SfmData structure. Mainly used by wrapped code. More... | |
GTSAM_EXPORT bool | writeBAL (const std::string &filename, const SfmData &data) |
This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure. More... | |
GTSAM_EXPORT bool | writeBALfromValues (const std::string &filename, const SfmData &data, const Values &values) |
This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure and a value structure (measurements are the same as the SfM input data, while camera poses and values are read from Values) More... | |
GTSAM_EXPORT Pose3 | openGL2gtsam (const Rot3 &R, double tx, double ty, double tz) |
This function converts an openGL camera pose to an GTSAM camera pose. More... | |
GTSAM_EXPORT Pose3 | gtsam2openGL (const Rot3 &R, double tx, double ty, double tz) |
This function converts a GTSAM camera pose to an openGL camera pose. More... | |
GTSAM_EXPORT Pose3 | gtsam2openGL (const Pose3 &PoseGTSAM) |
This function converts a GTSAM camera pose to an openGL camera pose. More... | |
GTSAM_EXPORT Values | initialCamerasEstimate (const SfmData &db) |
This function creates initial values for cameras from db. More... | |
GTSAM_EXPORT Values | initialCamerasAndPointsEstimate (const SfmData &db) |
This function creates initial values for cameras and points from db. More... | |
GTSAM_EXPORT std::string | findExampleDataFile (const std::string &name) |
GTSAM_EXPORT std::string | createRewrittenFileName (const std::string &name) |
template<typename T > | |
GTSAM_EXPORT std::map< size_t, T > | parseVariables (const std::string &filename, size_t maxIndex=0) |
template<typename T > | |
GTSAM_EXPORT std::vector< BinaryMeasurement< T > > | parseMeasurements (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model=nullptr, size_t maxIndex=0) |
template<typename T > | |
GTSAM_EXPORT std::vector< typename BetweenFactor< T >::shared_ptr > | parseFactors (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model=nullptr, size_t maxIndex=0) |
GTSAM_EXPORT std::optional< IndexedPose > | parseVertexPose (std::istream &is, const std::string &tag) |
GTSAM_EXPORT std::optional< IndexedLandmark > | parseVertexLandmark (std::istream &is, const std::string &tag) |
GTSAM_EXPORT std::optional< IndexedEdge > | parseEdge (std::istream &is, const std::string &tag) |
GTSAM_EXPORT GraphAndValues | load2D (std::pair< std::string, SharedNoiseModel > dataset, size_t maxIndex=0, bool addNoise=false, bool smart=true, NoiseFormat noiseFormat=NoiseFormatAUTO, KernelFunctionType kernelFunctionType=KernelFunctionTypeNONE) |
GTSAM_EXPORT GraphAndValues | load2D (const std::string &filename, SharedNoiseModel model=SharedNoiseModel(), size_t maxIndex=0, bool addNoise=false, bool smart=true, NoiseFormat noiseFormat=NoiseFormatAUTO, KernelFunctionType kernelFunctionType=KernelFunctionTypeNONE) |
GTSAM_EXPORT void | save2D (const NonlinearFactorGraph &graph, const Values &config, const noiseModel::Diagonal::shared_ptr model, const std::string &filename) |
GTSAM_EXPORT 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... | |
GTSAM_EXPORT 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... | |
GTSAM_EXPORT GraphAndValues | load3D (const std::string &filename) |
Load TORO 3D Graph. | |
GTSAM_EXPORT BetweenFactorPose2s | parse2DFactors (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model=nullptr, size_t maxIndex=0) |
GTSAM_EXPORT BetweenFactorPose3s | parse3DFactors (const std::string &filename, const noiseModel::Diagonal::shared_ptr &model=nullptr, size_t maxIndex=0) |
Point2_ | transformTo (const Pose2_ &x, const Point2_ &p) |
Double_ | range (const Point2_ &p, const Point2_ &q) |
Point3_ | transformTo (const Pose3_ &x, const Point3_ &p) |
Point3_ | transformFrom (const Pose3_ &x, const Point3_ &p) |
Line3_ | transformTo (const Pose3_ &wTc, const Line3_ &wL) |
Pose3_ | transformPoseTo (const Pose3_ &p, const Pose3_ &q) |
Point3_ | normalize (const Point3_ &a) |
Point3_ | cross (const Point3_ &a, const Point3_ &b) |
Double_ | dot (const Point3_ &a, const Point3_ &b) |
Rot3_ | rotation (const Pose3_ &pose) |
Point3_ | translation (const Pose3_ &pose) |
Point3_ | rotate (const Rot3_ &x, const Point3_ &p) |
Point3_ | point3 (const Unit3_ &v) |
Unit3_ | rotate (const Rot3_ &x, const Unit3_ &p) |
Point3_ | unrotate (const Rot3_ &x, const Point3_ &p) |
Unit3_ | unrotate (const Rot3_ &x, const Unit3_ &p) |
Double_ | distance (const OrientedPlane3_ &p) |
Unit3_ | normal (const OrientedPlane3_ &p) |
Point2_ | project (const Point3_ &p_cam) |
Expression version of PinholeBase::Project. | |
Point2_ | project (const Unit3_ &p_cam) |
template<class CAMERA , class POINT > | |
Point2_ | project2 (const Expression< CAMERA > &camera_, const Expression< POINT > &p_) |
template<class CALIBRATION , class POINT > | |
Point2_ | project3 (const Pose3_ &x, const Expression< POINT > &p, const Expression< CALIBRATION > &K) |
template<class CALIBRATION > | |
Point2_ | uncalibrate (const Expression< CALIBRATION > &K, const Point2_ &xy_hat) |
template<class CALIBRATION > | |
Pose3_ | getPose (const Expression< PinholeCamera< CALIBRATION > > &cam) |
template<typename T > | |
gtsam::Expression< typename gtsam::traits< T >::TangentVector > | logmap (const gtsam::Expression< T > &x1, const gtsam::Expression< T > &x2) |
logmap | |
GTSAM_EXPORT SharedNoiseModel | ConvertNoiseModel (const SharedNoiseModel &model, size_t n, bool defaultToUnit=true) |
template<class T , class ALLOC > | |
T | FindKarcherMeanImpl (const vector< T, ALLOC > &rotations) |
template<class T > | |
T | FindKarcherMean (const std::vector< T > &rotations) |
template<class T > | |
T | FindKarcherMean (const std::vector< T, Eigen::aligned_allocator< T >> &rotations) |
template<class T > | |
T | FindKarcherMean (std::initializer_list< T > &&rotations) |
template<class T , class P > | |
P | transform_point (const T &trans, const P &global, OptionalMatrixType Dtrans, OptionalMatrixType Dglobal) |
GTSAM_EXPORT std::pair< std::shared_ptr< SymbolicConditional >, std::shared_ptr< SymbolicFactor > > | EliminateSymbolic (const SymbolicFactorGraph &factors, const Ordering &keys) |
std::pair< Pose2, bool > | moveWithBounce (const Pose2 &cur_pose, double step_size, const std::vector< SimWall2D > walls, Sampler &angle_drift, Sampler &reflect_noise, const Rot2 &bias=Rot2()) |
template<class PROBLEM > | |
Key | maxKey (const PROBLEM &problem) |
template<class LinearGraph > | |
KeyDimMap | collectKeyDim (const LinearGraph &linearGraph) |
void GTSAM_UNSTABLE_EXPORT | synchronize (ConcurrentFilter &filter, ConcurrentSmoother &smoother) |
std::string | serializeGraph (const NonlinearFactorGraph &graph) |
NonlinearFactorGraph::shared_ptr | deserializeGraph (const std::string &serialized_graph) |
std::string | serializeGraphXML (const NonlinearFactorGraph &graph, const std::string &name="graph") |
NonlinearFactorGraph::shared_ptr | deserializeGraphXML (const std::string &serialized_graph, const std::string &name="graph") |
std::string | serializeValues (const Values &values) |
Values::shared_ptr | deserializeValues (const std::string &serialized_values) |
std::string | serializeValuesXML (const Values &values, const std::string &name="values") |
Values::shared_ptr | deserializeValuesXML (const std::string &serialized_values, const std::string &name="values") |
bool | serializeGraphToFile (const NonlinearFactorGraph &graph, const std::string &fname) |
bool | serializeGraphToXMLFile (const NonlinearFactorGraph &graph, const std::string &fname, const std::string &name="graph") |
bool | serializeValuesToFile (const Values &values, const std::string &fname) |
bool | serializeValuesToXMLFile (const Values &values, const std::string &fname, const std::string &name="values") |
NonlinearFactorGraph::shared_ptr | deserializeGraphFromFile (const std::string &fname) |
NonlinearFactorGraph::shared_ptr | deserializeGraphFromXMLFile (const std::string &fname, const std::string &name="graph") |
Values::shared_ptr | deserializeValuesFromFile (const std::string &fname) |
Values::shared_ptr | deserializeValuesFromXMLFile (const std::string &fname, const std::string &name="values") |
Standard serialization | |
Serialization in default compressed format | |
template<class T > | |
void | serializeToStream (const T &input, std::ostream &out_archive_stream) |
template<class T > | |
void | deserializeFromStream (std::istream &in_archive_stream, T &output) |
deserializes from a stream | |
template<class T > | |
std::string | serializeToString (const T &input) |
serializes to a string | |
template<class T > | |
void | deserializeFromString (const std::string &serialized, T &output) |
deserializes from a string | |
template<class T > | |
bool | serializeToFile (const T &input, const std::string &filename) |
serializes to a file | |
template<class T > | |
bool | deserializeFromFile (const std::string &filename, T &output) |
deserializes from a file | |
template<class T > | |
std::string | serialize (const T &input) |
serializes to a string | |
template<class T > | |
void | deserialize (const std::string &serialized, T &output) |
deserializes from a string | |
XML Serialization | |
Serialization to XML format with named structures | |
template<class T > | |
void | serializeToXMLStream (const T &input, std::ostream &out_archive_stream, const std::string &name="data") |
template<class T > | |
void | deserializeFromXMLStream (std::istream &in_archive_stream, T &output, const std::string &name="data") |
deserializes from a stream in XML | |
template<class T > | |
std::string | serializeToXMLString (const T &input, const std::string &name="data") |
serializes to a string in XML | |
template<class T > | |
void | deserializeFromXMLString (const std::string &serialized, T &output, const std::string &name="data") |
deserializes from a string in XML | |
template<class T > | |
bool | serializeToXMLFile (const T &input, const std::string &filename, const std::string &name="data") |
serializes to an XML file | |
template<class T > | |
bool | deserializeFromXMLFile (const std::string &filename, T &output, const std::string &name="data") |
deserializes from an XML file | |
template<class T > | |
std::string | serializeXML (const T &input, const std::string &name="data") |
serializes to a string in XML | |
template<class T > | |
void | deserializeXML (const std::string &serialized, T &output, const std::string &name="data") |
deserializes from a string in XML | |
Binary Serialization | |
Serialization to binary format with named structures | |
template<class T > | |
void | serializeToBinaryStream (const T &input, std::ostream &out_archive_stream, const std::string &name="data") |
template<class T > | |
void | deserializeFromBinaryStream (std::istream &in_archive_stream, T &output, const std::string &name="data") |
deserializes from a stream in binary | |
template<class T > | |
std::string | serializeToBinaryString (const T &input, const std::string &name="data") |
serializes to a string in binary | |
template<class T > | |
void | deserializeFromBinaryString (const std::string &serialized, T &output, const std::string &name="data") |
deserializes from a string in binary | |
template<class T > | |
bool | serializeToBinaryFile (const T &input, const std::string &filename, const std::string &name="data") |
serializes to a binary file | |
template<class T > | |
bool | deserializeFromBinaryFile (const std::string &filename, T &output, const std::string &name="data") |
deserializes from a binary file | |
template<class T > | |
std::string | serializeBinary (const T &input, const std::string &name="data") |
serializes to a string in binary | |
template<class T > | |
void | deserializeBinary (const std::string &serialized, T &output, const std::string &name="data") |
deserializes from a string in binary | |
utility functions | |
VectorValues | buildVectorValues (const Vector &v, const Ordering &ordering, const std::map< Key, size_t > &dimensions) |
Create VectorValues from a Vector. | |
VectorValues | buildVectorValues (const Vector &v, const KeyInfo &keyInfo) |
Create VectorValues from a Vector and a KeyInfo class. | |
Variables | |
GTSAM_EXTERN_EXPORT FastMap< std::string, ValueWithDefault< bool, false > > | debugFlags |
const G & | b |
const G double | tol |
const double | logSqrt2PI = log(std::sqrt(2.0 * M_PI)) |
constant needed below | |
Global functions in a separate testing namespace
These should not be used outside of tests, as they are just remappings of the original functions. We use these to avoid needing to do too much std::bind magic or writing a bunch of separate proxy functions.
Don't expect all classes to work for all of these functions.
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> >
This file supports creating continuous functions f(x;p)
as a linear combination of basis functions
such as the Fourier basis on SO(2) or a set of Chebyshev polynomials on [-1,1].
In the expression f(x;p)
the variable x
is the continuous argument at which the function is evaluated, and p
are the parameters which are coefficients of the different basis functions, e.g. p = [4; 3; 2] => 4 + 3x + 2x^2 for a polynomial. However, different parameterizations are also possible.
The Basis
class below defines a number of functors that can be used to evaluate f(x;p)
at a given x
, and these functors also calculate the Jacobian of f(x;p)
with respect to the parameters p
. This is actually the most important calculation, as it will allow GTSAM to optimize over the parameters p
.
This functionality is implemented using the CRTP
or "Curiously recurring
template pattern" C++ idiom, which is a meta-programming technique in which the derived class is passed as a template argument to Basis<DERIVED>
. The DERIVED class is assumed to satisfy a C++ concept, i.e., we expect it to define the following types and methods:
Parameters
: the parameters p
in f(x;p)CalculateWeights(size_t N, double x, double a=default, double b=default)
DerivativeWeights(size_t N, double x, double a=default, double b=default)
where Weights
is an N*1 row vector which defines the basis values for the polynomial at the specified point x
.
E.g. A Fourier series would give the following:
CalculateWeights
-> For N=5, the values for the bases: [1, cos(x), sin(x), cos(2x), sin(2x)]DerivativeWeights
-> For N=5, these are: [0, -sin(x), cos(x), -2sin(2x), 2cos(x)]Note that for a pseudo-spectral basis (as in Chebyshev2), the weights are instead the values for the Barycentric interpolation formula, since the values at the polynomial points (e.g. Chebyshev points) define the bases.
triangulationFactor.h
typedef FastVector< FactorIndex > gtsam::FactorIndices |
Define collection types:
Define collection type:
using gtsam::GraphAndValues = typedef std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> |
Return type for load functions, which return a graph and initial values. For landmarks, the gtsam::Symbol L(index) is used to insert into the Values. Bearing-range measurements also refer to landmarks with L(index).
using gtsam::OptionalMatrixType = typedef Matrix* |
This typedef will be used everywhere boost::optional<Matrix&> reference was used previously. This is used to indicate that the Jacobian is optional. In the future we will change this to OptionalJacobian
using gtsam::OptionalMatrixVecType = typedef std::vector<Matrix>* |
The OptionalMatrixVecType is a pointer to a vector of matrices. It will be used in situations where a vector of matrices is optional, like in unwhitenedError.
using gtsam::PinholeCameraCal3_S2 = typedef gtsam::PinholeCamera<gtsam::Cal3_S2> |
Convenient aliases for Pinhole camera classes with different calibrations. Also needed as forward declarations in the wrapper.
typedef Vector2 gtsam::Point2 |
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2
typedef Vector3 gtsam::Point3 |
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3
typedef noiseModel::Base::shared_ptr gtsam::SharedNoiseModel |
Aliases. Deliberately not in noiseModel namespace.
typedef Eigen::SparseMatrix<double, Eigen::ColMajor, int> gtsam::SparseEigen |
Eigen-format sparse matrix. Note: ColMajor is ~20% faster since InnerIndices must be sorted
typedef Vector3 gtsam::Velocity3 |
Velocity is currently typedef'd to Vector3.
Syntactic sugar to clarify components.
Linearization mode: what factor to linearize to.
SmartFactorParams: parameters and (linearization/degeneracy) modes for SmartProjection and SmartStereoProjection factors
enum gtsam::NoiseFormat |
Indicates how noise parameters are stored in file.
DecisionTree<L, Y> gtsam::apply | ( | const DecisionTree< L, Y > & | f, |
const typename DecisionTree< L, Y >::Unary & | op | ||
) |
Apply unary operator op
to DecisionTree f
.
free versions of apply
bool gtsam::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
bool gtsam::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
bool gtsam::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)
bool gtsam::assert_container_equal | ( | const V & | expected, |
const V & | actual, | ||
double | tol = 1e-9 |
||
) |
General function for comparing containers of testable objects
bool gtsam::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 ==
bool gtsam::assert_container_equality | ( | const V & | expected, |
const V & | actual | ||
) |
General function for comparing containers of objects with operator==
Equals testing for basic types
bool gtsam::assert_equal | ( | const std::optional< V > & | expected, |
const std::optional< V > & | actual, | ||
double | tol = 1e-9 |
||
) |
Comparisons for std.optional objects that checks whether objects exist before comparing their values. First version allows for both to be std::nullopt, but the second, with expected given rather than optional
Concept requirement: V is testable
bool gtsam::assert_equal | ( | const V & | expected, |
const V & | actual, | ||
double | tol = 1e-9 |
||
) |
This template works for any type with equals
GTSAM_EXPORT bool gtsam::assert_equal | ( | const Matrix & | A, |
const Matrix & | B, | ||
double | tol = 1e-9 |
||
) |
equals with an tolerance, prints out message if unequal
GTSAM_EXPORT bool gtsam::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
GTSAM_EXPORT bool gtsam::assert_equal | ( | const Vector & | vec1, |
const Vector & | vec2, | ||
double | tol = 1e-9 |
||
) |
Same, prints if error
vec1 | Vector |
vec2 | Vector |
tol | 1e-9 |
GTSAM_EXPORT bool gtsam::assert_equal | ( | const SubVector & | vec1, |
const SubVector & | vec2, | ||
double | tol = 1e-9 |
||
) |
Same, prints if error
vec1 | Vector |
vec2 | Vector |
tol | 1e-9 |
|
inline |
Compare strings for unit tests
GTSAM_EXPORT bool gtsam::assert_inequal | ( | const Matrix & | A, |
const Matrix & | B, | ||
double | tol = 1e-9 |
||
) |
inequals with an tolerance, prints out message if within tolerance
GTSAM_EXPORT bool gtsam::assert_inequal | ( | const Vector & | vec1, |
const Vector & | vec2, | ||
double | tol = 1e-9 |
||
) |
Not the same, prints if error
vec1 | Vector |
vec2 | Vector |
tol | 1e-9 |
bool gtsam::assert_inequal | ( | const V & | expected, |
const V & | actual, | ||
double | tol = 1e-9 |
||
) |
Allow for testing inequality
bool gtsam::assert_print_equal | ( | const std::string & | expected, |
const V & | actual, | ||
const std::string & | s = "" |
||
) |
Capture print function output and compare against string.
s | Optional string to pass to the print() method. |
bool gtsam::assert_stdout_equal | ( | const std::string & | expected, |
const V & | actual | ||
) |
Capture std out via cout stream and compare against string.
GTSAM_EXPORT Vector gtsam::backSubstituteLower | ( | const Matrix & | L, |
const Vector & | b, | ||
bool | unit = false |
||
) |
backSubstitute L*x=b
L | an lower triangular matrix |
b | an RHS vector |
unit,set | true if unit triangular |
GTSAM_EXPORT Vector gtsam::backSubstituteUpper | ( | const Matrix & | U, |
const Vector & | b, | ||
bool | unit = false |
||
) |
backSubstitute U*x=b
U | an upper triangular matrix |
b | an RHS vector |
unit,set | true if unit triangular |
GTSAM_EXPORT Vector gtsam::backSubstituteUpper | ( | const Vector & | b, |
const Matrix & | U, | ||
bool | unit = false |
||
) |
backSubstitute x'*U=b'
U | an upper triangular matrix |
b | an RHS vector |
unit,set | true if unit triangular |
T gtsam::BCH | ( | const T & | X, |
const T & | Y | ||
) |
AGC: bracket() only appears in Rot3 tests, should this be used elsewhere?
Three term approximation of the Baker-Campbell-Hausdorff formula In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y) it is not true that Z = X+Y. Instead, Z can be calculated using the BCH formula: Z = X + Y + [X,Y]/2 + [X-Y,[X,Y]]/12 - [Y,[X,[X,Y]]]/24 http://en.wikipedia.org/wiki/Baker-Campbell-Hausdorff_formula
|
inline |
These core global functions can be specialized by new Lie types for better performance.Compute l0 s.t. l2=l1*l0
GaussianFactorGraph gtsam::buildFactorSubgraph | ( | const GaussianFactorGraph & | gfg, |
const Subgraph & | subgraph, | ||
const bool | clone | ||
) |
Select the factors in a factor graph according to the subgraph.
void gtsam::calibrateJacobians | ( | const Cal & | calibration, |
const Point2 & | pn, | ||
OptionalJacobian< 2, Dim > | Dcal = {} , |
||
OptionalJacobian< 2, 2 > | Dp = {} |
||
) |
Function which makes use of the Implicit Function Theorem to compute the Jacobians of calibrate
using uncalibrate
. This is useful when there are iterative operations in the calibrate
function which make computing jacobians difficult.
Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can easily compute the Jacobians: df/pi = -I (pn and pi are independent args) Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn) Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
Cal | Calibration model. |
Dim | The number of parameters in the calibration model. |
p | Calibrated point. |
Dcal | optional 2*p Jacobian wrpt p Cal3DS2 parameters. |
Dp | optional 2*2 Jacobian wrpt intrinsic coordinates. |
|
inline |
Convert pixel measurements in image to homogeneous measurements in the image plane using camera intrinsics of each measurement.
CAMERA | Camera type to use. |
cameras | Cameras corresponding to each measurement. |
measurements | Vector of measurements to undistort. |
|
inline |
Specialize for SphericalCamera to do nothing.
|
inline |
Convert pixel measurements in image to homogeneous measurements in the image plane using shared camera intrinsics.
CALIBRATION | Calibration type to use. |
cal | Calibration with which measurements were taken. |
measurements | Vector of measurements to undistort. |
GTSAM_EXPORT bool gtsam::checkConvergence | ( | double | relativeErrorTreshold, |
double | absoluteErrorTreshold, | ||
double | errorThreshold, | ||
double | currentError, | ||
double | newError, | ||
NonlinearOptimizerParams::Verbosity | verbosity = NonlinearOptimizerParams::SILENT |
||
) |
Check whether the relative error decrease is less than relativeErrorTreshold, the absolute error decrease is less than absoluteErrorTreshold, or the error itself is less than errorThreshold.
GTSAM_EXPORT 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, | ||
size_t | topleft = 0 |
||
) |
Partial Cholesky computes a factor [R S such that [R' 0 [R S = [A B 0 L] S' I] 0 L] B' C]. The input to this function is the matrix ABC = [A B], and the parameter [B' C] nFrontal determines the split between A, B, and C, with A being of size nFrontal x nFrontal.
if non-zero, factorization proceeds in bottom-right corner starting at topleft
true
if the decomposition is successful, false
if A
was not positive-definite. GTSAM_EXPORT std::list<Point2> gtsam::circleCircleIntersection | ( | Point2 | c1, |
double | r1, | ||
Point2 | c2, | ||
double | r2, | ||
double | tol = 1e-9 |
||
) |
Intersect 2 circles.
c1 | center of first circle |
r1 | radius of first circle |
c2 | center of second circle |
r2 | radius of second circle |
tol | absolute tolerance below which we consider touching circles |
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
matrices | is a vector of matrices in the order to be collected |
m | is the number of rows of a single matrix |
n | is the number of columns of a single matrix |
const MATRIX::ConstColXpr gtsam::column | ( | const MATRIX & | A, |
size_t | j | ||
) |
Extracts a column view from a matrix that avoids a copy
A | matrix to extract column from |
j | index of the column |
std::shared_ptr< Values > gtsam::composePoses | ( | const G & | graph, |
const PredecessorMap< KEY > & | tree, | ||
const POSE & | rootPose | ||
) |
Compose the poses by following the chain specified by the spanning tree
GTSAM_EXPORT Vector gtsam::concatVectors | ( | const std::list< Vector > & | vs | ) |
concatenate Vectors
GTSAM_EXPORT Vector gtsam::concatVectors | ( | size_t | nrVectors, |
... | |||
) |
concatenate Vectors
GTSAM_EXPORT Vector gtsam::conjugateGradientDescent | ( | const System & | Ab, |
const Vector & | x, | ||
const ConjugateGradientParameters & | parameters | ||
) |
Method of conjugate gradients (CG), System version
GTSAM_EXPORT Vector gtsam::conjugateGradientDescent | ( | const Matrix & | A, |
const Vector & | b, | ||
const Vector & | x, | ||
const ConjugateGradientParameters & | parameters | ||
) |
Method of conjugate gradients (CG), Matrix version
GTSAM_EXPORT VectorValues gtsam::conjugateGradientDescent | ( | const GaussianFactorGraph & | fg, |
const VectorValues & | x, | ||
const ConjugateGradientParameters & | parameters | ||
) |
Method of conjugate gradients (CG), Gaussian Factor Graph version
V gtsam::conjugateGradients | ( | const S & | Ab, |
V | x, | ||
const ConjugateGradientParameters & | parameters, | ||
bool | steepest = false |
||
) |
Method of conjugate gradients (CG) template "System" class S needs gradient(S,v), e=S*v, v=S^e "Vector" class V needs dot(v,v), -v, v+v, s*v "Vector" class E needs dot(v,v)
Ab,the | "system" that needs to be solved, examples below |
x | is the initial estimate |
steepest | flag, if true does steepest descent, not CG |
GTSAM_EXPORT SharedNoiseModel gtsam::ConvertNoiseModel | ( | const SharedNoiseModel & | model, |
size_t | n, | ||
bool | defaultToUnit = true |
||
) |
When creating (any) FrobeniusFactor we can convert a Rot/Pose BetweenFactor noise model into a n-dimensional isotropic noise model used to weight the Frobenius norm. If the noise model passed is null we return a n-dimensional isotropic noise model with sigma=1.0. If not, we we check if the d-dimensional noise model on rotations is isotropic. If it is, we extend to 'n' dimensions, otherwise we throw an error. If the noise model is a robust error model, we use the sigmas of the underlying noise model.
If defaultToUnit == false throws an exception on unexepcted input.
Cal3_S2 gtsam::createPinholeCalibration | ( | const CALIBRATION & | cal | ) |
GTSAM_EXPORT std::string gtsam::createRewrittenFileName | ( | const std::string & | name | ) |
Creates a temporary file name that needs to be ignored in .gitingnore for checking read-write oprations
std::vector< Expression< T > > gtsam::createUnknowns | ( | size_t | n, |
char | c, | ||
size_t | start = 0 |
||
) |
Construct an array of leaves.
Construct an array of unknown expressions with successive symbol keys Example: createUnknowns<Pose2>(3,'x') creates unknown expressions for x0,x1,x2
GTSAM_EXPORT Point3 gtsam::cross | ( | const Point3 & | p, |
const Point3 & | q, | ||
OptionalJacobian< 3, 3 > | H_p = {} , |
||
OptionalJacobian< 3, 3 > | H_q = {} |
||
) |
cross product
GTSAM_EXPORT Matrix gtsam::diag | ( | const std::vector< Matrix > & | Hs | ) |
Create a matrix with submatrices along its diagonal
GTSAM_EXPORT std::tuple<int, double, Vector> gtsam::DLT | ( | const Matrix & | A, |
double | rank_tol = 1e-9 |
||
) |
Direct linear transform algorithm that calls svd to find a vector v that minimizes the algebraic error A*v
A | of size m*n, where m>=n (pad with zero rows if not!) Returns rank of A, minimum error (singular value), and corresponding eigenvector (column of V, with A=U*S*V') |
|
inline |
Dot product
GTSAM_EXPORT Vector gtsam::ediv_ | ( | const Vector & | a, |
const Vector & | b | ||
) |
elementwise division, but 0/0 = 0, not inf
a | first vector |
b | second vector |
GTSAM_EXPORT std::pair<std::shared_ptr<GaussianConditional>, std::shared_ptr<JacobianFactor> > gtsam::EliminateQR | ( | const GaussianFactorGraph & | factors, |
const Ordering & | keys | ||
) |
Multiply all factors and eliminate the given keys from the resulting factor using a QR variant that handles constraints (zero sigmas). Computation happens in noiseModel::Gaussian::QR Returns a conditional on those keys, and a new factor on the separator.
GTSAM_EXPORT std::pair<std::shared_ptr<SymbolicConditional>, std::shared_ptr<SymbolicFactor> > gtsam::EliminateSymbolic | ( | const SymbolicFactorGraph & | factors, |
const Ordering & | keys | ||
) |
Dense elimination function for symbolic factors. This is usually provided as an argument to one of the factor graph elimination functions (see EliminateableFactorGraph). The factor graph elimination functions do sparse variable elimination, and use this function to eliminate single variables or variable cliques.
|
inline |
Call equal on the object
|
inline |
Call equal without tolerance (use default tolerance)
|
inline |
Override of equal in Lie.h
|
inline |
Override of equal in Lie.h
bool gtsam::equal_with_abs_tol | ( | const Eigen::DenseBase< MATRIX > & | A, |
const Eigen::DenseBase< MATRIX > & | B, | ||
double | tol = 1e-9 |
||
) |
equals with a tolerance
GTSAM_EXPORT bool gtsam::equal_with_abs_tol | ( | const Vector & | vec1, |
const Vector & | vec2, | ||
double | tol = 1e-9 |
||
) |
VecA == VecB up to tolerance
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
x | exponential coordinates, vector of size n @ return a T |
GTSAM_EXPORT Matrix gtsam::expm | ( | const Matrix & | A, |
size_t | K = 7 |
||
) |
Numerical exponential map, naive approach, not industrial strength !!!
A | matrix to exponentiate |
K | number of iterations |
|
inline |
Exponential map centered at l0, s.t. exp(t,d) = t*exp(d)
std::vector<double> gtsam::expNormalize | ( | const std::vector< double > & | logProbs | ) |
Normalize a set of log probabilities.
Normalizing a set of log probabilities in a numerically stable way is tricky. To avoid overflow/underflow issues, we compute the largest (finite) log probability and subtract it from each log probability before normalizing. This comes from the observation that if: p_i = exp(L_i) / ( sum_j exp(L_j) ), Then, p_i = exp(Z) exp(L_i - Z) / (exp(Z) sum_j exp(L_j - Z)), = exp(L_i - Z) / ( sum_j exp(L_j - Z) )
Setting Z = max_j L_j, we can avoid numerical issues that arise when all of the (unnormalized) log probabilities are either very large or very small.
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.
std::invalid_argument | if no matching file could be found using the search process described above. |
T gtsam::FindKarcherMean | ( | const std::vector< T, Eigen::aligned_allocator< T >> & | rotations | ) |
Optimize for the Karcher mean, minimizing the geodesic distance to each of the given rotations, by constructing a factor graph out of simple PriorFactors.
PredecessorMap< KEY > gtsam::findMinimumSpanningTree | ( | const G & | g | ) |
find the minimum spanning tree using boost graph library
GTSAM_EXPORT bool gtsam::fpEqual | ( | double | a, |
double | b, | ||
double | tol, | ||
bool | check_relative_also = true |
||
) |
Ensure we are not including a different version of Eigen in user code than while compiling gtsam, since it can lead to hard-to-understand runtime crashes. Numerically stable function for comparing if floating point values are equal within epsilon tolerance. Used for vector and matrix comparison with C++11 compatible functions.
If either value is NaN or Inf, we check for both values to be NaN or Inf respectively for the comparison to be true. If one is NaN/Inf and the other is not, returns false.
check_relative_also | is a flag which toggles additional checking for relative error. This means that if either the absolute error or the relative error is within the tolerance, the result will be true. By default, the flag is true. |
Return true if two numbers are close wrt tol.
GenericValue<T> gtsam::genericValue | ( | const T & | v | ) |
Functional constructor of GenericValue<T> so T can be automatically deduced
GTSAM_EXPORT bool gtsam::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
This function converts a GTSAM camera pose to an openGL camera pose.
R | rotation in GTSAM |
tx | x component of the translation in GTSAM |
ty | y component of the translation in GTSAM |
tz | z component of the translation in GTSAM |
This function converts a GTSAM camera pose to an openGL camera pose.
PoseGTSAM | pose in GTSAM format |
GTSAM_EXPORT bool gtsam::hasConstraints | ( | const GaussianFactorGraph & | factors | ) |
Evaluates whether linear factors have any constrained noise models
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
k | number of columns to zero out below diagonal |
A | matrix |
GTSAM_EXPORT void gtsam::householder_ | ( | Matrix & | A, |
size_t | k, | ||
bool | copy_vectors = true |
||
) |
Householder transformation, Householder vectors below diagonal
k | number of columns to zero out below diagonal |
A | matrix |
copy_vectors | - true to copy Householder vectors below diagonal |
GTSAM_EXPORT double gtsam::houseInPlace | ( | Vector & | x | ) |
beta = house(x) computes the HouseHolder vector in place
GTSAM_EXPORT const Ordering gtsam::HybridOrdering | ( | const HybridGaussianFactorGraph & | graph | ) |
Return a Colamd constrained ordering where the discrete keys are eliminated after the continuous keys.
|
inline |
compatibility version for ublas' inner_prod()
GTSAM_EXPORT void gtsam::inplace_QR | ( | Matrix & | A | ) |
QR factorization using Eigen's internal block QR algorithm
A | is the input matrix, and is the output |
clear_below_diagonal | enables zeroing out below diagonal |
void gtsam::insertSub | ( | Eigen::MatrixBase< Derived1 > & | fullMatrix, |
const Eigen::MatrixBase< Derived2 > & | subMatrix, | ||
size_t | i, | ||
size_t | j | ||
) |
insert a submatrix IN PLACE at a specified location in a larger matrix NOTE: there is no size checking
fullMatrix | matrix to be updated |
subMatrix | matrix to be inserted |
i | is the row of the upper left corner insert location |
j | is the column of the upper left corner insert location |
T gtsam::interpolate | ( | const T & | X, |
const T & | Y, | ||
double | t, | ||
typename MakeOptionalJacobian< T, T >::type | Hx = {} , |
||
typename MakeOptionalJacobian< T, T >::type | Hy = {} |
||
) |
Linear interpolation between X and Y by coefficient t. Typically t [0,1], but can also be used to extrapolate before pose X or after pose Y.
GTSAM_EXPORT Matrix gtsam::inverse_square_root | ( | const Matrix & | A | ) |
Use Cholesky to calculate inverse square root of a matrix
GTSAM_EXPORT bool gtsam::linear_dependent | ( | const Matrix & | A, |
const Matrix & | B, | ||
double | tol = 1e-9 |
||
) |
check whether the rows of two matrices are linear dependent
GTSAM_EXPORT bool gtsam::linear_dependent | ( | const Vector & | vec1, |
const Vector & | vec2, | ||
double | tol = 1e-9 |
||
) |
check whether two vectors are linearly dependent
vec1 | Vector |
vec2 | Vector |
tol | 1e-9 |
GTSAM_EXPORT bool gtsam::linear_independent | ( | const Matrix & | A, |
const Matrix & | B, | ||
double | tol = 1e-9 |
||
) |
check whether the rows of two matrices are linear independent
Expression<T> gtsam::linearExpression | ( | const std::function< T(A)> & | f, |
const Expression< A > & | expression, | ||
const Eigen::Matrix< double, traits< T >::dimension, traits< A >::dimension > & | dTdA | ||
) |
Create an expression out of a linear function f:T->A with (constant) Jacobian dTdA TODO(frank): create a more efficient version like ScalarMultiplyExpression. This version still does a malloc every linearize.
|
inline |
Linearize a nonlinear factor using numerical differentiation The benefit of this method is that it does not need to know what types are involved to evaluate the factor. If all the machinery of gtsam is working correctly, we should get the correct numerical derivatives out the other side. NOTE(frank): factors that have non vector-space measurements use between or LocalCoordinates to evaluate the error, and their derivatives will only be correct for near-zero errors. This is fixable but expensive, and does not matter in practice as most factors will sit near zero errors anyway. However, it means that below will only be exact for the correct measurement.
double gtsam::lineSearch | ( | const S & | system, |
const V | currentValues, | ||
const W & | gradient | ||
) |
Implement the golden-section line search algorithm
GTSAM_EXPORT GraphAndValues gtsam::load2D | ( | std::pair< std::string, SharedNoiseModel > | dataset, |
size_t | maxIndex = 0 , |
||
bool | addNoise = false , |
||
bool | smart = true , |
||
NoiseFormat | noiseFormat = NoiseFormatAUTO , |
||
KernelFunctionType | kernelFunctionType = KernelFunctionTypeNONE |
||
) |
Load TORO 2D Graph
dataset/model | pair as constructed by [dataset] |
maxIndex | if non-zero cut out vertices >= maxIndex |
addNoise | add noise to the edges |
smart | try to reduce complexity of covariance to cheapest model |
GTSAM_EXPORT GraphAndValues gtsam::load2D | ( | const std::string & | filename, |
SharedNoiseModel | model = SharedNoiseModel() , |
||
size_t | maxIndex = 0 , |
||
bool | addNoise = false , |
||
bool | smart = true , |
||
NoiseFormat | noiseFormat = NoiseFormatAUTO , |
||
KernelFunctionType | kernelFunctionType = KernelFunctionTypeNONE |
||
) |
Load TORO/G2O style graph files
filename | |
model | optional noise model to use instead of one specified by file |
maxIndex | if non-zero cut out vertices >= maxIndex |
addNoise | add noise to the edges |
smart | try to reduce complexity of covariance to cheapest model |
noiseFormat | how noise parameters are stored |
kernelFunctionType | whether to wrap the noise model in a robust kernel |
|
inline |
Log map centered at l0, s.t. exp(l0,log(l0,lp)) = lp
gtsam::enable_if_t<needs_eigen_aligned_allocator<T>::value, std::shared_ptr<T> > gtsam::make_shared | ( | Args &&... | args | ) |
Add our own make_shared
as a layer of wrapping on std::make_shared
This solves the problem with the stock make_shared
that custom alignment is not respected, causing SEGFAULTs at runtime, which is notoriously hard to debug.
The template needs_eigen_aligned_allocator<T>::value
will evaluate to std::true_type
if the type alias _eigen_aligned_allocator_trait = void
is present in a class, which is automatically added by the GTSAM_MAKE_ALIGNED_OPERATOR_NEW
macro.
This function declaration will only be taken when the above condition is true, so if some object does not need to be aligned, gtsam::make_shared
will fall back to the next definition, which is a simple wrapper for std::make_shared
.
T | The type of object being constructed |
Args | Type of the arguments of the constructor |
args | Arguments of the constructor |
|
inline |
Return the ordering as a binary tree such that all parent nodes are above their children.
This will result in a nested dissection Bayes tree after elimination.
input | The original ordering. |
FunctorizedFactor<R, T> gtsam::MakeFunctorizedFactor | ( | Key | key, |
const R & | z, | ||
const SharedNoiseModel & | model, | ||
const FUNC | func | ||
) |
Helper function to create a functorized factor.
Uses function template deduction to identify return type and functor type, so template list only needs the functor argument type.
FunctorizedFactor2<R, T1, T2> gtsam::MakeFunctorizedFactor2 | ( | Key | key1, |
Key | key2, | ||
const R & | z, | ||
const SharedNoiseModel & | model, | ||
const FUNC | func | ||
) |
Helper function to create a functorized factor.
Uses function template deduction to identify return type and functor type, so template list only needs the functor argument type.
|
inline |
Create a switching system chain. A switching system is a continuous system which depends on a discrete mode at each time step of the chain.
n | The number of chain elements. |
keyFunc | The functional to help specify the continuous key. |
dKeyFunc | The functional to help specify the discrete key. |
Key gtsam::maxKey | ( | const PROBLEM & | problem | ) |
Find the max key in a problem. Useful to determine unique keys for additional slack variables
std::pair<Pose2, bool> gtsam::moveWithBounce | ( | const Pose2 & | cur_pose, |
double | step_size, | ||
const std::vector< SimWall2D > | walls, | ||
Sampler & | angle_drift, | ||
Sampler & | reflect_noise, | ||
const Rot2 & | bias = Rot2() |
||
) |
Calculates the next pose in a trajectory constrained by walls, with noise on angular drift and reflection noise
cur_pose | is the pose of the robot |
step_size | is the size of the forward step the robot tries to take |
walls | is a set of walls to use for bouncing |
angle_drift | is a sampler for angle drift (dim=1) |
reflect_noise | is a sampler for scatter after hitting a wall (dim=3) |
|
inline |
Create a symbol key from a character, label and index, i.e. xA5.
|
inline |
Return the character portion of a symbol key.
|
inline |
Return the index portion of a symbol key.
|
inline |
Return the label portion of a symbol key.
std::tuple<V, int> gtsam::nonlinearConjugateGradient | ( | const S & | system, |
const V & | initial, | ||
const NonlinearOptimizerParams & | params, | ||
const bool | singleIteration, | ||
const bool | gradientDescent = false |
||
) |
Implement the nonlinear conjugate gradient method using the Polak-Ribiere formula suggested in http://en.wikipedia.org/wiki/Nonlinear_conjugate_gradient_method.
The S (system) class requires three member functions: error(state), gradient(state) and advance(state, step-size, direction). The V class denotes the state or the solution.
The last parameter is a switch between gradient-descent and conjugate gradient
internal::FixedSizeMatrix<Y, X>::type gtsam::numericalDerivative11 | ( | std::function< Y(const X &)> | h, |
const X & | x, | ||
double | delta = 1e-5 |
||
) |
New-style numerical derivatives using manifold_traits.
Computes numerical derivative in argument 1 of unary function
h | unary function yielding m-vector |
x | n-dimensional value at which to evaluate h |
delta | increment for numerical derivative Class Y is the output argument Class X is the input argument |
int | N is the dimension of the X input value if variable dimension type but known at test time |
internal::FixedSizeMatrix<Y,X>::type gtsam::numericalDerivative11 | ( | Y(*)(const X &) | h, |
const X & | x, | ||
double | delta = 1e-5 |
||
) |
use a raw C++ function pointer
internal::FixedSizeMatrix<Y,X1>::type gtsam::numericalDerivative21 | ( | const std::function< Y(const X1 &, const X2 &)> & | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
double | delta = 1e-5 |
||
) |
Compute numerical derivative in argument 1 of binary function
h | binary function yielding m-vector |
x1 | n-dimensional first argument value |
x2 | second argument value |
delta | increment for numerical derivative |
int | N is the dimension of the X1 input value if variable dimension type but known at test time |
internal::FixedSizeMatrix<Y,X1>::type gtsam::numericalDerivative21 | ( | Y(*)(const X1 &, const X2 &) | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
double | delta = 1e-5 |
||
) |
use a raw C++ function pointer
internal::FixedSizeMatrix<Y,X2>::type gtsam::numericalDerivative22 | ( | std::function< Y(const X1 &, const X2 &)> | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
double | delta = 1e-5 |
||
) |
Compute numerical derivative in argument 2 of binary function
h | binary function yielding m-vector |
x1 | first argument value |
x2 | n-dimensional second argument value |
delta | increment for numerical derivative |
int | N is the dimension of the X2 input value if variable dimension type but known at test time |
internal::FixedSizeMatrix<Y,X2>::type gtsam::numericalDerivative22 | ( | Y(*)(const X1 &, const X2 &) | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
double | delta = 1e-5 |
||
) |
use a raw C++ function pointer
internal::FixedSizeMatrix<Y,X1>::type gtsam::numericalDerivative31 | ( | std::function< Y(const X1 &, const X2 &, const X3 &)> | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
const X3 & | x3, | ||
double | delta = 1e-5 |
||
) |
Compute numerical derivative in argument 1 of ternary function
h | ternary function yielding m-vector |
x1 | n-dimensional first argument value |
x2 | second argument value |
x3 | third argument value |
delta | increment for numerical derivative |
int | N is the dimension of the X1 input value if variable dimension type but known at test time |
internal::FixedSizeMatrix<Y,X2>::type gtsam::numericalDerivative32 | ( | std::function< Y(const X1 &, const X2 &, const X3 &)> | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
const X3 & | x3, | ||
double | delta = 1e-5 |
||
) |
Compute numerical derivative in argument 2 of ternary function
h | ternary function yielding m-vector |
x1 | n-dimensional first argument value |
x2 | second argument value |
x3 | third argument value |
delta | increment for numerical derivative |
int | N is the dimension of the X2 input value if variable dimension type but known at test time |
internal::FixedSizeMatrix<Y,X3>::type gtsam::numericalDerivative33 | ( | std::function< Y(const X1 &, const X2 &, const X3 &)> | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
const X3 & | x3, | ||
double | delta = 1e-5 |
||
) |
Compute numerical derivative in argument 3 of ternary function
h | ternary function yielding m-vector |
x1 | n-dimensional first argument value |
x2 | second argument value |
x3 | third argument value |
delta | increment for numerical derivative |
int | N is the dimension of the X3 input value if variable dimension type but known at test time |
internal::FixedSizeMatrix<Y,X1>::type gtsam::numericalDerivative41 | ( | std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
const X3 & | x3, | ||
const X4 & | x4, | ||
double | delta = 1e-5 |
||
) |
Compute numerical derivative in argument 1 of 4-argument function
h | quartic function yielding m-vector |
x1 | n-dimensional first argument value |
x2 | second argument value |
x3 | third argument value |
x4 | fourth argument value |
delta | increment for numerical derivative |
int | N is the dimension of the X1 input value if variable dimension type but known at test time |
internal::FixedSizeMatrix<Y,X2>::type gtsam::numericalDerivative42 | ( | std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
const X3 & | x3, | ||
const X4 & | x4, | ||
double | delta = 1e-5 |
||
) |
Compute numerical derivative in argument 2 of 4-argument function
h | quartic function yielding m-vector |
x1 | first argument value |
x2 | n-dimensional second argument value |
x3 | third argument value |
x4 | fourth argument value |
delta | increment for numerical derivative |
int | N is the dimension of the X2 input value if variable dimension type but known at test time |
internal::FixedSizeMatrix<Y,X3>::type gtsam::numericalDerivative43 | ( | std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
const X3 & | x3, | ||
const X4 & | x4, | ||
double | delta = 1e-5 |
||
) |
Compute numerical derivative in argument 3 of 4-argument function
h | quartic function yielding m-vector |
x1 | first argument value |
x2 | second argument value |
x3 | n-dimensional third argument value |
x4 | fourth argument value |
delta | increment for numerical derivative |
int | N is the dimension of the X3 input value if variable dimension type but known at test time |
internal::FixedSizeMatrix<Y,X4>::type gtsam::numericalDerivative44 | ( | std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &)> | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
const X3 & | x3, | ||
const X4 & | x4, | ||
double | delta = 1e-5 |
||
) |
Compute numerical derivative in argument 4 of 4-argument function
h | quartic function yielding m-vector |
x1 | first argument value |
x2 | second argument value |
x3 | third argument value |
x4 | n-dimensional fourth argument value |
delta | increment for numerical derivative |
int | N is the dimension of the X4 input value if variable dimension type but known at test time |
internal::FixedSizeMatrix<Y,X1>::type gtsam::numericalDerivative51 | ( | std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
const X3 & | x3, | ||
const X4 & | x4, | ||
const X5 & | x5, | ||
double | delta = 1e-5 |
||
) |
Compute numerical derivative in argument 1 of 5-argument function
h | quintic function yielding m-vector |
x1 | n-dimensional first argument value |
x2 | second argument value |
x3 | third argument value |
x4 | fourth argument value |
x5 | fifth argument value |
delta | increment for numerical derivative |
int | N is the dimension of the X1 input value if variable dimension type but known at test time |
internal::FixedSizeMatrix<Y,X2>::type gtsam::numericalDerivative52 | ( | std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
const X3 & | x3, | ||
const X4 & | x4, | ||
const X5 & | x5, | ||
double | delta = 1e-5 |
||
) |
Compute numerical derivative in argument 2 of 5-argument function
h | quintic function yielding m-vector |
x1 | n-dimensional first argument value |
x2 | second argument value |
x3 | third argument value |
x4 | fourth argument value |
x5 | fifth argument value |
delta | increment for numerical derivative |
int | N is the dimension of the X2 input value if variable dimension type but known at test time |
internal::FixedSizeMatrix<Y,X3>::type gtsam::numericalDerivative53 | ( | std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
const X3 & | x3, | ||
const X4 & | x4, | ||
const X5 & | x5, | ||
double | delta = 1e-5 |
||
) |
Compute numerical derivative in argument 3 of 5-argument function
h | quintic function yielding m-vector |
x1 | n-dimensional first argument value |
x2 | second argument value |
x3 | third argument value |
x4 | fourth argument value |
x5 | fifth argument value |
delta | increment for numerical derivative |
int | N is the dimension of the X3 input value if variable dimension type but known at test time |
internal::FixedSizeMatrix<Y,X4>::type gtsam::numericalDerivative54 | ( | std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
const X3 & | x3, | ||
const X4 & | x4, | ||
const X5 & | x5, | ||
double | delta = 1e-5 |
||
) |
Compute numerical derivative in argument 4 of 5-argument function
h | quintic function yielding m-vector |
x1 | n-dimensional first argument value |
x2 | second argument value |
x3 | third argument value |
x4 | fourth argument value |
x5 | fifth argument value |
delta | increment for numerical derivative |
int | N is the dimension of the X4 input value if variable dimension type but known at test time |
internal::FixedSizeMatrix<Y,X5>::type gtsam::numericalDerivative55 | ( | std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &)> | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
const X3 & | x3, | ||
const X4 & | x4, | ||
const X5 & | x5, | ||
double | delta = 1e-5 |
||
) |
Compute numerical derivative in argument 5 of 5-argument function
h | quintic function yielding m-vector |
x1 | n-dimensional first argument value |
x2 | second argument value |
x3 | third argument value |
x4 | fourth argument value |
x5 | fifth argument value |
delta | increment for numerical derivative |
int | N is the dimension of the X5 input value if variable dimension type but known at test time |
internal::FixedSizeMatrix<Y,X1>::type gtsam::numericalDerivative61 | ( | std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
const X3 & | x3, | ||
const X4 & | x4, | ||
const X5 & | x5, | ||
const X6 & | x6, | ||
double | delta = 1e-5 |
||
) |
Compute numerical derivative in argument 1 of 6-argument function
h | quintic function yielding m-vector |
x1 | n-dimensional first argument value |
x2 | second argument value |
x3 | third argument value |
x4 | fourth argument value |
x5 | fifth argument value |
x6 | sixth argument value |
delta | increment for numerical derivative |
int | N is the dimension of the X1 input value if variable dimension type but known at test time |
internal::FixedSizeMatrix<Y,X2>::type gtsam::numericalDerivative62 | ( | std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
const X3 & | x3, | ||
const X4 & | x4, | ||
const X5 & | x5, | ||
const X6 & | x6, | ||
double | delta = 1e-5 |
||
) |
Compute numerical derivative in argument 2 of 6-argument function
h | quintic function yielding m-vector |
x1 | n-dimensional first argument value |
x2 | second argument value |
x3 | third argument value |
x4 | fourth argument value |
x5 | fifth argument value |
x6 | sixth argument value |
delta | increment for numerical derivative |
int | N is the dimension of the X2 input value if variable dimension type but known at test time |
internal::FixedSizeMatrix<Y,X3>::type gtsam::numericalDerivative63 | ( | std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
const X3 & | x3, | ||
const X4 & | x4, | ||
const X5 & | x5, | ||
const X6 & | x6, | ||
double | delta = 1e-5 |
||
) |
Compute numerical derivative in argument 3 of 6-argument function
h | quintic function yielding m-vector |
x1 | n-dimensional first argument value |
x2 | second argument value |
x3 | third argument value |
x4 | fourth argument value |
x5 | fifth argument value |
x6 | sixth argument value |
delta | increment for numerical derivative |
int | N is the dimension of the X3 input value if variable dimension type but known at test time |
internal::FixedSizeMatrix<Y,X4>::type gtsam::numericalDerivative64 | ( | std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
const X3 & | x3, | ||
const X4 & | x4, | ||
const X5 & | x5, | ||
const X6 & | x6, | ||
double | delta = 1e-5 |
||
) |
Compute numerical derivative in argument 4 of 6-argument function
h | quintic function yielding m-vector |
x1 | n-dimensional first argument value |
x2 | second argument value |
x3 | third argument value |
x4 | fourth argument value |
x5 | fifth argument value |
x6 | sixth argument value |
delta | increment for numerical derivative |
int | N is the dimension of the X4 input value if variable dimension type but known at test time |
internal::FixedSizeMatrix<Y,X5>::type gtsam::numericalDerivative65 | ( | std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
const X3 & | x3, | ||
const X4 & | x4, | ||
const X5 & | x5, | ||
const X6 & | x6, | ||
double | delta = 1e-5 |
||
) |
Compute numerical derivative in argument 5 of 6-argument function
h | quintic function yielding m-vector |
x1 | n-dimensional first argument value |
x2 | second argument value |
x3 | third argument value |
x4 | fourth argument value |
x5 | fifth argument value |
x6 | sixth argument value |
delta | increment for numerical derivative |
int | N is the dimension of the X5 input value if variable dimension type but known at test time |
internal::FixedSizeMatrix<Y, X6>::type gtsam::numericalDerivative66 | ( | std::function< Y(const X1 &, const X2 &, const X3 &, const X4 &, const X5 &, const X6 &)> | h, |
const X1 & | x1, | ||
const X2 & | x2, | ||
const X3 & | x3, | ||
const X4 & | x4, | ||
const X5 & | x5, | ||
const X6 & | x6, | ||
double | delta = 1e-5 |
||
) |
Compute numerical derivative in argument 6 of 6-argument function
h | quintic function yielding m-vector |
x1 | n-dimensional first argument value |
x2 | second argument value |
x3 | third argument value |
x4 | fourth argument value |
x5 | fifth argument value |
x6 | sixth argument value |
delta | increment for numerical derivative |
int | N is the dimension of the X6 input value if variable dimension type but known at test time |
Eigen::Matrix<double, N, 1> gtsam::numericalGradient | ( | std::function< double(const X &)> | h, |
const X & | x, | ||
double | delta = 1e-5 |
||
) |
Numerically compute gradient of scalar function.
|
inline |
Compute numerical Hessian matrix. Requires a single-argument Lie->scalar function. This is implemented simply as the derivative of the gradient.
f | A function taking a Lie object as input and returning a scalar |
x | The center point for computing the Hessian |
delta | The numerical derivative step size |
|
inline |
Numerical Hessian for tenary functions
This function converts an openGL camera pose to an GTSAM camera pose.
R | rotation in openGL |
tx | x component of the translation in openGL |
ty | y component of the translation in openGL |
tz | z component of the translation in openGL |
|
inline |
inequality
GTSAM_EXPORT Signature gtsam::operator% | ( | const DiscreteKey & | key, |
const std::string & | parent | ||
) |
GTSAM_EXPORT Signature gtsam::operator% | ( | const DiscreteKey & | key, |
const Signature::Table & | parent | ||
) |
ScalarMultiplyExpression<T> gtsam::operator* | ( | double | s, |
const Expression< T > & | e | ||
) |
Construct an expression that executes the scalar multiplication with an input expression The type T must be a vector space Example: Expression<Point2> a(0), b = 12 * a;
Expression< T > gtsam::operator* | ( | const Expression< T > & | e1, |
const Expression< T > & | e2 | ||
) |
Construct a product expression, assumes T::compose(T) -> T.
Construct a product expression, assumes T::compose(T) -> T Example: Expression<Point2> a(0), b(1), c = a*b;
BinarySumExpression<T> gtsam::operator+ | ( | const Expression< T > & | e1, |
const Expression< T > & | e2 | ||
) |
Construct an expression that sums two input expressions of the same type T The type T must be a vector space Example: Expression<Point2> a(0), b(1), c = a + b;
|
inline |
equality is just equal_with_abs_tol 1e-9
GTSAM_EXPORT bool gtsam::operator== | ( | const Vector & | vec1, |
const Vector & | vec2 | ||
) |
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 Vector gtsam::operator^ | ( | const Matrix & | A, |
const Vector & | v | ||
) |
overload ^ for trans(A)*v We transpose the vectors for speed.
GTSAM_EXPORT Signature gtsam::operator| | ( | const DiscreteKey & | key, |
const DiscreteKey & | parent | ||
) |
GTSAM_EXPORT Point3 gtsam::optimize | ( | const NonlinearFactorGraph & | graph, |
const Values & | values, | ||
Key | landmarkKey | ||
) |
Optimize for triangulation
graph | nonlinear factors for projection |
values | initial values |
landmarkKey | to refer to landmark |
size_t gtsam::optimizeWildfire | ( | const ISAM2Clique::shared_ptr & | root, |
double | threshold, | ||
const KeySet & | replaced, | ||
VectorValues * | delta | ||
) |
Optimize the BayesTree, starting from the root.
threshold | The maximum change against the PREVIOUS delta for non-replaced variables that can be ignored, ie. the old delta entry is kept and recursive backsubstitution might eventually stop if none of the changed variables are contained in the subtree. |
replaced | Needs to contain all variables that are contained in the top of the Bayes tree that has been redone. |
delta | The current solution, an offset from the linearization point. |
GTSAM_EXPORT std::optional<IndexedEdge> gtsam::parseEdge | ( | std::istream & | is, |
const std::string & | tag | ||
) |
Parse TORO/G2O edge "id1 id2 x y yaw"
is | input stream |
tag | string parsed from input stream, will only parse if edge type |
GTSAM_EXPORT std::vector<typename BetweenFactor<T>::shared_ptr> gtsam::parseFactors | ( | const std::string & | filename, |
const noiseModel::Diagonal::shared_ptr & | model = nullptr , |
||
size_t | maxIndex = 0 |
||
) |
GTSAM_EXPORT std::vector<BinaryMeasurement<T> > gtsam::parseMeasurements | ( | const std::string & | filename, |
const noiseModel::Diagonal::shared_ptr & | model = nullptr , |
||
size_t | maxIndex = 0 |
||
) |
GTSAM_EXPORT std::map<size_t, T> gtsam::parseVariables | ( | const std::string & | filename, |
size_t | maxIndex = 0 |
||
) |
GTSAM_EXPORT std::optional<IndexedLandmark> gtsam::parseVertexLandmark | ( | std::istream & | is, |
const std::string & | tag | ||
) |
Parse G2O landmark vertex "id x y"
is | input stream |
tag | string parsed from input stream, will only parse if vertex type |
GTSAM_EXPORT std::optional<IndexedPose> gtsam::parseVertexPose | ( | std::istream & | is, |
const std::string & | tag | ||
) |
Parse TORO/G2O vertex "id x y yaw"
is | input stream |
tag | string parsed from input stream, will only parse if vertex type |
std::tuple< G, V, std::map< KEY, V > > gtsam::predecessorMap2Graph | ( | const PredecessorMap< KEY > & | p_map | ) |
Build takes a predecessor map, and builds a directed graph corresponding to the tree. G = Graph type V = Vertex type
std::list< KEY > gtsam::predecessorMap2Keys | ( | const PredecessorMap< KEY > & | p_map | ) |
Generate a list of keys from a spanning tree represented by its predecessor map
GTSAM_EXPORT void gtsam::print | ( | const Vector & | v, |
const std::string & | s, | ||
std::ostream & | stream | ||
) |
print without optional string, must specify cout yourself
GTSAM_EXPORT void gtsam::print | ( | const Vector & | v, |
const std::string & | s = "" |
||
) |
print with optional string to cout
GTSAM_EXPORT void gtsam::print | ( | const Matrix & | A, |
const std::string & | s, | ||
std::ostream & | stream | ||
) |
print without optional string, must specify cout yourself
GTSAM_EXPORT void gtsam::print | ( | const Matrix & | A, |
const std::string & | s = "" |
||
) |
print with optional string to cout
|
inline |
products using old-style format to improve compatibility
GTSAM_EXPORT std::pair<Matrix,Matrix> gtsam::qr | ( | const Matrix & | A | ) |
QR factorization, inefficient, best use imperative householder below m*n matrix -> m*m Q, m*n R
A | a matrix |
GTSAM_EXPORT SfmData gtsam::readBal | ( | const std::string & | filename | ) |
This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data as a SfmData structure. Mainly used by wrapped code.
filename | The name of the BAL file. |
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.
filename | The name of the g2o file\ |
is3D | indicates if the file describes a 2D or 3D problem |
kernelFunctionType | whether to wrap the noise model in a robust kernel |
const MATRIX::ConstRowXpr gtsam::row | ( | const MATRIX & | A, |
size_t | j | ||
) |
Extracts a row view from a matrix that avoids a copy
A | matrix to extract row from |
j | index of the row |
GTSAM_EXPORT std::pair<Matrix3, Vector3> gtsam::RQ | ( | const Matrix3 & | A, |
OptionalJacobian< 3, 9 > | H = {} |
||
) |
[RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx' such that A = R*Q = R*Qz'*Qy'*Qx'. When A is a rotation matrix, R will be the identity and Q is a yaw-pitch-roll decomposition of A. The implementation uses Givens rotations and is based on Hartley-Zisserman.
A | 3 by 3 matrix A=RQ |
GTSAM_EXPORT void gtsam::save | ( | const Vector & | A, |
const std::string & | s, | ||
const std::string & | filename | ||
) |
save a vector to file, which can be loaded by matlab
GTSAM_EXPORT void gtsam::save | ( | const Matrix & | A, |
const std::string & | s, | ||
const std::string & | filename | ||
) |
save a matrix to file, which can be loaded by matlab
GTSAM_EXPORT void gtsam::save2D | ( | const NonlinearFactorGraph & | graph, |
const Values & | config, | ||
const noiseModel::Diagonal::shared_ptr | model, | ||
const std::string & | filename | ||
) |
save 2d graph
void gtsam::serializeToBinaryStream | ( | const T & | input, |
std::ostream & | out_archive_stream, | ||
const std::string & | name = "data" |
||
) |
serializes to a stream in binary
void gtsam::serializeToStream | ( | const T & | input, |
std::ostream & | out_archive_stream | ||
) |
serializes to a stream
void gtsam::serializeToXMLStream | ( | const T & | input, |
std::ostream & | out_archive_stream, | ||
const std::string & | name = "data" |
||
) |
serializes to a stream in XML
|
inline |
skew symmetric matrix returns this: 0 -wz wy wz 0 -wx -wy wx 0
wx | 3 dimensional vector |
wy | |
wz |
void gtsam::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::pair<GaussianFactorGraph, GaussianFactorGraph> gtsam::splitFactorGraph | ( | const GaussianFactorGraph & | factorGraph, |
const Subgraph & | subgraph | ||
) |
Split the graph into a subgraph and the remaining edges. Note that the remaining factorgraph has null factors.
GTSAM_EXPORT Matrix gtsam::stack | ( | size_t | nrMatrices, |
... | |||
) |
create a matrix by stacking other matrices Given a set of matrices: A1, A2, A3...
... | pointers to matrices to be stacked |
GTSAM_EXPORT Vector gtsam::steepestDescent | ( | const System & | Ab, |
const Vector & | x, | ||
const IterativeOptimizationParameters & | parameters | ||
) |
Method of steepest gradients, System version
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 VectorValues gtsam::steepestDescent | ( | const GaussianFactorGraph & | fg, |
const VectorValues & | x, | ||
const ConjugateGradientParameters & | parameters | ||
) |
Method of steepest gradients, Gaussian Factor Graph version
GTSAM_EXPORT Matrix43 gtsam::stiefel | ( | const SO4 & | Q, |
OptionalJacobian< 12, 6 > | H = {} |
||
) |
Project to Stiefel manifold of 4*3 orthonormal 3-frames in R^4, i.e., pi(Q) -> \( S \in St(3,4) \).
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
A | matrix |
i1 | first row index |
i2 | last row index + 1 |
j1 | first col index |
j2 | last col index + 1 |
GTSAM_EXPORT void gtsam::svd | ( | const Matrix & | A, |
Matrix & | U, | ||
Vector & | S, | ||
Matrix & | V | ||
) |
SVD computes economy SVD A=U*S*V'
A | an m*n matrix |
U | output argument: rotation matrix |
S | output argument: sorted vector of singular values |
V | output argument: rotation matrix if m > n then U*S*V' = (m*n)*(n*n)*(n*n) if m < n then U*S*V' = (m*m)*(m*m)*(m*n) Careful! The dimensions above reflect V', not V, which is n*m if m<n. U is a basis in R^m, V is a basis in R^n You can just pass empty matrices U,V, and vector S, they will be re-allocated. |
|
inline |
Create a symbol key from a character and index, i.e. x5.
|
inline |
Return the character portion of a symbol key.
|
inline |
Return the index portion of a symbol key.
SDGraph< KEY > gtsam::toBoostGraph | ( | const G & | graph | ) |
GTSAM_EXPORT Matrix3 gtsam::topLeft | ( | const SO4 & | Q, |
OptionalJacobian< 9, 6 > | H = {} |
||
) |
Project to top-left 3*3 matrix. Note this is not in general SO(3).
|
inline |
static transpose function, just calls Eigen transpose member function
P gtsam::transform_point | ( | const T & | trans, |
const P & | global, | ||
OptionalMatrixType | Dtrans, | ||
OptionalMatrixType | Dglobal | ||
) |
Transform function that must be specialized specific domains
T | is a Transform type |
P | is a point type |
GTSAM_EXPORT Line3 gtsam::transformTo | ( | const Pose3 & | wTc, |
const Line3 & | wL, | ||
OptionalJacobian< 4, 6 > | Dpose = {} , |
||
OptionalJacobian< 4, 4 > | Dline = {} |
||
) |
Transform a line from world to camera frame
wTc | - Pose3 of camera in world frame |
wL | - Line3 in world frame |
Dpose | - OptionalJacobian of transformed line with respect to p |
Dline | - OptionalJacobian of transformed line with respect to l |
GTSAM_EXPORT Point3 gtsam::triangulateDLT | ( | const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 >> & | projection_matrices, |
const Point2Vector & | measurements, | ||
double | rank_tol = 1e-9 |
||
) |
DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
projection_matrices | Projection matrices (K*P^-1) |
measurements | 2D measurements |
rank_tol | SVD rank tolerance |
GTSAM_EXPORT Point3 gtsam::triangulateDLT | ( | const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 >> & | projection_matrices, |
const std::vector< Unit3 > & | measurements, | ||
double | rank_tol = 1e-9 |
||
) |
overload of previous function to work with Unit3 (projected to canonical camera)
GTSAM_EXPORT Vector4 gtsam::triangulateHomogeneousDLT | ( | const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 >> & | projection_matrices, |
const Point2Vector & | measurements, | ||
double | rank_tol = 1e-9 |
||
) |
DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
projection_matrices | Projection matrices (K*P^-1) |
measurements | 2D measurements |
rank_tol | SVD rank tolerance |
GTSAM_EXPORT Vector4 gtsam::triangulateHomogeneousDLT | ( | const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 >> & | projection_matrices, |
const std::vector< Unit3 > & | measurements, | ||
double | rank_tol = 1e-9 |
||
) |
Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman)
projection_matrices | Projection matrices (K*P^-1) |
measurements | Unit3 bearing measurements |
rank_tol | SVD rank tolerance |
GTSAM_EXPORT Point3 gtsam::triangulateLOST | ( | const std::vector< Pose3 > & | poses, |
const Point3Vector & | calibratedMeasurements, | ||
const SharedIsotropic & | measurementNoise | ||
) |
Triangulation using the LOST (Linear Optimal Sine Triangulation) algorithm proposed in https://arxiv.org/pdf/2205.12197.pdf by Sebastien Henry and John Christian.
poses | camera poses in world frame |
calibratedMeasurements | measurements in homogeneous coordinates in each camera pose |
measurementNoise | isotropic noise model for the measurements |
Point3 gtsam::triangulateNonlinear | ( | const std::vector< Pose3 > & | poses, |
std::shared_ptr< CALIBRATION > | sharedCal, | ||
const Point2Vector & | measurements, | ||
const Point3 & | initialEstimate, | ||
const SharedNoiseModel & | model = nullptr |
||
) |
Given an initial estimate , refine a point using measurements in several cameras
poses | Camera poses |
sharedCal | shared pointer to single calibration object |
measurements | 2D measurements |
initialEstimate |
Point3 gtsam::triangulateNonlinear | ( | const CameraSet< CAMERA > & | cameras, |
const typename CAMERA::MeasurementVector & | measurements, | ||
const Point3 & | initialEstimate, | ||
const SharedNoiseModel & | model = nullptr |
||
) |
Given an initial estimate , refine a point using measurements in several cameras
cameras | pinhole cameras (monocular or stereo) |
measurements | 2D measurements |
initialEstimate |
Point3 gtsam::triangulatePoint3 | ( | const std::vector< Pose3 > & | poses, |
std::shared_ptr< CALIBRATION > | sharedCal, | ||
const Point2Vector & | measurements, | ||
double | rank_tol = 1e-9 , |
||
bool | optimize = false , |
||
const SharedNoiseModel & | model = nullptr , |
||
const bool | useLOST = false |
||
) |
Function to triangulate 3D landmark point from an arbitrary number of poses (at least 2) using the DLT. The function checks that the resulting point lies in front of all cameras, but has no other checks to verify the quality of the triangulation.
poses | A vector of camera poses |
sharedCal | shared pointer to single calibration object |
measurements | A vector of camera measurements |
rank_tol | rank tolerance, default 1e-9 |
optimize | Flag to turn on nonlinear refinement of triangulation |
useLOST | whether to use the LOST algorithm instead of DLT |
Point3 gtsam::triangulatePoint3 | ( | const CameraSet< CAMERA > & | cameras, |
const typename CAMERA::MeasurementVector & | measurements, | ||
double | rank_tol = 1e-9 , |
||
bool | optimize = false , |
||
const SharedNoiseModel & | model = nullptr , |
||
const bool | useLOST = false |
||
) |
Function to triangulate 3D landmark point from an arbitrary number of poses (at least 2) using the DLT. This function is similar to the one above, except that each camera has its own calibration. The function checks that the resulting point lies in front of all cameras, but has no other checks to verify the quality of the triangulation.
cameras | pinhole cameras |
measurements | A vector of camera measurements |
rank_tol | rank tolerance, default 1e-9 |
optimize | Flag to turn on nonlinear refinement of triangulation |
useLOST | whether to use the LOST algorithm instead of DLT |
std::pair<NonlinearFactorGraph, Values> gtsam::triangulationGraph | ( | const std::vector< Pose3 > & | poses, |
std::shared_ptr< CALIBRATION > | sharedCal, | ||
const Point2Vector & | measurements, | ||
Key | landmarkKey, | ||
const Point3 & | initialEstimate, | ||
const SharedNoiseModel & | model = noiseModel::Unit::Create(2) |
||
) |
Create a factor graph with projection factors from poses and one calibration
poses | Camera poses |
sharedCal | shared pointer to single calibration object (monocular only!) |
measurements | 2D measurements |
landmarkKey | to refer to landmark |
initialEstimate |
std::pair<NonlinearFactorGraph, Values> gtsam::triangulationGraph | ( | const CameraSet< CAMERA > & | cameras, |
const typename CAMERA::MeasurementVector & | measurements, | ||
Key | landmarkKey, | ||
const Point3 & | initialEstimate, | ||
const SharedNoiseModel & | model = nullptr |
||
) |
Create a factor graph with projection factors from pinhole cameras (each camera has a pose and calibration)
cameras | pinhole cameras (monocular or stereo) |
measurements | 2D measurements |
landmarkKey | to refer to landmark |
initialEstimate |
MEASUREMENT gtsam::undistortMeasurementInternal | ( | const CALIBRATION & | cal, |
const MEASUREMENT & | measurement, | ||
std::optional< Cal3_S2 > | pinholeCal = {} |
||
) |
Internal undistortMeasurement to be used by undistortMeasurement and undistortMeasurements
Point2Vector gtsam::undistortMeasurements | ( | const CALIBRATION & | cal, |
const Point2Vector & | measurements | ||
) |
Remove distortion for measurements so as if the measurements came from a pinhole camera.
Removes distortion but maintains the K matrix of the initial cal. Operates by calibrating using full calibration and uncalibrating with only the pinhole component of the calibration.
CALIBRATION | Calibration type to use. |
cal | Calibration with which measurements were taken. |
measurements | Vector of measurements to undistort. |
|
inline |
Specialization for Cal3_S2 as it doesn't need to be undistorted.
CAMERA::MeasurementVector gtsam::undistortMeasurements | ( | const CameraSet< CAMERA > & | cameras, |
const typename CAMERA::MeasurementVector & | measurements | ||
) |
Remove distortion for measurements so as if the measurements came from a pinhole camera.
Removes distortion but maintains the K matrix of the initial calibrations. Operates by calibrating using full calibration and uncalibrating with only the pinhole component of the calibration.
CAMERA | Camera type to use. |
cameras | Cameras corresponding to each measurement. |
measurements | Vector of measurements to undistort. |
|
inline |
Specialize for Cal3_S2 to do nothing.
|
inline |
Specialize for SphericalCamera to do nothing.
std::pair<DecisionTree<L, T1>, DecisionTree<L, T2> > gtsam::unzip | ( | const DecisionTree< L, std::pair< T1, T2 > > & | input | ) |
unzip a DecisionTree with std::pair
values.
input | the DecisionTree with (T1,T2) 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
inf_mask | when true, will not scale with a NaN or inf value. |
Matrix gtsam::wedge | ( | const Vector & | x | ) |
Declaration of wedge (see Murray94book) used to convert from n exponential coordinates to n*n element of the Lie algebra
|
inline |
specialization for pose2 wedge function (generic template in Lie.h)
|
inline |
wedge for Pose3:
xi | 6-dim twist (omega,v) where omega = 3D angular velocity v = 3D velocity |
GTSAM_EXPORT std::list<std::tuple<Vector, double, double> > gtsam::weighted_eliminate | ( | Matrix & | A, |
Vector & | b, | ||
const Vector & | sigmas | ||
) |
Imperative algorithm for in-place full elimination with weights and constraint handling
A | is a matrix to eliminate |
b | is the rhs |
sigmas | is a vector of the measurement standard deviation |
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
v | is the first column of the matrix to solve |
weights | is a vector of weights/precisions where w=1/(s*s) |
GTSAM_EXPORT bool gtsam::writeBAL | ( | const std::string & | filename, |
const SfmData & | data | ||
) |
This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure.
filename | The name of the BAL file to write |
data | SfM structure where the data is stored |
GTSAM_EXPORT bool gtsam::writeBALfromValues | ( | const std::string & | filename, |
const SfmData & | data, | ||
const Values & | values | ||
) |
This function writes a "Bundle Adjustment in the Large" (BAL) file from a SfmData structure and a value structure (measurements are the same as the SfM input data, while camera poses and values are read from Values)
filename | The name of the BAL file to write |
data | SfM structure where the data is stored |
values | structure where the graph values are stored (values can be either Pose3 or PinholeCamera<Cal3Bundler> for the cameras, and should be Point3 for the 3D points). Note: assumes that the keys are "i" for pose i and "Symbol::('p',j)" for landmark j. |
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.
filename | The name of the g2o file to write |
graph | NonlinearFactor graph storing the measurements |
estimate | Values |
Note:behavior change in PR #471: to be consistent with load2D and load3D, we write the indices to file and not the full Keys. This change really only affects landmarks, which get read as indices but stored in values with the symbol L(index).
void gtsam::zeroBelowDiagonal | ( | MATRIX & | A, |
size_t | cols = 0 |
||
) |
Zeros all of the elements below the diagonal of a matrix, in place
A | is a matrix, to be modified in place |
cols | is the number of columns to zero, use zero for all columns |