2011-12-02 06:06:03 +08:00
/**
2011-12-07 11:05:37 +08:00
* GTSAM Wrap Module Definition
2011-12-02 06:06:03 +08:00
*
* These are the current classes available through the matlab toolbox interface ,
* add more functions / classes as they are available .
*
* Requirements :
2011-12-07 11:05:37 +08:00
* Classes must start with an uppercase letter
2012-10-02 22:40:07 +08:00
* - Can wrap a typedef
2012-07-10 02:51:07 +08:00
* Only one Method / Constructor per line , though methods / constructors can extend across multiple lines
2011-12-07 11:05:37 +08:00
* Methods can return
* - Eigen types : Matrix , Vector
2013-01-27 05:22:56 +08:00
* - C / C + + basic types : string , bool , size_t , int , double , char , unsigned char
2011-12-07 11:05:37 +08:00
* - void
* - Any class with which be copied with boost : : make_shared ( )
* - boost : : shared_ptr of any object type
2012-07-14 07:19:37 +08:00
* Constructors
2012-07-10 04:19:37 +08:00
* - Overloads are supported
* - A class with no constructors can be returned from other functions but not allocated directly in MATLAB
* Methods
2012-06-24 03:24:57 +08:00
* - Constness has no effect
2012-07-10 04:27:23 +08:00
* - Specify by - value ( not reference ) return types , even if C + + method returns reference
2013-04-22 03:29:41 +08:00
* - Must start with a letter ( upper or lowercase )
2012-07-10 04:19:37 +08:00
* - Overloads are supported
* Static methods
* - Must start with a letter ( upper or lowercase ) and use the " static " keyword
2012-07-10 04:27:23 +08:00
* - The first letter will be made uppercase in the generated MATLAB interface
2012-07-10 04:19:37 +08:00
* - Overloads are supported
2012-07-14 07:19:37 +08:00
* Arguments to functions any of
2012-10-02 22:40:07 +08:00
* - Eigen types : Matrix , Vector
* - Eigen types and classes as an optionally const reference
2012-06-05 04:51:28 +08:00
* - C / C + + basic types : string , bool , size_t , size_t , double , char , unsigned char
2011-12-07 11:05:37 +08:00
* - Any class with which be copied with boost : : make_shared ( ) ( except Eigen )
* - boost : : shared_ptr of any object type ( except Eigen )
* Comments can use either C + + or C style , with multiple lines
2011-12-09 04:58:06 +08:00
* Namespace definitions
* - Names of namespaces must start with a lowercase letter
2012-10-02 22:40:07 +08:00
* - start a namespace with " namespace { "
* - end a namespace with exactly " } "
* - Namespaces can be nested
2011-12-09 04:58:06 +08:00
* Namespace usage
2012-10-02 22:40:07 +08:00
* - Namespaces can be specified for classes in arguments and return values
* - In each case , the namespace must be fully specified , e . g . , " namespace1::namespace2::ClassName "
2011-12-10 00:36:50 +08:00
* Includes in C + + wrappers
2012-07-18 02:30:02 +08:00
* - All includes will be collected and added in a single file
* - All namespaces must have angle brackets : < path >
* - No default includes will be added
2012-07-24 02:24:39 +08:00
* Global / Namespace functions
* - Functions specified outside of a class are global
* - Can be overloaded with different arguments
* - Can have multiple functions of the same name in different namespaces
2012-07-10 04:19:37 +08:00
* Using classes defined in other modules
2011-12-16 05:27:08 +08:00
* - If you are using a class ' OtherClass ' not wrapped in this definition file , add " class OtherClass; " to avoid a dependency error
2012-07-10 04:19:37 +08:00
* Virtual inheritance
2012-09-11 01:00:02 +08:00
* - Specify fully - qualified base classes , i . e . " virtual class Derived : ns::Base { " where " ns " is the namespace
* - Mark with ' virtual ' keyword , e . g . " virtual class Base { " , and also " virtual class Derived : ns::Base { "
* - Forward declarations must also be marked virtual , e . g . " virtual class ns::Base; " and
* also " virtual class ns::Derived; "
2012-07-10 04:19:37 +08:00
* - Pure virtual ( abstract ) classes should list no constructors in this interface file
* - Virtual classes must have a clone ( ) function in C + + ( though it does not have to be included
* in the MATLAB interface ) . clone ( ) will be called whenever an object copy is needed , instead
* of using the copy constructor ( which is used for non - virtual objects ) .
2012-09-11 01:00:02 +08:00
* - Signature of clone function - will be called virtually , so must appear at least at the top of the inheritance tree
2012-10-02 22:40:07 +08:00
* virtual boost : : shared_ptr < CLASS_NAME > clone ( ) const ;
2013-06-13 22:29:32 +08:00
* Class Templates
2012-07-14 07:19:37 +08:00
* - Basic templates are supported either with an explicit list of types to instantiate ,
* e . g . template < T = { gtsam : : Pose2 , gtsam : : Rot2 , gtsam : : Point3 } > class Class1 { . . . } ;
* or with typedefs , e . g .
* template < T , U > class Class2 { . . . } ;
* typedef Class2 < Type1 , Type2 > MyInstantiatedClass ;
2012-07-24 05:27:36 +08:00
* - In the class definition , appearances of the template argument ( s ) will be replaced with their
* instantiated types , e . g . ' void setValue ( const T & value ) ; ' .
* - To refer to the instantiation of the template class itself , use ' This ' , i . e . ' static This Create ( ) ; '
2012-07-14 07:19:37 +08:00
* - To create new instantiations in other modules , you must copy - and - paste the whole class definition
* into the new module , but use only your new instantiation types .
* - When forward - declaring template instantiations , use the generated / typedefed name , e . g .
* class gtsam : : Class1Pose2 ;
* class gtsam : : MyInstantiatedClass ;
2013-06-20 01:50:10 +08:00
* Boost . serialization within Matlab :
* - you need to mark classes as being serializable in the markup file ( see this file for an example ) .
* - There are two options currently , depending on the class . To " mark " a class as serializable ,
* add a function with a particular signature so that wrap will catch it .
* - Add " void serialize() " to a class to create serialization functions for a class .
* Adding this flag subsumes the serializable ( ) flag below . Requirements :
* - A default constructor must be publicly accessible
* - Must not be an abstract base class
* - The class must have an actual boost . serialization serialize ( ) function .
* - Add " void serializable() " to a class if you only want the class to be serialized as a
* part of a container ( such as noisemodel ) . This version does not require a publicly
* accessible default constructor .
2011-12-07 11:05:37 +08:00
*/
/**
* Status :
2011-12-07 23:23:20 +08:00
* - TODO : default values for arguments
2013-06-13 22:29:32 +08:00
* - WORKAROUND : make multiple versions of the same function for different configurations of default arguments
2012-01-31 06:00:13 +08:00
* - TODO : Handle gtsam : : Rot3M conversions to quaternions
2013-06-13 22:29:32 +08:00
* - TODO : Parse return of const ref arguments
* - TODO : Parse std : : string variants and convert directly to special string
* - TODO : Add enum support
* - TODO : Add generalized serialization support via boost . serialization with hooks to matlab save / load
2011-12-02 06:06:03 +08:00
*/
2012-12-18 06:30:54 +08:00
namespace std {
# include <vector>
2012-09-08 07:10:49 +08:00
template < T >
class vector
{
2012-12-18 06:30:54 +08:00
//Do we need these?
2012-09-11 01:00:02 +08:00
//Capacity
2012-12-18 06:30:54 +08:00
/*size_t size() const;
2012-09-08 07:10:49 +08:00
size_t max_size ( ) const ;
2012-12-18 06:30:54 +08:00
//void resize(size_t sz);
2012-09-08 07:10:49 +08:00
size_t capacity ( ) const ;
bool empty ( ) const ;
void reserve ( size_t n ) ;
2013-06-13 22:29:32 +08:00
//Element access
2012-12-18 06:30:54 +08:00
T * at ( size_t n ) ;
T * front ( ) ;
T * back ( ) ;
2012-09-08 07:10:49 +08:00
//Modifiers
void assign ( size_t n , const T & u ) ;
void push_back ( const T & x ) ;
2012-12-18 06:30:54 +08:00
void pop_back ( ) ; */
} ;
//typedef std::vector
# include <list>
template < T >
class list
{
2012-09-08 07:10:49 +08:00
} ;
2012-12-18 06:30:54 +08:00
}
2011-12-10 04:29:47 +08:00
2012-12-18 06:30:54 +08:00
namespace gtsam {
2012-09-08 07:10:49 +08:00
2012-01-18 04:04:37 +08:00
//*************************************************************************
// base
//*************************************************************************
2012-07-24 02:24:37 +08:00
/** gtsam namespace functions */
bool linear_independent ( Matrix A , Matrix B , double tol ) ;
2012-07-10 04:19:37 +08:00
virtual class Value {
2012-10-02 22:40:07 +08:00
// No constructors because this is an abstract class
2012-07-09 08:02:43 +08:00
2012-10-02 22:40:07 +08:00
// Testable
void print ( string s ) const ;
2012-07-08 20:27:39 +08:00
2012-10-02 22:40:07 +08:00
// Manifold
size_t dim ( ) const ;
2012-07-08 20:27:39 +08:00
} ;
2014-11-08 05:41:21 +08:00
class Vector3 {
Vector3 ( Vector v ) ;
} ;
class Vector6 {
Vector6 ( Vector v ) ;
} ;
2012-08-11 10:53:56 +08:00
# include <gtsam/base/LieScalar.h>
2014-11-07 23:41:43 +08:00
class LieScalar {
2012-10-02 22:40:07 +08:00
// Standard constructors
LieScalar ( ) ;
LieScalar ( double d ) ;
2012-08-11 10:53:56 +08:00
2012-10-02 22:40:07 +08:00
// Standard interface
double value ( ) const ;
2012-08-11 10:53:56 +08:00
2012-10-02 22:40:07 +08:00
// Testable
void print ( string s ) const ;
bool equals ( const gtsam : : LieScalar & expected , double tol ) const ;
2012-08-11 10:53:56 +08:00
2012-10-02 22:40:07 +08:00
// Group
static gtsam : : LieScalar identity ( ) ;
gtsam : : LieScalar inverse ( ) const ;
gtsam : : LieScalar compose ( const gtsam : : LieScalar & p ) const ;
gtsam : : LieScalar between ( const gtsam : : LieScalar & l2 ) const ;
2012-08-11 10:53:56 +08:00
2012-10-02 22:40:07 +08:00
// Manifold
size_t dim ( ) const ;
gtsam : : LieScalar retract ( Vector v ) const ;
Vector localCoordinates ( const gtsam : : LieScalar & t2 ) const ;
2012-08-11 10:53:56 +08:00
2012-10-02 22:40:07 +08:00
// Lie group
static gtsam : : LieScalar Expmap ( Vector v ) ;
static Vector Logmap ( const gtsam : : LieScalar & p ) ;
2012-08-11 10:53:56 +08:00
} ;
2012-07-18 02:30:02 +08:00
# include <gtsam/base/LieVector.h>
2014-11-07 23:41:43 +08:00
class LieVector {
2012-10-02 22:40:07 +08:00
// Standard constructors
LieVector ( ) ;
LieVector ( Vector v ) ;
2012-06-19 02:12:19 +08:00
2012-10-02 22:40:07 +08:00
// Standard interface
Vector vector ( ) const ;
2012-06-19 02:12:19 +08:00
2012-10-02 22:40:07 +08:00
// Testable
void print ( string s ) const ;
bool equals ( const gtsam : : LieVector & expected , double tol ) const ;
2012-06-19 02:12:19 +08:00
2012-10-02 22:40:07 +08:00
// Group
static gtsam : : LieVector identity ( ) ;
gtsam : : LieVector inverse ( ) const ;
gtsam : : LieVector compose ( const gtsam : : LieVector & p ) const ;
gtsam : : LieVector between ( const gtsam : : LieVector & l2 ) const ;
2012-06-19 02:12:19 +08:00
2012-10-02 22:40:07 +08:00
// Manifold
size_t dim ( ) const ;
gtsam : : LieVector retract ( Vector v ) const ;
Vector localCoordinates ( const gtsam : : LieVector & t2 ) const ;
2012-06-19 02:12:19 +08:00
2012-10-02 22:40:07 +08:00
// Lie group
static gtsam : : LieVector Expmap ( Vector v ) ;
static Vector Logmap ( const gtsam : : LieVector & p ) ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2012-06-19 02:12:19 +08:00
} ;
2012-07-18 02:30:02 +08:00
# include <gtsam/base/LieMatrix.h>
2014-11-07 23:41:43 +08:00
class LieMatrix {
2012-10-02 22:40:07 +08:00
// Standard constructors
LieMatrix ( ) ;
LieMatrix ( Matrix v ) ;
2012-07-17 23:51:32 +08:00
2012-10-02 22:40:07 +08:00
// Standard interface
Matrix matrix ( ) const ;
2012-07-17 23:51:32 +08:00
2012-10-02 22:40:07 +08:00
// Testable
void print ( string s ) const ;
bool equals ( const gtsam : : LieMatrix & expected , double tol ) const ;
2012-07-17 23:51:32 +08:00
2012-10-02 22:40:07 +08:00
// Group
static gtsam : : LieMatrix identity ( ) ;
gtsam : : LieMatrix inverse ( ) const ;
gtsam : : LieMatrix compose ( const gtsam : : LieMatrix & p ) const ;
gtsam : : LieMatrix between ( const gtsam : : LieMatrix & l2 ) const ;
2012-07-17 23:51:32 +08:00
2012-10-02 22:40:07 +08:00
// Manifold
size_t dim ( ) const ;
gtsam : : LieMatrix retract ( Vector v ) const ;
Vector localCoordinates ( const gtsam : : LieMatrix & t2 ) const ;
2012-07-17 23:51:32 +08:00
2012-10-02 22:40:07 +08:00
// Lie group
static gtsam : : LieMatrix Expmap ( Vector v ) ;
static Vector Logmap ( const gtsam : : LieMatrix & p ) ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2012-07-17 23:51:32 +08:00
} ;
2012-01-18 04:04:37 +08:00
//*************************************************************************
// geometry
//*************************************************************************
2014-11-07 23:41:43 +08:00
class Point2 {
2012-06-05 04:17:24 +08:00
// Standard Constructors
2012-10-02 22:40:07 +08:00
Point2 ( ) ;
Point2 ( double x , double y ) ;
Point2 ( Vector v ) ;
2012-06-05 04:17:24 +08:00
// Testable
void print ( string s ) const ;
bool equals ( const gtsam : : Point2 & pose , double tol ) const ;
// Group
static gtsam : : Point2 identity ( ) ;
gtsam : : Point2 inverse ( ) const ;
gtsam : : Point2 compose ( const gtsam : : Point2 & p2 ) const ;
gtsam : : Point2 between ( const gtsam : : Point2 & p2 ) const ;
// Manifold
static size_t Dim ( ) ;
size_t dim ( ) const ;
gtsam : : Point2 retract ( Vector v ) const ;
Vector localCoordinates ( const gtsam : : Point2 & p ) const ;
// Lie Group
static gtsam : : Point2 Expmap ( Vector v ) ;
2012-07-31 04:16:54 +08:00
static Vector Logmap ( const gtsam : : Point2 & p ) ;
2012-06-05 04:17:24 +08:00
// Standard Interface
double x ( ) const ;
2012-07-31 04:16:54 +08:00
double y ( ) const ;
2012-06-05 04:17:24 +08:00
Vector vector ( ) const ;
2012-07-31 04:16:54 +08:00
double dist ( const gtsam : : Point2 & p2 ) const ;
2012-12-18 06:30:54 +08:00
double norm ( ) const ;
2013-06-20 01:50:01 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2011-10-14 02:41:56 +08:00
} ;
2014-11-07 23:41:43 +08:00
class StereoPoint2 {
2012-06-05 04:17:24 +08:00
// Standard Constructors
2012-06-04 10:41:14 +08:00
StereoPoint2 ( ) ;
StereoPoint2 ( double uL , double uR , double v ) ;
2012-06-05 04:17:24 +08:00
// Testable
void print ( string s ) const ;
bool equals ( const gtsam : : StereoPoint2 & point , double tol ) const ;
// Group
static gtsam : : StereoPoint2 identity ( ) ;
gtsam : : StereoPoint2 inverse ( ) const ;
gtsam : : StereoPoint2 compose ( const gtsam : : StereoPoint2 & p2 ) const ;
gtsam : : StereoPoint2 between ( const gtsam : : StereoPoint2 & p2 ) const ;
// Manifold
static size_t Dim ( ) ;
size_t dim ( ) const ;
gtsam : : StereoPoint2 retract ( Vector v ) const ;
Vector localCoordinates ( const gtsam : : StereoPoint2 & p ) const ;
// Lie Group
2012-06-04 10:41:14 +08:00
static gtsam : : StereoPoint2 Expmap ( Vector v ) ;
static Vector Logmap ( const gtsam : : StereoPoint2 & p ) ;
2012-06-05 04:17:24 +08:00
// Standard Interface
Vector vector ( ) const ;
2013-07-26 01:01:33 +08:00
double uL ( ) const ;
double uR ( ) const ;
double v ( ) const ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2012-06-04 10:41:14 +08:00
} ;
2014-11-07 23:41:43 +08:00
class Point3 {
2012-06-05 04:17:24 +08:00
// Standard Constructors
2012-10-02 22:40:07 +08:00
Point3 ( ) ;
Point3 ( double x , double y , double z ) ;
Point3 ( Vector v ) ;
2012-06-05 04:17:24 +08:00
// Testable
void print ( string s ) const ;
bool equals ( const gtsam : : Point3 & p , double tol ) const ;
// Group
static gtsam : : Point3 identity ( ) ;
gtsam : : Point3 inverse ( ) const ;
gtsam : : Point3 compose ( const gtsam : : Point3 & p2 ) const ;
gtsam : : Point3 between ( const gtsam : : Point3 & p2 ) const ;
// Manifold
static size_t Dim ( ) ;
size_t dim ( ) const ;
gtsam : : Point3 retract ( Vector v ) const ;
Vector localCoordinates ( const gtsam : : Point3 & p ) const ;
// Lie Group
static gtsam : : Point3 Expmap ( Vector v ) ;
2012-10-02 22:40:07 +08:00
static Vector Logmap ( const gtsam : : Point3 & p ) ;
2012-06-05 04:17:24 +08:00
// Standard Interface
2012-10-02 22:40:07 +08:00
Vector vector ( ) const ;
double x ( ) const ;
double y ( ) const ;
double z ( ) const ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2011-10-21 13:07:35 +08:00
} ;
2011-10-14 02:41:56 +08:00
2014-11-07 23:41:43 +08:00
class Rot2 {
2012-06-05 04:17:24 +08:00
// Standard Constructors and Named Constructors
2012-10-02 22:40:07 +08:00
Rot2 ( ) ;
Rot2 ( double theta ) ;
2012-06-05 04:17:24 +08:00
static gtsam : : Rot2 fromAngle ( double theta ) ;
static gtsam : : Rot2 fromDegrees ( double theta ) ;
static gtsam : : Rot2 fromCosSin ( double c , double s ) ;
// Testable
void print ( string s ) const ;
bool equals ( const gtsam : : Rot2 & rot , double tol ) const ;
// Group
static gtsam : : Rot2 identity ( ) ;
2012-06-05 04:51:28 +08:00
gtsam : : Rot2 inverse ( ) ;
2012-06-05 04:17:24 +08:00
gtsam : : Rot2 compose ( const gtsam : : Rot2 & p2 ) const ;
gtsam : : Rot2 between ( const gtsam : : Rot2 & p2 ) const ;
// Manifold
static size_t Dim ( ) ;
size_t dim ( ) const ;
gtsam : : Rot2 retract ( Vector v ) const ;
Vector localCoordinates ( const gtsam : : Rot2 & p ) const ;
// Lie Group
static gtsam : : Rot2 Expmap ( Vector v ) ;
2012-10-02 22:40:07 +08:00
static Vector Logmap ( const gtsam : : Rot2 & p ) ;
2012-06-05 04:17:24 +08:00
// Group Action on Point2
gtsam : : Point2 rotate ( const gtsam : : Point2 & point ) const ;
gtsam : : Point2 unrotate ( const gtsam : : Point2 & point ) const ;
// Standard Interface
2012-10-02 22:40:07 +08:00
static gtsam : : Rot2 relativeBearing ( const gtsam : : Point2 & d ) ; // Ignoring derivative
static gtsam : : Rot2 atan2 ( double y , double x ) ;
double theta ( ) const ;
double degrees ( ) const ;
double c ( ) const ;
double s ( ) const ;
2012-06-05 04:17:24 +08:00
Matrix matrix ( ) const ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2011-10-14 12:43:40 +08:00
} ;
2014-11-07 23:41:43 +08:00
class Rot3 {
2012-06-05 04:17:24 +08:00
// Standard Constructors and Named Constructors
2012-10-02 22:40:07 +08:00
Rot3 ( ) ;
Rot3 ( Matrix R ) ;
static gtsam : : Rot3 Rx ( double t ) ;
static gtsam : : Rot3 Ry ( double t ) ;
static gtsam : : Rot3 Rz ( double t ) ;
2013-02-16 01:27:38 +08:00
static gtsam : : Rot3 RzRyRx ( double x , double y , double z ) ;
2012-10-02 22:40:07 +08:00
static gtsam : : Rot3 RzRyRx ( Vector xyz ) ;
static gtsam : : Rot3 yaw ( double t ) ; // positive yaw is to right (as in aircraft heading)
static gtsam : : Rot3 pitch ( double t ) ; // positive pitch is up (increasing aircraft altitude)
static gtsam : : Rot3 roll ( double t ) ; // positive roll is to right (increasing yaw in aircraft)
static gtsam : : Rot3 ypr ( double y , double p , double r ) ;
static gtsam : : Rot3 quaternion ( double w , double x , double y , double z ) ;
static gtsam : : Rot3 rodriguez ( Vector v ) ;
2012-06-05 04:17:24 +08:00
// Testable
2012-10-02 22:40:07 +08:00
void print ( string s ) const ;
bool equals ( const gtsam : : Rot3 & rot , double tol ) const ;
2012-06-05 04:17:24 +08:00
// Group
2012-10-02 22:40:07 +08:00
static gtsam : : Rot3 identity ( ) ;
2012-12-18 06:30:54 +08:00
gtsam : : Rot3 inverse ( ) const ;
2012-10-02 22:40:07 +08:00
gtsam : : Rot3 compose ( const gtsam : : Rot3 & p2 ) const ;
gtsam : : Rot3 between ( const gtsam : : Rot3 & p2 ) const ;
2012-06-05 04:17:24 +08:00
// Manifold
static size_t Dim ( ) ;
size_t dim ( ) const ;
2013-06-06 13:01:16 +08:00
//gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options
2012-06-05 04:17:24 +08:00
gtsam : : Rot3 retract ( Vector v ) const ;
Vector localCoordinates ( const gtsam : : Rot3 & p ) const ;
// Group Action on Point3
2012-10-02 22:40:07 +08:00
gtsam : : Point3 rotate ( const gtsam : : Point3 & p ) const ;
gtsam : : Point3 unrotate ( const gtsam : : Point3 & p ) const ;
2012-06-05 04:17:24 +08:00
// Standard Interface
2012-10-02 22:40:07 +08:00
static gtsam : : Rot3 Expmap ( Vector v ) ;
static Vector Logmap ( const gtsam : : Rot3 & p ) ;
Matrix matrix ( ) const ;
Matrix transpose ( ) const ;
gtsam : : Point3 column ( size_t index ) const ;
Vector xyz ( ) const ;
Vector ypr ( ) const ;
Vector rpy ( ) const ;
double roll ( ) const ;
double pitch ( ) const ;
double yaw ( ) const ;
2011-12-07 23:23:20 +08:00
// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly
2013-03-26 01:58:11 +08:00
Vector quaternion ( ) const ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2011-12-01 09:59:34 +08:00
} ;
2014-11-07 23:41:43 +08:00
class Pose2 {
2012-06-05 04:17:24 +08:00
// Standard Constructor
2012-10-02 22:40:07 +08:00
Pose2 ( ) ;
2013-06-21 03:44:50 +08:00
Pose2 ( const gtsam : : Pose2 & pose ) ;
2012-10-02 22:40:07 +08:00
Pose2 ( double x , double y , double theta ) ;
Pose2 ( double theta , const gtsam : : Point2 & t ) ;
Pose2 ( const gtsam : : Rot2 & r , const gtsam : : Point2 & t ) ;
Pose2 ( Vector v ) ;
2012-06-05 04:17:24 +08:00
// Testable
void print ( string s ) const ;
bool equals ( const gtsam : : Pose2 & pose , double tol ) const ;
// Group
static gtsam : : Pose2 identity ( ) ;
gtsam : : Pose2 inverse ( ) const ;
gtsam : : Pose2 compose ( const gtsam : : Pose2 & p2 ) const ;
gtsam : : Pose2 between ( const gtsam : : Pose2 & p2 ) const ;
// Manifold
static size_t Dim ( ) ;
size_t dim ( ) const ;
gtsam : : Pose2 retract ( Vector v ) const ;
Vector localCoordinates ( const gtsam : : Pose2 & p ) const ;
// Lie Group
2012-10-02 22:40:07 +08:00
static gtsam : : Pose2 Expmap ( Vector v ) ;
static Vector Logmap ( const gtsam : : Pose2 & p ) ;
2013-05-01 01:20:23 +08:00
Matrix AdjointMap ( ) const ;
Vector Adjoint ( const Vector & xi ) const ;
2012-06-05 04:51:28 +08:00
static Matrix wedge ( double vx , double vy , double w ) ;
2012-06-05 04:17:24 +08:00
// Group Actions on Point2
gtsam : : Point2 transform_from ( const gtsam : : Point2 & p ) const ;
gtsam : : Point2 transform_to ( const gtsam : : Point2 & p ) const ;
// Standard Interface
2012-10-02 22:40:07 +08:00
double x ( ) const ;
double y ( ) const ;
double theta ( ) const ;
gtsam : : Rot2 bearing ( const gtsam : : Point2 & point ) const ;
double range ( const gtsam : : Point2 & point ) const ;
gtsam : : Point2 translation ( ) const ;
gtsam : : Rot2 rotation ( ) const ;
2012-06-05 04:17:24 +08:00
Matrix matrix ( ) const ;
2013-06-20 01:49:57 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2011-10-21 13:07:35 +08:00
} ;
2014-11-07 23:41:43 +08:00
class Pose3 {
2012-10-02 22:40:07 +08:00
// Standard Constructors
Pose3 ( ) ;
Pose3 ( const gtsam : : Pose3 & pose ) ;
Pose3 ( const gtsam : : Rot3 & r , const gtsam : : Point3 & t ) ;
Pose3 ( const gtsam : : Pose2 & pose2 ) ; // FIXME: shadows Pose3(Pose3 pose)
Pose3 ( Matrix t ) ;
// Testable
void print ( string s ) const ;
bool equals ( const gtsam : : Pose3 & pose , double tol ) const ;
// Group
static gtsam : : Pose3 identity ( ) ;
gtsam : : Pose3 inverse ( ) const ;
gtsam : : Pose3 compose ( const gtsam : : Pose3 & p2 ) const ;
gtsam : : Pose3 between ( const gtsam : : Pose3 & p2 ) const ;
// Manifold
static size_t Dim ( ) ;
size_t dim ( ) const ;
gtsam : : Pose3 retract ( Vector v ) const ;
gtsam : : Pose3 retractFirstOrder ( Vector v ) const ;
Vector localCoordinates ( const gtsam : : Pose3 & T2 ) const ;
// Lie Group
static gtsam : : Pose3 Expmap ( Vector v ) ;
static Vector Logmap ( const gtsam : : Pose3 & p ) ;
2013-04-22 03:29:41 +08:00
Matrix AdjointMap ( ) const ;
Vector Adjoint ( Vector xi ) const ;
2012-10-02 22:40:07 +08:00
static Matrix wedge ( double wx , double wy , double wz , double vx , double vy , double vz ) ;
// Group Action on Point3
gtsam : : Point3 transform_from ( const gtsam : : Point3 & p ) const ;
gtsam : : Point3 transform_to ( const gtsam : : Point3 & p ) const ;
// Standard Interface
gtsam : : Rot3 rotation ( ) const ;
gtsam : : Point3 translation ( ) const ;
double x ( ) const ;
double y ( ) const ;
double z ( ) const ;
Matrix matrix ( ) const ;
gtsam : : Pose3 transform_to ( const gtsam : : Pose3 & pose ) const ; // FIXME: shadows other transform_to()
double range ( const gtsam : : Point3 & point ) ;
double range ( const gtsam : : Pose3 & pose ) ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2011-12-01 09:59:34 +08:00
} ;
2014-02-23 05:20:28 +08:00
# include <gtsam/geometry/Unit3.h>
2014-11-07 23:41:43 +08:00
class Unit3 {
2013-12-18 00:18:31 +08:00
// Standard Constructors
2014-02-23 05:20:28 +08:00
Unit3 ( ) ;
Unit3 ( const gtsam : : Point3 & pose ) ;
2013-12-18 00:18:31 +08:00
// Testable
void print ( string s ) const ;
2014-02-23 05:20:28 +08:00
bool equals ( const gtsam : : Unit3 & pose , double tol ) const ;
2013-12-18 00:18:31 +08:00
// Other functionality
2013-12-18 11:11:58 +08:00
Matrix basis ( ) const ;
2013-12-18 00:18:31 +08:00
Matrix skew ( ) const ;
// Manifold
static size_t Dim ( ) ;
size_t dim ( ) const ;
2014-02-23 05:20:28 +08:00
gtsam : : Unit3 retract ( Vector v ) const ;
Vector localCoordinates ( const gtsam : : Unit3 & s ) const ;
2013-12-18 00:18:31 +08:00
} ;
# include <gtsam/geometry/EssentialMatrix.h>
2014-11-07 23:41:43 +08:00
class EssentialMatrix {
2013-12-18 00:18:31 +08:00
// Standard Constructors
2014-02-23 05:20:28 +08:00
EssentialMatrix ( const gtsam : : Rot3 & aRb , const gtsam : : Unit3 & aTb ) ;
2013-12-18 00:18:31 +08:00
// Testable
void print ( string s ) const ;
bool equals ( const gtsam : : EssentialMatrix & pose , double tol ) const ;
// Manifold
static size_t Dim ( ) ;
size_t dim ( ) const ;
gtsam : : EssentialMatrix retract ( Vector v ) const ;
Vector localCoordinates ( const gtsam : : EssentialMatrix & s ) const ;
// Other methods:
gtsam : : Rot3 rotation ( ) const ;
2014-02-23 05:20:28 +08:00
gtsam : : Unit3 direction ( ) const ;
2013-12-18 00:18:31 +08:00
Matrix matrix ( ) const ;
double error ( Vector vA , Vector vB ) ;
} ;
2014-11-07 23:41:43 +08:00
class Cal3_S2 {
2012-06-05 04:17:24 +08:00
// Standard Constructors
2012-06-04 10:41:14 +08:00
Cal3_S2 ( ) ;
Cal3_S2 ( double fx , double fy , double s , double u0 , double v0 ) ;
2012-10-02 22:40:07 +08:00
Cal3_S2 ( Vector v ) ;
Cal3_S2 ( double fov , int w , int h ) ;
2012-06-04 10:41:14 +08:00
2012-06-05 04:17:24 +08:00
// Testable
2012-06-04 10:41:14 +08:00
void print ( string s ) const ;
2012-07-12 10:11:32 +08:00
bool equals ( const gtsam : : Cal3_S2 & rhs , double tol ) const ;
2012-06-05 04:17:24 +08:00
// Manifold
static size_t Dim ( ) ;
size_t dim ( ) const ;
gtsam : : Cal3_S2 retract ( Vector v ) const ;
Vector localCoordinates ( const gtsam : : Cal3_S2 & c ) const ;
// Action on Point2
gtsam : : Point2 calibrate ( const gtsam : : Point2 & p ) const ;
gtsam : : Point2 uncalibrate ( const gtsam : : Point2 & p ) const ;
// Standard Interface
double fx ( ) const ;
double fy ( ) const ;
double skew ( ) const ;
double px ( ) const ;
double py ( ) const ;
gtsam : : Point2 principalPoint ( ) const ;
Vector vector ( ) const ;
Matrix matrix ( ) const ;
Matrix matrix_inverse ( ) const ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2012-06-04 10:41:14 +08:00
} ;
2012-07-18 02:30:02 +08:00
# include <gtsam/geometry/Cal3DS2.h>
2014-11-07 23:41:43 +08:00
class Cal3DS2 {
2012-10-02 22:40:07 +08:00
// Standard Constructors
Cal3DS2 ( ) ;
Cal3DS2 ( double fx , double fy , double s , double u0 , double v0 , double k1 , double k2 , double k3 , double k4 ) ;
Cal3DS2 ( Vector v ) ;
// Testable
void print ( string s ) const ;
bool equals ( const gtsam : : Cal3DS2 & rhs , double tol ) const ;
// Manifold
static size_t Dim ( ) ;
size_t dim ( ) const ;
gtsam : : Cal3DS2 retract ( Vector v ) const ;
Vector localCoordinates ( const gtsam : : Cal3DS2 & c ) const ;
// Action on Point2
2013-02-16 01:27:38 +08:00
gtsam : : Point2 calibrate ( const gtsam : : Point2 & p , double tol ) const ;
gtsam : : Point2 calibrate ( const gtsam : : Point2 & p ) const ;
2012-10-02 22:40:07 +08:00
gtsam : : Point2 uncalibrate ( const gtsam : : Point2 & p ) const ;
// TODO: D2d functions that start with an uppercase letter
// Standard Interface
double fx ( ) const ;
double fy ( ) const ;
double skew ( ) const ;
double px ( ) const ;
double py ( ) const ;
Vector vector ( ) const ;
Vector k ( ) const ;
//Matrix K() const; //FIXME: Uppercase
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2012-07-12 10:11:32 +08:00
} ;
2012-06-04 10:41:14 +08:00
class Cal3_S2Stereo {
2012-06-05 04:17:24 +08:00
// Standard Constructors
2012-06-04 10:41:14 +08:00
Cal3_S2Stereo ( ) ;
Cal3_S2Stereo ( double fx , double fy , double s , double u0 , double v0 , double b ) ;
2013-07-23 04:26:20 +08:00
Cal3_S2Stereo ( Vector v ) ;
2012-06-04 10:41:14 +08:00
2012-06-05 04:17:24 +08:00
// Testable
2012-06-04 10:41:14 +08:00
void print ( string s ) const ;
2013-07-23 23:32:38 +08:00
bool equals ( const gtsam : : Cal3_S2Stereo & K , double tol ) const ;
2012-06-05 04:17:24 +08:00
// Standard Interface
double fx ( ) const ;
double fy ( ) const ;
double skew ( ) const ;
double px ( ) const ;
double py ( ) const ;
gtsam : : Point2 principalPoint ( ) const ;
double baseline ( ) const ;
2012-06-04 10:41:14 +08:00
} ;
2014-11-11 01:06:09 +08:00
# include <gtsam/geometry/Cal3Bundler.h>
class Cal3Bundler {
// Standard Constructors
Cal3Bundler ( ) ;
Cal3Bundler ( double fx , double k1 , double k2 , double u0 , double v0 ) ;
// Testable
void print ( string s ) const ;
bool equals ( const gtsam : : Cal3Bundler & rhs , double tol ) const ;
// Manifold
static size_t Dim ( ) ;
size_t dim ( ) const ;
gtsam : : Cal3Bundler retract ( Vector v ) const ;
Vector localCoordinates ( const gtsam : : Cal3Bundler & c ) const ;
// Action on Point2
gtsam : : Point2 calibrate ( const gtsam : : Point2 & p , double tol ) const ;
gtsam : : Point2 calibrate ( const gtsam : : Point2 & p ) const ;
gtsam : : Point2 uncalibrate ( const gtsam : : Point2 & p ) const ;
// Standard Interface
double fx ( ) const ;
double fy ( ) const ;
double k1 ( ) const ;
double k2 ( ) const ;
double u0 ( ) const ;
double v0 ( ) const ;
Vector vector ( ) const ;
Vector k ( ) const ;
//Matrix K() const; //FIXME: Uppercase
// enabling serialization functionality
void serialize ( ) const ;
} ;
2014-11-07 23:41:43 +08:00
class CalibratedCamera {
2012-06-05 04:17:24 +08:00
// Standard Constructors and Named Constructors
2012-10-02 22:40:07 +08:00
CalibratedCamera ( ) ;
CalibratedCamera ( const gtsam : : Pose3 & pose ) ;
CalibratedCamera ( const Vector & v ) ;
2012-07-16 23:24:27 +08:00
static gtsam : : CalibratedCamera Level ( const gtsam : : Pose2 & pose2 , double height ) ;
2012-02-06 22:05:59 +08:00
2012-06-05 04:17:24 +08:00
// Testable
2012-10-02 22:40:07 +08:00
void print ( string s ) const ;
bool equals ( const gtsam : : CalibratedCamera & camera , double tol ) const ;
2012-02-06 22:05:59 +08:00
2012-06-05 04:17:24 +08:00
// Manifold
static size_t Dim ( ) ;
size_t dim ( ) const ;
gtsam : : CalibratedCamera retract ( const Vector & d ) const ;
Vector localCoordinates ( const gtsam : : CalibratedCamera & T2 ) const ;
2012-02-06 22:05:59 +08:00
2012-06-05 04:17:24 +08:00
// Group
gtsam : : CalibratedCamera compose ( const gtsam : : CalibratedCamera & c ) const ;
gtsam : : CalibratedCamera inverse ( ) const ;
2012-02-06 22:05:59 +08:00
2012-06-05 04:17:24 +08:00
// Action on Point3
gtsam : : Point2 project ( const gtsam : : Point3 & point ) const ;
static gtsam : : Point2 project_to_camera ( const gtsam : : Point3 & cameraPoint ) ;
2012-02-06 22:05:59 +08:00
2012-06-05 04:17:24 +08:00
// Standard Interface
2012-10-02 22:40:07 +08:00
gtsam : : Pose3 pose ( ) const ;
2012-06-05 04:17:24 +08:00
double range ( const gtsam : : Point3 & p ) const ; // TODO: Other overloaded range methods
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2012-06-05 04:17:24 +08:00
} ;
2012-06-03 13:25:50 +08:00
2014-11-07 23:41:43 +08:00
class SimpleCamera {
2012-06-05 13:15:56 +08:00
// Standard Constructors and Named Constructors
2012-10-02 22:40:07 +08:00
SimpleCamera ( ) ;
2012-06-19 07:31:57 +08:00
SimpleCamera ( const gtsam : : Pose3 & pose ) ;
SimpleCamera ( const gtsam : : Pose3 & pose , const gtsam : : Cal3_S2 & K ) ;
2012-07-16 23:24:27 +08:00
static gtsam : : SimpleCamera Level ( const gtsam : : Cal3_S2 & K ,
2012-06-19 07:31:57 +08:00
const gtsam : : Pose2 & pose , double height ) ;
2013-02-16 01:27:38 +08:00
static gtsam : : SimpleCamera Level ( const gtsam : : Pose2 & pose , double height ) ;
2012-07-16 23:24:27 +08:00
static gtsam : : SimpleCamera Lookat ( const gtsam : : Point3 & eye ,
2012-06-19 07:31:57 +08:00
const gtsam : : Point3 & target , const gtsam : : Point3 & upVector ,
const gtsam : : Cal3_S2 & K ) ;
2012-06-05 13:15:56 +08:00
// Testable
2012-10-02 22:40:07 +08:00
void print ( string s ) const ;
bool equals ( const gtsam : : SimpleCamera & camera , double tol ) const ;
2012-06-05 13:15:56 +08:00
// Standard Interface
2012-06-19 07:31:57 +08:00
gtsam : : Pose3 pose ( ) const ;
gtsam : : Cal3_S2 calibration ( ) ;
2012-06-06 17:42:27 +08:00
2012-10-02 22:40:07 +08:00
// Manifold
2012-06-19 07:31:57 +08:00
gtsam : : SimpleCamera retract ( const Vector & d ) const ;
Vector localCoordinates ( const gtsam : : SimpleCamera & T2 ) const ;
size_t dim ( ) const ;
static size_t Dim ( ) ;
// Transformations and measurement functions
static gtsam : : Point2 project_to_camera ( const gtsam : : Point3 & cameraPoint ) ;
pair < gtsam : : Point2 , bool > projectSafe ( const gtsam : : Point3 & pw ) const ;
2012-10-02 22:40:07 +08:00
gtsam : : Point2 project ( const gtsam : : Point3 & point ) ;
gtsam : : Point3 backproject ( const gtsam : : Point2 & p , double depth ) const ;
2012-06-19 07:31:57 +08:00
double range ( const gtsam : : Point3 & point ) ;
2013-02-16 01:27:38 +08:00
double range ( const gtsam : : Pose3 & point ) ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2013-02-16 01:27:38 +08:00
} ;
template < CALIBRATION = { gtsam : : Cal3DS2 } >
2014-11-07 23:41:43 +08:00
class PinholeCamera {
2013-02-16 01:27:38 +08:00
// Standard Constructors and Named Constructors
PinholeCamera ( ) ;
PinholeCamera ( const gtsam : : Pose3 & pose ) ;
PinholeCamera ( const gtsam : : Pose3 & pose , const gtsam : : Cal3DS2 & K ) ;
static This Level ( const gtsam : : Cal3DS2 & K ,
const gtsam : : Pose2 & pose , double height ) ;
static This Level ( const gtsam : : Pose2 & pose , double height ) ;
static This Lookat ( const gtsam : : Point3 & eye ,
const gtsam : : Point3 & target , const gtsam : : Point3 & upVector ,
const gtsam : : Cal3DS2 & K ) ;
// Testable
void print ( string s ) const ;
bool equals ( const This & camera , double tol ) const ;
// Standard Interface
gtsam : : Pose3 pose ( ) const ;
CALIBRATION calibration ( ) const ;
// Manifold
This retract ( const Vector & d ) const ;
Vector localCoordinates ( const This & T2 ) const ;
size_t dim ( ) const ;
static size_t Dim ( ) ;
// Transformations and measurement functions
static gtsam : : Point2 project_to_camera ( const gtsam : : Point3 & cameraPoint ) ;
pair < gtsam : : Point2 , bool > projectSafe ( const gtsam : : Point3 & pw ) const ;
gtsam : : Point2 project ( const gtsam : : Point3 & point ) ;
gtsam : : Point3 backproject ( const gtsam : : Point2 & p , double depth ) const ;
double range ( const gtsam : : Point3 & point ) ;
double range ( const gtsam : : Pose3 & point ) ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2013-02-16 01:27:38 +08:00
} ;
2012-07-24 05:27:42 +08:00
2014-11-07 23:41:43 +08:00
class StereoCamera {
2013-07-23 23:32:38 +08:00
// Standard Constructors and Named Constructors
StereoCamera ( ) ;
StereoCamera ( const gtsam : : Pose3 & pose , const gtsam : : Cal3_S2Stereo * K ) ;
// Testable
void print ( string s ) const ;
bool equals ( const gtsam : : StereoCamera & camera , double tol ) const ;
// Standard Interface
gtsam : : Pose3 pose ( ) const ;
double baseline ( ) const ;
gtsam : : Cal3_S2Stereo * calibration ( ) const ;
// Manifold
gtsam : : StereoCamera retract ( const Vector & d ) const ;
Vector localCoordinates ( const gtsam : : StereoCamera & T2 ) const ;
size_t dim ( ) const ;
static size_t Dim ( ) ;
// Transformations and measurement functions
gtsam : : StereoPoint2 project ( const gtsam : : Point3 & point ) ;
gtsam : : Point3 backproject ( const gtsam : : StereoPoint2 & p ) const ;
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2013-07-23 23:32:38 +08:00
} ;
2012-01-18 04:04:37 +08:00
//*************************************************************************
2013-08-13 02:21:30 +08:00
// Symbolic
2012-01-18 04:04:37 +08:00
//*************************************************************************
2012-06-22 07:53:49 +08:00
2013-08-13 02:21:30 +08:00
# include <gtsam/symbolic/SymbolicFactor.h>
virtual class SymbolicFactor {
2012-06-22 07:53:49 +08:00
// Standard Constructors and Named Constructors
2013-08-13 02:21:30 +08:00
SymbolicFactor ( const gtsam : : SymbolicFactor & f ) ;
SymbolicFactor ( ) ;
SymbolicFactor ( size_t j ) ;
SymbolicFactor ( size_t j1 , size_t j2 ) ;
SymbolicFactor ( size_t j1 , size_t j2 , size_t j3 ) ;
SymbolicFactor ( size_t j1 , size_t j2 , size_t j3 , size_t j4 ) ;
SymbolicFactor ( size_t j1 , size_t j2 , size_t j3 , size_t j4 , size_t j5 ) ;
SymbolicFactor ( size_t j1 , size_t j2 , size_t j3 , size_t j4 , size_t j5 , size_t j6 ) ;
static gtsam : : SymbolicFactor FromKeys ( const gtsam : : KeyVector & js ) ;
2012-06-22 07:53:49 +08:00
// From Factor
size_t size ( ) const ;
void print ( string s ) const ;
2013-08-13 02:21:30 +08:00
bool equals ( const gtsam : : SymbolicFactor & other , double tol ) const ;
gtsam : : KeyVector keys ( ) ;
2012-06-22 07:53:49 +08:00
} ;
2013-08-13 02:21:30 +08:00
# include <gtsam/symbolic/SymbolicFactorGraph.h>
2014-11-07 23:41:43 +08:00
virtual class SymbolicFactorGraph {
2012-06-22 07:53:49 +08:00
SymbolicFactorGraph ( ) ;
SymbolicFactorGraph ( const gtsam : : SymbolicBayesNet & bayesNet ) ;
SymbolicFactorGraph ( const gtsam : : SymbolicBayesTree & bayesTree ) ;
// From FactorGraph
2013-08-13 02:21:30 +08:00
void push_back ( gtsam : : SymbolicFactor * factor ) ;
2012-06-22 07:53:49 +08:00
void print ( string s ) const ;
bool equals ( const gtsam : : SymbolicFactorGraph & rhs , double tol ) const ;
size_t size ( ) const ;
2013-08-24 04:06:08 +08:00
bool exists ( size_t idx ) const ;
2012-06-22 07:53:49 +08:00
// Standard interface
2013-08-13 02:21:30 +08:00
gtsam : : KeySet keys ( ) const ;
void push_back ( gtsam : : SymbolicFactor * factor ) ;
void push_back ( const gtsam : : SymbolicFactorGraph & graph ) ;
void push_back ( const gtsam : : SymbolicBayesNet & bayesNet ) ;
void push_back ( const gtsam : : SymbolicBayesTree & bayesTree ) ;
2012-08-31 03:58:33 +08:00
2012-09-08 07:10:49 +08:00
//Advanced Interface
void push_factor ( size_t key ) ;
void push_factor ( size_t key1 , size_t key2 ) ;
void push_factor ( size_t key1 , size_t key2 , size_t key3 ) ;
void push_factor ( size_t key1 , size_t key2 , size_t key3 , size_t key4 ) ;
2013-08-13 02:21:30 +08:00
gtsam : : SymbolicBayesNet * eliminateSequential ( ) ;
gtsam : : SymbolicBayesNet * eliminateSequential ( const gtsam : : Ordering & ordering ) ;
gtsam : : SymbolicBayesTree * eliminateMultifrontal ( ) ;
gtsam : : SymbolicBayesTree * eliminateMultifrontal ( const gtsam : : Ordering & ordering ) ;
pair < gtsam : : SymbolicBayesNet * , gtsam : : SymbolicFactorGraph * > eliminatePartialSequential (
const gtsam : : Ordering & ordering ) ;
pair < gtsam : : SymbolicBayesNet * , gtsam : : SymbolicFactorGraph * > eliminatePartialSequential (
const gtsam : : KeyVector & keys ) ;
pair < gtsam : : SymbolicBayesTree * , gtsam : : SymbolicFactorGraph * > eliminatePartialMultifrontal (
const gtsam : : Ordering & ordering ) ;
pair < gtsam : : SymbolicBayesTree * , gtsam : : SymbolicFactorGraph * > eliminatePartialMultifrontal (
const gtsam : : KeyVector & keys ) ;
gtsam : : SymbolicBayesNet * marginalMultifrontalBayesNet ( const gtsam : : Ordering & variables ) ;
gtsam : : SymbolicBayesNet * marginalMultifrontalBayesNet ( const gtsam : : KeyVector & variables ) ;
gtsam : : SymbolicBayesNet * marginalMultifrontalBayesNet ( const gtsam : : Ordering & variables ,
const gtsam : : Ordering & marginalizedVariableOrdering ) ;
gtsam : : SymbolicBayesNet * marginalMultifrontalBayesNet ( const gtsam : : KeyVector & variables ,
const gtsam : : Ordering & marginalizedVariableOrdering ) ;
gtsam : : SymbolicFactorGraph * marginal ( const gtsam : : KeyVector & variables ) ;
} ;
# include <gtsam/symbolic/SymbolicConditional.h>
virtual class SymbolicConditional : gtsam : : SymbolicFactor {
2012-06-22 07:53:49 +08:00
// Standard Constructors and Named Constructors
2013-08-13 02:21:30 +08:00
SymbolicConditional ( ) ;
SymbolicConditional ( const gtsam : : SymbolicConditional & other ) ;
SymbolicConditional ( size_t key ) ;
SymbolicConditional ( size_t key , size_t parent ) ;
SymbolicConditional ( size_t key , size_t parent1 , size_t parent2 ) ;
SymbolicConditional ( size_t key , size_t parent1 , size_t parent2 , size_t parent3 ) ;
static gtsam : : SymbolicConditional FromKeys ( const gtsam : : KeyVector & keys , size_t nrFrontals ) ;
2012-06-22 07:53:49 +08:00
// Testable
void print ( string s ) const ;
2013-08-13 02:21:30 +08:00
bool equals ( const gtsam : : SymbolicConditional & other , double tol ) const ;
2012-06-22 07:53:49 +08:00
// Standard interface
2013-08-13 02:21:30 +08:00
size_t nrFrontals ( ) const ;
size_t nrParents ( ) const ;
2012-06-22 07:53:49 +08:00
} ;
2013-08-13 02:21:30 +08:00
# include <gtsam/symbolic/SymbolicBayesNet.h>
class SymbolicBayesNet {
SymbolicBayesNet ( ) ;
SymbolicBayesNet ( const gtsam : : SymbolicBayesNet & other ) ;
2012-06-22 07:53:49 +08:00
// Testable
void print ( string s ) const ;
2013-08-13 02:21:30 +08:00
bool equals ( const gtsam : : SymbolicBayesNet & other , double tol ) const ;
2012-06-22 07:53:49 +08:00
// Standard interface
2013-08-13 02:21:30 +08:00
size_t size ( ) const ;
void saveGraph ( string s ) const ;
2013-12-04 22:36:13 +08:00
gtsam : : SymbolicConditional * at ( size_t idx ) const ;
2013-08-13 02:21:30 +08:00
gtsam : : SymbolicConditional * front ( ) const ;
gtsam : : SymbolicConditional * back ( ) const ;
void push_back ( gtsam : : SymbolicConditional * conditional ) ;
void push_back ( const gtsam : : SymbolicBayesNet & bayesNet ) ;
2012-06-22 07:53:49 +08:00
} ;
2013-08-13 02:21:30 +08:00
# include <gtsam/symbolic/SymbolicBayesTree.h>
class SymbolicBayesTree {
//Constructors
SymbolicBayesTree ( ) ;
2013-10-11 01:52:57 +08:00
SymbolicBayesTree ( const gtsam : : SymbolicBayesTree & other ) ;
2013-08-13 02:21:30 +08:00
// Testable
void print ( string s ) ;
bool equals ( const gtsam : : SymbolicBayesTree & other , double tol ) const ;
//Standard Interface
//size_t findParentClique(const gtsam::IndexVector& parents) const;
size_t size ( ) ;
void saveGraph ( string s ) const ;
void clear ( ) ;
void deleteCachedShortcuts ( ) ;
size_t numCachedSeparatorMarginals ( ) const ;
2013-10-11 01:52:57 +08:00
gtsam : : SymbolicConditional * marginalFactor ( size_t key ) const ;
gtsam : : SymbolicFactorGraph * joint ( size_t key1 , size_t key2 ) const ;
gtsam : : SymbolicBayesNet * jointBayesNet ( size_t key1 , size_t key2 ) const ;
2013-08-13 02:21:30 +08:00
} ;
// class SymbolicBayesTreeClique {
// BayesTreeClique();
// BayesTreeClique(CONDITIONAL* conditional);
// // BayesTreeClique(const std::pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
//
// bool equals(const This& other, double tol) const;
// void print(string s) const;
// void printTree() const; // Default indent of ""
// void printTree(string indent) const;
// size_t numCachedSeparatorMarginals() const;
//
// CONDITIONAL* conditional() const;
// bool isRoot() const;
// size_t treeSize() const;
// // const std::list<derived_ptr>& children() const { return children_; }
// // derived_ptr parent() const { return parent_.lock(); }
//
// // FIXME: need wrapped versions graphs, BayesNet
// // BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;
// // FactorGraph<FactorType> marginal(derived_ptr root, Eliminate function) const;
// // FactorGraph<FactorType> joint(derived_ptr C2, derived_ptr root, Eliminate function) const;
2013-12-04 22:36:13 +08:00
//
2013-08-13 02:21:30 +08:00
// void deleteCachedShortcuts();
// };
# include <gtsam/inference/VariableIndex.h>
2012-06-22 07:53:49 +08:00
class VariableIndex {
// Standard Constructors and Named Constructors
VariableIndex ( ) ;
2012-09-10 22:39:36 +08:00
// TODO: Templetize constructor when wrap supports it
2012-09-08 07:10:49 +08:00
//template<T = {gtsam::FactorGraph}>
//VariableIndex(const T& factorGraph, size_t nVariables);
//VariableIndex(const T& factorGraph);
2012-06-22 07:53:49 +08:00
VariableIndex ( const gtsam : : SymbolicFactorGraph & factorGraph ) ;
2012-09-08 07:10:49 +08:00
VariableIndex ( const gtsam : : GaussianFactorGraph & factorGraph ) ;
VariableIndex ( const gtsam : : NonlinearFactorGraph & factorGraph ) ;
2012-06-22 07:53:49 +08:00
VariableIndex ( const gtsam : : VariableIndex & other ) ;
// Testable
bool equals ( const gtsam : : VariableIndex & other , double tol ) const ;
void print ( string s ) const ;
// Standard interface
size_t size ( ) const ;
size_t nFactors ( ) const ;
size_t nEntries ( ) const ;
} ;
2012-06-03 13:25:50 +08:00
2012-01-18 04:04:37 +08:00
//*************************************************************************
// linear
//*************************************************************************
2012-06-19 14:08:55 +08:00
namespace noiseModel {
2012-07-17 23:51:35 +08:00
# include <gtsam/linear/NoiseModel.h>
2012-07-11 06:38:21 +08:00
virtual class Base {
2012-06-23 03:36:49 +08:00
} ;
2012-07-11 06:38:21 +08:00
virtual class Gaussian : gtsam : : noiseModel : : Base {
2012-10-02 22:40:07 +08:00
static gtsam : : noiseModel : : Gaussian * SqrtInformation ( Matrix R ) ;
static gtsam : : noiseModel : : Gaussian * Covariance ( Matrix R ) ;
2013-06-14 05:17:21 +08:00
Matrix R ( ) const ;
2012-10-02 22:40:07 +08:00
bool equals ( gtsam : : noiseModel : : Base & expected , double tol ) ;
void print ( string s ) const ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
void serializable ( ) const ;
2011-10-21 13:07:35 +08:00
} ;
2012-07-11 06:38:21 +08:00
virtual class Diagonal : gtsam : : noiseModel : : Gaussian {
2012-10-02 22:40:07 +08:00
static gtsam : : noiseModel : : Diagonal * Sigmas ( Vector sigmas ) ;
static gtsam : : noiseModel : : Diagonal * Variances ( Vector variances ) ;
static gtsam : : noiseModel : : Diagonal * Precisions ( Vector precisions ) ;
2013-06-14 05:17:21 +08:00
Matrix R ( ) const ;
2012-10-02 22:40:07 +08:00
void print ( string s ) const ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
void serializable ( ) const ;
2012-06-19 14:08:55 +08:00
} ;
2012-09-08 07:10:49 +08:00
virtual class Constrained : gtsam : : noiseModel : : Diagonal {
static gtsam : : noiseModel : : Constrained * MixedSigmas ( const Vector & mu , const Vector & sigmas ) ;
static gtsam : : noiseModel : : Constrained * MixedSigmas ( double m , const Vector & sigmas ) ;
static gtsam : : noiseModel : : Constrained * MixedVariances ( const Vector & mu , const Vector & variances ) ;
static gtsam : : noiseModel : : Constrained * MixedVariances ( const Vector & variances ) ;
static gtsam : : noiseModel : : Constrained * MixedPrecisions ( const Vector & mu , const Vector & precisions ) ;
static gtsam : : noiseModel : : Constrained * MixedPrecisions ( const Vector & precisions ) ;
static gtsam : : noiseModel : : Constrained * All ( size_t dim ) ;
2013-04-11 03:35:48 +08:00
static gtsam : : noiseModel : : Constrained * All ( size_t dim , double mu ) ;
2012-09-08 07:10:49 +08:00
gtsam : : noiseModel : : Constrained * unit ( ) const ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
void serializable ( ) const ;
2012-09-08 07:10:49 +08:00
} ;
2012-07-12 02:18:57 +08:00
virtual class Isotropic : gtsam : : noiseModel : : Diagonal {
2012-10-02 22:40:07 +08:00
static gtsam : : noiseModel : : Isotropic * Sigma ( size_t dim , double sigma ) ;
static gtsam : : noiseModel : : Isotropic * Variance ( size_t dim , double varianace ) ;
static gtsam : : noiseModel : : Isotropic * Precision ( size_t dim , double precision ) ;
void print ( string s ) const ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
void serializable ( ) const ;
2011-10-21 13:07:35 +08:00
} ;
2012-07-12 02:18:57 +08:00
virtual class Unit : gtsam : : noiseModel : : Isotropic {
2012-10-02 22:40:07 +08:00
static gtsam : : noiseModel : : Unit * Create ( size_t dim ) ;
void print ( string s ) const ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
void serializable ( ) const ;
2012-06-19 14:08:55 +08:00
} ;
2013-02-08 03:19:33 +08:00
namespace mEstimator {
virtual class Base {
} ;
virtual class Null : gtsam : : noiseModel : : mEstimator : : Base {
Null ( ) ;
void print ( string s ) const ;
static gtsam : : noiseModel : : mEstimator : : Null * Create ( ) ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
void serializable ( ) const ;
2013-02-08 03:19:33 +08:00
} ;
virtual class Fair : gtsam : : noiseModel : : mEstimator : : Base {
Fair ( double c ) ;
void print ( string s ) const ;
static gtsam : : noiseModel : : mEstimator : : Fair * Create ( double c ) ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
void serializable ( ) const ;
2013-02-08 03:19:33 +08:00
} ;
virtual class Huber : gtsam : : noiseModel : : mEstimator : : Base {
Huber ( double k ) ;
void print ( string s ) const ;
static gtsam : : noiseModel : : mEstimator : : Huber * Create ( double k ) ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
void serializable ( ) const ;
2013-02-08 03:19:33 +08:00
} ;
virtual class Tukey : gtsam : : noiseModel : : mEstimator : : Base {
Tukey ( double k ) ;
void print ( string s ) const ;
static gtsam : : noiseModel : : mEstimator : : Tukey * Create ( double k ) ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
void serializable ( ) const ;
2013-02-08 03:19:33 +08:00
} ;
} ///\namespace mEstimator
virtual class Robust : gtsam : : noiseModel : : Base {
Robust ( const gtsam : : noiseModel : : mEstimator : : Base * robust , const gtsam : : noiseModel : : Base * noise ) ;
static gtsam : : noiseModel : : Robust * Create ( const gtsam : : noiseModel : : mEstimator : : Base * robust , const gtsam : : noiseModel : : Base * noise ) ;
void print ( string s ) const ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
void serializable ( ) const ;
2013-02-08 03:19:33 +08:00
} ;
2012-06-19 14:08:55 +08:00
} ///\namespace noiseModel
2012-07-18 02:30:02 +08:00
# include <gtsam/linear/Sampler.h>
2012-06-23 00:38:01 +08:00
class Sampler {
2012-09-08 07:10:49 +08:00
//Constructors
2012-10-02 22:40:07 +08:00
Sampler ( gtsam : : noiseModel : : Diagonal * model , int seed ) ;
Sampler ( Vector sigmas , int seed ) ;
Sampler ( int seed ) ;
2012-06-23 00:38:01 +08:00
2012-09-08 07:10:49 +08:00
//Standard Interface
2012-10-02 22:40:07 +08:00
size_t dim ( ) const ;
Vector sigmas ( ) const ;
gtsam : : noiseModel : : Diagonal * model ( ) const ;
Vector sample ( ) ;
Vector sampleNewModel ( gtsam : : noiseModel : : Diagonal * model ) ;
2012-06-19 14:08:55 +08:00
} ;
2013-08-13 02:21:30 +08:00
# include <gtsam/linear/VectorValues.h>
2011-10-21 13:07:35 +08:00
class VectorValues {
2012-09-08 07:10:49 +08:00
//Constructors
2012-10-02 22:40:07 +08:00
VectorValues ( ) ;
VectorValues ( const gtsam : : VectorValues & other ) ;
//Named Constructors
static gtsam : : VectorValues Zero ( const gtsam : : VectorValues & model ) ;
//Standard Interface
size_t size ( ) const ;
size_t dim ( size_t j ) const ;
bool exists ( size_t j ) const ;
void print ( string s ) const ;
bool equals ( const gtsam : : VectorValues & expected , double tol ) const ;
void insert ( size_t j , Vector value ) ;
2013-08-13 02:21:30 +08:00
Vector vector ( ) const ;
2012-10-02 22:40:07 +08:00
Vector at ( size_t j ) const ;
2013-08-13 02:21:30 +08:00
void update ( const gtsam : : VectorValues & values ) ;
2012-10-02 22:40:07 +08:00
//Advanced Interface
void setZero ( ) ;
2012-09-08 07:10:49 +08:00
2013-02-24 04:31:42 +08:00
gtsam : : VectorValues add ( const gtsam : : VectorValues & c ) const ;
2013-08-13 02:21:30 +08:00
void addInPlace ( const gtsam : : VectorValues & c ) ;
gtsam : : VectorValues subtract ( const gtsam : : VectorValues & c ) const ;
gtsam : : VectorValues scale ( double a ) const ;
void scaleInPlace ( double a ) ;
2011-10-21 13:07:35 +08:00
2013-08-13 02:21:30 +08:00
bool hasSameStructure ( const gtsam : : VectorValues & other ) const ;
double dot ( const gtsam : : VectorValues & V ) const ;
double norm ( ) const ;
double squaredNorm ( ) const ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2011-10-23 03:42:02 +08:00
} ;
2013-08-13 02:21:30 +08:00
# include <gtsam/linear/GaussianFactor.h>
2012-07-12 02:18:57 +08:00
virtual class GaussianFactor {
2012-10-02 22:40:07 +08:00
void print ( string s ) const ;
bool equals ( const gtsam : : GaussianFactor & lf , double tol ) const ;
double error ( const gtsam : : VectorValues & c ) const ;
2013-08-13 02:21:30 +08:00
gtsam : : GaussianFactor * clone ( ) const ;
2012-10-02 22:40:07 +08:00
gtsam : : GaussianFactor * negate ( ) const ;
2012-10-09 06:40:37 +08:00
Matrix augmentedInformation ( ) const ;
Matrix information ( ) const ;
2013-08-13 02:21:30 +08:00
Matrix augmentedJacobian ( ) const ;
pair < Matrix , Vector > jacobian ( ) const ;
2012-10-02 22:40:07 +08:00
size_t size ( ) const ;
2013-08-13 02:21:30 +08:00
bool empty ( ) const ;
2011-10-23 03:42:02 +08:00
} ;
2011-10-21 13:07:35 +08:00
2013-08-13 02:21:30 +08:00
# include <gtsam/linear/JacobianFactor.h>
2012-07-12 02:18:57 +08:00
virtual class JacobianFactor : gtsam : : GaussianFactor {
2013-10-09 05:43:14 +08:00
//Constructors
2012-10-02 22:40:07 +08:00
JacobianFactor ( ) ;
2013-08-13 02:21:30 +08:00
JacobianFactor ( const gtsam : : GaussianFactor & factor ) ;
2012-10-02 22:40:07 +08:00
JacobianFactor ( Vector b_in ) ;
JacobianFactor ( size_t i1 , Matrix A1 , Vector b ,
const gtsam : : noiseModel : : Diagonal * model ) ;
JacobianFactor ( size_t i1 , Matrix A1 , size_t i2 , Matrix A2 , Vector b ,
const gtsam : : noiseModel : : Diagonal * model ) ;
JacobianFactor ( size_t i1 , Matrix A1 , size_t i2 , Matrix A2 , size_t i3 , Matrix A3 ,
Vector b , const gtsam : : noiseModel : : Diagonal * model ) ;
2013-10-09 05:43:14 +08:00
JacobianFactor ( const gtsam : : GaussianFactorGraph & graph ) ;
2012-10-02 22:40:07 +08:00
//Testable
void print ( string s ) const ;
void printKeys ( string s ) const ;
bool equals ( const gtsam : : GaussianFactor & lf , double tol ) const ;
size_t size ( ) const ;
Vector unweighted_error ( const gtsam : : VectorValues & c ) const ;
Vector error_vector ( const gtsam : : VectorValues & c ) const ;
double error ( const gtsam : : VectorValues & c ) const ;
//Standard Interface
2012-10-09 06:40:37 +08:00
Matrix getA ( ) const ;
Vector getb ( ) const ;
size_t rows ( ) const ;
size_t cols ( ) const ;
bool isConstrained ( ) const ;
2013-08-13 02:21:30 +08:00
pair < Matrix , Vector > jacobianUnweighted ( ) const ;
Matrix augmentedJacobianUnweighted ( ) const ;
2012-10-09 06:40:37 +08:00
void transposeMultiplyAdd ( double alpha , const Vector & e , gtsam : : VectorValues & x ) const ;
gtsam : : JacobianFactor whiten ( ) const ;
2013-08-13 02:21:30 +08:00
pair < gtsam : : GaussianConditional * , gtsam : : JacobianFactor * > eliminate ( const gtsam : : Ordering & keys ) const ;
2012-10-09 06:40:37 +08:00
void setModel ( bool anyConstrained , const Vector & sigmas ) ;
2013-08-13 02:21:30 +08:00
gtsam : : noiseModel : : Diagonal * get_model ( ) const ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2011-10-23 03:42:02 +08:00
} ;
2011-10-21 13:07:35 +08:00
2013-08-13 02:21:30 +08:00
# include <gtsam/linear/HessianFactor.h>
2012-07-12 02:18:57 +08:00
virtual class HessianFactor : gtsam : : GaussianFactor {
2013-10-09 05:43:14 +08:00
//Constructors
2012-10-02 22:40:07 +08:00
HessianFactor ( ) ;
2013-08-13 02:21:30 +08:00
HessianFactor ( const gtsam : : GaussianFactor & factor ) ;
2012-10-02 22:40:07 +08:00
HessianFactor ( size_t j , Matrix G , Vector g , double f ) ;
HessianFactor ( size_t j , Vector mu , Matrix Sigma ) ;
HessianFactor ( size_t j1 , size_t j2 , Matrix G11 , Matrix G12 , Vector g1 , Matrix G22 ,
Vector g2 , double f ) ;
HessianFactor ( size_t j1 , size_t j2 , size_t j3 , Matrix G11 , Matrix G12 , Matrix G13 ,
Vector g1 , Matrix G22 , Matrix G23 , Vector g2 , Matrix G33 , Vector g3 ,
double f ) ;
2013-10-09 05:43:14 +08:00
HessianFactor ( const gtsam : : GaussianFactorGraph & factors ) ;
2012-10-02 22:40:07 +08:00
//Testable
size_t size ( ) const ;
void print ( string s ) const ;
void printKeys ( string s ) const ;
bool equals ( const gtsam : : GaussianFactor & lf , double tol ) const ;
double error ( const gtsam : : VectorValues & c ) const ;
//Standard Interface
size_t rows ( ) const ;
Matrix info ( ) const ;
double constantTerm ( ) const ;
Vector linearTerm ( ) const ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2012-01-20 12:35:12 +08:00
} ;
2013-08-13 02:21:30 +08:00
# include <gtsam/linear/GaussianFactorGraph.h>
2011-10-23 03:42:02 +08:00
class GaussianFactorGraph {
2012-10-02 22:40:07 +08:00
GaussianFactorGraph ( ) ;
2013-08-13 02:21:30 +08:00
GaussianFactorGraph ( const gtsam : : GaussianBayesNet & bayesNet ) ;
GaussianFactorGraph ( const gtsam : : GaussianBayesTree & bayesTree ) ;
2012-10-02 22:40:07 +08:00
// From FactorGraph
void print ( string s ) const ;
bool equals ( const gtsam : : GaussianFactorGraph & lfgraph , double tol ) const ;
size_t size ( ) const ;
gtsam : : GaussianFactor * at ( size_t idx ) const ;
2013-08-13 02:21:30 +08:00
gtsam : : KeySet keys ( ) const ;
2013-08-24 04:06:08 +08:00
bool exists ( size_t idx ) const ;
2012-10-02 22:40:07 +08:00
// Building the graph
2013-10-31 05:34:52 +08:00
void push_back ( const gtsam : : GaussianFactor * factor ) ;
void push_back ( const gtsam : : GaussianConditional * factor ) ;
2013-08-13 02:21:30 +08:00
void push_back ( const gtsam : : GaussianFactorGraph & graph ) ;
void push_back ( const gtsam : : GaussianBayesNet & bayesNet ) ;
void push_back ( const gtsam : : GaussianBayesTree & bayesTree ) ;
2012-10-02 22:40:07 +08:00
void add ( const gtsam : : GaussianFactor & factor ) ;
void add ( Vector b ) ;
void add ( size_t key1 , Matrix A1 , Vector b , const gtsam : : noiseModel : : Diagonal * model ) ;
void add ( size_t key1 , Matrix A1 , size_t key2 , Matrix A2 , Vector b ,
const gtsam : : noiseModel : : Diagonal * model ) ;
void add ( size_t key1 , Matrix A1 , size_t key2 , Matrix A2 , size_t key3 , Matrix A3 ,
Vector b , const gtsam : : noiseModel : : Diagonal * model ) ;
2012-01-20 12:35:12 +08:00
2012-10-02 22:40:07 +08:00
// error and probability
double error ( const gtsam : : VectorValues & c ) const ;
double probPrime ( const gtsam : : VectorValues & c ) const ;
2012-01-20 12:35:12 +08:00
2013-11-07 23:29:38 +08:00
gtsam : : GaussianFactorGraph clone ( ) const ;
gtsam : : GaussianFactorGraph negate ( ) const ;
2013-08-13 02:21:30 +08:00
// Optimizing and linear algebra
gtsam : : VectorValues optimize ( ) const ;
gtsam : : VectorValues optimize ( const gtsam : : Ordering & ordering ) const ;
gtsam : : VectorValues optimizeGradientSearch ( ) const ;
gtsam : : VectorValues gradient ( const gtsam : : VectorValues & x0 ) const ;
gtsam : : VectorValues gradientAtZero ( ) const ;
2012-01-20 12:35:12 +08:00
2013-09-19 01:23:33 +08:00
// Elimination and marginals
gtsam : : GaussianBayesNet * eliminateSequential ( ) ;
gtsam : : GaussianBayesNet * eliminateSequential ( const gtsam : : Ordering & ordering ) ;
gtsam : : GaussianBayesTree * eliminateMultifrontal ( ) ;
gtsam : : GaussianBayesTree * eliminateMultifrontal ( const gtsam : : Ordering & ordering ) ;
pair < gtsam : : GaussianBayesNet * , gtsam : : GaussianFactorGraph * > eliminatePartialSequential (
const gtsam : : Ordering & ordering ) ;
pair < gtsam : : GaussianBayesNet * , gtsam : : GaussianFactorGraph * > eliminatePartialSequential (
const gtsam : : KeyVector & keys ) ;
pair < gtsam : : GaussianBayesTree * , gtsam : : GaussianFactorGraph * > eliminatePartialMultifrontal (
const gtsam : : Ordering & ordering ) ;
pair < gtsam : : GaussianBayesTree * , gtsam : : GaussianFactorGraph * > eliminatePartialMultifrontal (
const gtsam : : KeyVector & keys ) ;
gtsam : : GaussianBayesNet * marginalMultifrontalBayesNet ( const gtsam : : Ordering & variables ) ;
gtsam : : GaussianBayesNet * marginalMultifrontalBayesNet ( const gtsam : : KeyVector & variables ) ;
gtsam : : GaussianBayesNet * marginalMultifrontalBayesNet ( const gtsam : : Ordering & variables ,
const gtsam : : Ordering & marginalizedVariableOrdering ) ;
gtsam : : GaussianBayesNet * marginalMultifrontalBayesNet ( const gtsam : : KeyVector & variables ,
const gtsam : : Ordering & marginalizedVariableOrdering ) ;
gtsam : : GaussianFactorGraph * marginal ( const gtsam : : KeyVector & variables ) ;
2012-10-02 22:40:07 +08:00
// Conversion to matrices
Matrix sparseJacobian_ ( ) const ;
Matrix augmentedJacobian ( ) const ;
2013-10-30 23:54:03 +08:00
Matrix augmentedJacobian ( const gtsam : : Ordering & ordering ) const ;
2012-10-02 22:40:07 +08:00
pair < Matrix , Vector > jacobian ( ) const ;
2013-10-30 23:54:03 +08:00
pair < Matrix , Vector > jacobian ( const gtsam : : Ordering & ordering ) const ;
2012-10-02 22:40:07 +08:00
Matrix augmentedHessian ( ) const ;
2013-10-30 23:54:03 +08:00
Matrix augmentedHessian ( const gtsam : : Ordering & ordering ) const ;
2012-10-02 22:40:07 +08:00
pair < Matrix , Vector > hessian ( ) const ;
2013-10-30 23:54:03 +08:00
pair < Matrix , Vector > hessian ( const gtsam : : Ordering & ordering ) const ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2011-10-23 03:42:02 +08:00
} ;
2011-10-14 02:41:56 +08:00
2013-08-13 02:21:30 +08:00
# include <gtsam/linear/GaussianConditional.h>
virtual class GaussianConditional : gtsam : : GaussianFactor {
2013-12-04 22:36:13 +08:00
//Constructors
2013-08-13 02:21:30 +08:00
GaussianConditional ( size_t key , Vector d , Matrix R , const gtsam : : noiseModel : : Diagonal * sigmas ) ;
GaussianConditional ( size_t key , Vector d , Matrix R , size_t name1 , Matrix S ,
const gtsam : : noiseModel : : Diagonal * sigmas ) ;
GaussianConditional ( size_t key , Vector d , Matrix R , size_t name1 , Matrix S ,
size_t name2 , Matrix T , const gtsam : : noiseModel : : Diagonal * sigmas ) ;
2012-09-08 07:10:49 +08:00
2013-12-04 22:36:13 +08:00
//Constructors with no noise model
GaussianConditional ( size_t key , Vector d , Matrix R ) ;
GaussianConditional ( size_t key , Vector d , Matrix R , size_t name1 , Matrix S ) ;
GaussianConditional ( size_t key , Vector d , Matrix R , size_t name1 , Matrix S ,
size_t name2 , Matrix T ) ;
2013-08-13 02:21:30 +08:00
//Standard Interface
void print ( string s ) const ;
bool equals ( const gtsam : : GaussianConditional & cg , double tol ) const ;
//Advanced Interface
gtsam : : VectorValues solve ( const gtsam : : VectorValues & parents ) const ;
gtsam : : VectorValues solveOtherRHS ( const gtsam : : VectorValues & parents , const gtsam : : VectorValues & rhs ) const ;
void solveTransposeInPlace ( gtsam : : VectorValues & gy ) const ;
void scaleFrontalsBySigma ( gtsam : : VectorValues & gy ) const ;
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2013-08-13 02:21:30 +08:00
} ;
# include <gtsam/linear/GaussianDensity.h>
virtual class GaussianDensity : gtsam : : GaussianConditional {
2012-09-08 07:10:49 +08:00
//Constructors
2013-08-13 02:21:30 +08:00
GaussianDensity ( size_t key , Vector d , Matrix R , const gtsam : : noiseModel : : Diagonal * sigmas ) ;
2012-09-08 07:10:49 +08:00
2013-08-13 02:21:30 +08:00
//Standard Interface
void print ( string s ) const ;
bool equals ( const gtsam : : GaussianDensity & cg , double tol ) const ;
Vector mean ( ) const ;
Matrix covariance ( ) const ;
2012-09-08 07:10:49 +08:00
} ;
2013-08-13 02:21:30 +08:00
# include <gtsam/linear/GaussianBayesNet.h>
virtual class GaussianBayesNet {
//Constructors
GaussianBayesNet ( ) ;
GaussianBayesNet ( const gtsam : : GaussianConditional * conditional ) ;
// Testable
void print ( string s ) const ;
bool equals ( const gtsam : : GaussianBayesNet & other , double tol ) const ;
2013-10-31 05:34:52 +08:00
size_t size ( ) const ;
2012-09-08 07:10:49 +08:00
2013-10-31 05:34:52 +08:00
// FactorGraph derived interface
2013-08-13 02:21:30 +08:00
size_t size ( ) const ;
2013-10-31 05:34:52 +08:00
gtsam : : GaussianConditional * at ( size_t idx ) const ;
gtsam : : KeySet keys ( ) const ;
bool exists ( size_t idx ) const ;
2013-08-13 02:21:30 +08:00
gtsam : : GaussianConditional * front ( ) const ;
gtsam : : GaussianConditional * back ( ) const ;
void push_back ( gtsam : : GaussianConditional * conditional ) ;
void push_back ( const gtsam : : GaussianBayesNet & bayesNet ) ;
gtsam : : VectorValues optimize ( ) const ;
2014-05-06 14:18:52 +08:00
gtsam : : VectorValues optimize ( gtsam : : VectorValues & solutionForMissing ) const ;
2013-08-13 02:21:30 +08:00
gtsam : : VectorValues optimizeGradientSearch ( ) const ;
gtsam : : VectorValues gradient ( const gtsam : : VectorValues & x0 ) const ;
gtsam : : VectorValues gradientAtZero ( ) const ;
double error ( const gtsam : : VectorValues & x ) const ;
double determinant ( ) const ;
double logDeterminant ( ) const ;
gtsam : : VectorValues backSubstitute ( const gtsam : : VectorValues & gx ) const ;
gtsam : : VectorValues backSubstituteTranspose ( const gtsam : : VectorValues & gx ) const ;
} ;
2012-09-08 07:10:49 +08:00
2013-08-13 02:21:30 +08:00
# include <gtsam/linear/GaussianBayesTree.h>
virtual class GaussianBayesTree {
// Standard Constructors and Named Constructors
GaussianBayesTree ( ) ;
GaussianBayesTree ( const gtsam : : GaussianBayesTree & other ) ;
bool equals ( const gtsam : : GaussianBayesTree & other , double tol ) const ;
void print ( string s ) ;
size_t size ( ) const ;
bool empty ( ) const ;
size_t numCachedSeparatorMarginals ( ) const ;
2012-06-07 21:17:46 +08:00
void saveGraph ( string s ) const ;
2013-08-13 02:21:30 +08:00
gtsam : : VectorValues optimize ( ) const ;
gtsam : : VectorValues optimizeGradientSearch ( ) const ;
gtsam : : VectorValues gradient ( const gtsam : : VectorValues & x0 ) const ;
gtsam : : VectorValues gradientAtZero ( ) const ;
double error ( const gtsam : : VectorValues & x ) const ;
double determinant ( ) const ;
double logDeterminant ( ) const ;
2012-06-07 13:19:43 +08:00
Matrix marginalCovariance ( size_t key ) const ;
2013-08-13 02:21:30 +08:00
gtsam : : GaussianConditional * marginalFactor ( size_t key ) const ;
gtsam : : GaussianFactorGraph * joint ( size_t key1 , size_t key2 ) const ;
2012-06-07 13:19:43 +08:00
gtsam : : GaussianBayesNet * jointBayesNet ( size_t key1 , size_t key2 ) const ;
} ;
2013-08-13 02:21:30 +08:00
class Errors {
2012-09-08 07:10:49 +08:00
//Constructors
2013-08-13 02:21:30 +08:00
Errors ( ) ;
Errors ( const gtsam : : VectorValues & V ) ;
2012-09-08 07:10:49 +08:00
2013-08-13 02:21:30 +08:00
//Testable
void print ( string s ) ;
bool equals ( const gtsam : : Errors & expected , double tol ) const ;
2011-12-10 00:36:50 +08:00
} ;
2013-08-13 02:21:30 +08:00
class GaussianISAM {
//Constructor
GaussianISAM ( ) ;
2012-10-28 03:45:30 +08:00
//Standard Interface
2013-08-13 02:21:30 +08:00
void update ( const gtsam : : GaussianFactorGraph & newFactors ) ;
void saveGraph ( string s ) const ;
void clear ( ) ;
2012-10-28 03:45:30 +08:00
} ;
2012-09-04 05:50:22 +08:00
# include <gtsam/linear/IterativeSolver.h>
virtual class IterativeOptimizationParameters {
string getVerbosity ( ) const ;
void setVerbosity ( string s ) ;
2012-09-08 07:10:49 +08:00
void print ( ) const ;
2012-09-04 05:50:22 +08:00
} ;
//virtual class IterativeSolver {
// IterativeSolver();
// gtsam::VectorValues optimize ();
//};
# include <gtsam/linear/ConjugateGradientSolver.h>
virtual class ConjugateGradientParameters : gtsam : : IterativeOptimizationParameters {
ConjugateGradientParameters ( ) ;
2013-11-19 22:04:52 +08:00
int getMinIterations ( ) const ;
int getMaxIterations ( ) const ;
int getReset ( ) const ;
2012-09-04 05:50:22 +08:00
double getEpsilon_rel ( ) const ;
double getEpsilon_abs ( ) const ;
2013-11-19 22:04:52 +08:00
void setMinIterations ( int value ) ;
void setMaxIterations ( int value ) ;
void setReset ( int value ) ;
2012-09-04 05:50:22 +08:00
void setEpsilon_rel ( double value ) ;
void setEpsilon_abs ( double value ) ;
2013-01-03 10:50:55 +08:00
void print ( ) ;
2012-09-04 05:50:22 +08:00
} ;
# include <gtsam/linear/SubgraphSolver.h>
virtual class SubgraphSolverParameters : gtsam : : ConjugateGradientParameters {
SubgraphSolverParameters ( ) ;
2013-01-03 10:50:55 +08:00
void print ( ) const ;
2012-09-04 05:50:22 +08:00
} ;
2014-06-18 01:47:43 +08:00
virtual class SubgraphSolver {
2013-08-13 02:21:30 +08:00
SubgraphSolver ( const gtsam : : GaussianFactorGraph & A , const gtsam : : SubgraphSolverParameters & parameters , const gtsam : : Ordering & ordering ) ;
SubgraphSolver ( const gtsam : : GaussianFactorGraph & Ab1 , const gtsam : : GaussianFactorGraph & Ab2 , const gtsam : : SubgraphSolverParameters & parameters , const gtsam : : Ordering & ordering ) ;
2012-09-04 05:50:22 +08:00
gtsam : : VectorValues optimize ( ) const ;
} ;
2012-07-18 02:30:02 +08:00
# include <gtsam/linear/KalmanFilter.h>
2011-11-04 08:27:02 +08:00
class KalmanFilter {
2012-10-02 22:40:07 +08:00
KalmanFilter ( size_t n ) ;
// gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0);
gtsam : : GaussianDensity * init ( Vector x0 , Matrix P0 ) ;
void print ( string s ) const ;
2013-07-05 23:46:07 +08:00
static size_t step ( gtsam : : GaussianDensity * p ) ;
2012-10-02 22:40:07 +08:00
gtsam : : GaussianDensity * predict ( gtsam : : GaussianDensity * p , Matrix F ,
Matrix B , Vector u , const gtsam : : noiseModel : : Diagonal * modelQ ) ;
gtsam : : GaussianDensity * predictQ ( gtsam : : GaussianDensity * p , Matrix F ,
Matrix B , Vector u , Matrix Q ) ;
gtsam : : GaussianDensity * predict2 ( gtsam : : GaussianDensity * p , Matrix A0 ,
Matrix A1 , Vector b , const gtsam : : noiseModel : : Diagonal * model ) ;
gtsam : : GaussianDensity * update ( gtsam : : GaussianDensity * p , Matrix H ,
Vector z , const gtsam : : noiseModel : : Diagonal * model ) ;
gtsam : : GaussianDensity * updateQ ( gtsam : : GaussianDensity * p , Matrix H ,
Vector z , Matrix Q ) ;
2011-11-04 08:27:02 +08:00
} ;
2012-01-18 04:04:37 +08:00
//*************************************************************************
// nonlinear
//*************************************************************************
2013-08-19 23:32:16 +08:00
# include <gtsam/inference/Symbol.h>
2012-07-28 03:02:11 +08:00
size_t symbol ( char chr , size_t index ) ;
char symbolChr ( size_t key ) ;
size_t symbolIndex ( size_t key ) ;
2012-05-23 03:01:40 +08:00
2013-05-01 02:41:06 +08:00
// Default keyformatter
2014-05-07 01:21:49 +08:00
void printKeyList ( const gtsam : : KeyList & keys ) ;
void printKeyList ( const gtsam : : KeyList & keys , string s ) ;
void printKeyVector ( const gtsam : : KeyVector & keys ) ;
void printKeyVector ( const gtsam : : KeyVector & keys , string s ) ;
void printKeySet ( const gtsam : : KeySet & keys ) ;
void printKeySet ( const gtsam : : KeySet & keys , string s ) ;
2013-05-01 02:41:06 +08:00
2013-08-19 23:32:16 +08:00
# include <gtsam/inference/LabeledSymbol.h>
2013-05-01 02:41:06 +08:00
class LabeledSymbol {
LabeledSymbol ( size_t full_key ) ;
LabeledSymbol ( const gtsam : : LabeledSymbol & key ) ;
LabeledSymbol ( unsigned char valType , unsigned char label , size_t j ) ;
size_t key ( ) const ;
unsigned char label ( ) const ;
unsigned char chr ( ) const ;
size_t index ( ) const ;
gtsam : : LabeledSymbol upper ( ) const ;
gtsam : : LabeledSymbol lower ( ) const ;
2013-11-24 03:11:43 +08:00
gtsam : : LabeledSymbol newChr ( unsigned char c ) const ;
gtsam : : LabeledSymbol newLabel ( unsigned char label ) const ;
2013-05-01 02:41:06 +08:00
void print ( string s ) const ;
} ;
2013-05-04 00:21:47 +08:00
size_t mrsymbol ( unsigned char c , unsigned char label , size_t j ) ;
unsigned char mrsymbolChr ( size_t key ) ;
unsigned char mrsymbolLabel ( size_t key ) ;
size_t mrsymbolIndex ( size_t key ) ;
2013-08-13 02:21:30 +08:00
# include <gtsam/inference/Ordering.h>
2011-11-04 12:27:55 +08:00
class Ordering {
2012-06-22 07:53:49 +08:00
// Standard Constructors and Named Constructors
Ordering ( ) ;
2013-08-13 02:21:30 +08:00
Ordering ( const gtsam : : Ordering & other ) ;
2012-06-22 07:53:49 +08:00
2012-10-02 22:40:07 +08:00
// Testable
void print ( string s ) const ;
bool equals ( const gtsam : : Ordering & ord , double tol ) const ;
2012-06-22 07:53:49 +08:00
2012-10-02 22:40:07 +08:00
// Standard interface
2012-06-22 07:53:49 +08:00
size_t size ( ) const ;
size_t at ( size_t key ) const ;
void push_back ( size_t key ) ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2011-10-22 00:56:50 +08:00
} ;
2012-05-22 06:32:40 +08:00
class NonlinearFactorGraph {
2012-10-02 22:40:07 +08:00
NonlinearFactorGraph ( ) ;
NonlinearFactorGraph ( const gtsam : : NonlinearFactorGraph & graph ) ;
// FactorGraph
void print ( string s ) const ;
bool equals ( const gtsam : : NonlinearFactorGraph & fg , double tol ) const ;
size_t size ( ) const ;
bool empty ( ) const ;
void remove ( size_t i ) ;
size_t nrFactors ( ) const ;
2013-06-07 02:07:55 +08:00
gtsam : : NonlinearFactor * at ( size_t idx ) const ;
2012-10-02 22:40:07 +08:00
void push_back ( const gtsam : : NonlinearFactorGraph & factors ) ;
2013-08-13 02:21:30 +08:00
void push_back ( gtsam : : NonlinearFactor * factor ) ;
2013-08-27 23:30:02 +08:00
void add ( gtsam : : NonlinearFactor * factor ) ;
2013-08-24 04:06:08 +08:00
bool exists ( size_t idx ) const ;
2012-10-02 22:40:07 +08:00
// NonlinearFactorGraph
double error ( const gtsam : : Values & values ) const ;
double probPrime ( const gtsam : : Values & values ) const ;
2013-08-13 02:21:30 +08:00
gtsam : : Ordering orderingCOLAMD ( ) const ;
2012-10-02 22:40:07 +08:00
// Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map<gtsam::Key,int>& constraints) const;
2013-08-13 02:21:30 +08:00
gtsam : : GaussianFactorGraph * linearize ( const gtsam : : Values & values ) const ;
2012-10-02 22:40:07 +08:00
gtsam : : NonlinearFactorGraph clone ( ) const ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2012-05-22 06:32:40 +08:00
} ;
2012-07-12 02:18:57 +08:00
virtual class NonlinearFactor {
2013-06-21 03:44:50 +08:00
// Factor base class
size_t size ( ) const ;
gtsam : : KeyVector keys ( ) const ;
2012-10-02 22:40:07 +08:00
void print ( string s ) const ;
void printKeys ( string s ) const ;
2013-06-21 03:44:50 +08:00
// NonlinearFactor
2012-10-02 22:40:07 +08:00
void equals ( const gtsam : : NonlinearFactor & other , double tol ) const ;
double error ( const gtsam : : Values & c ) const ;
2013-06-21 03:44:50 +08:00
size_t dim ( ) const ;
2012-10-02 22:40:07 +08:00
bool active ( const gtsam : : Values & c ) const ;
2013-08-13 02:21:30 +08:00
gtsam : : GaussianFactor * linearize ( const gtsam : : Values & c ) const ;
2012-10-02 22:40:07 +08:00
gtsam : : NonlinearFactor * clone ( ) const ;
// gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen
2012-06-15 00:03:57 +08:00
} ;
2013-06-21 03:44:50 +08:00
virtual class NoiseModelFactor : gtsam : : NonlinearFactor {
void equals ( const gtsam : : NoiseModelFactor & other , double tol ) const ;
gtsam : : noiseModel : : Base * get_noiseModel ( ) const ;
Vector unwhitenedError ( const gtsam : : Values & x ) const ;
Vector whitenedError ( const gtsam : : Values & x ) const ;
} ;
2012-08-08 02:21:35 +08:00
# include <gtsam/nonlinear/Values.h>
2012-05-22 06:32:40 +08:00
class Values {
2012-10-02 22:40:07 +08:00
Values ( ) ;
Values ( const gtsam : : Values & other ) ;
2012-08-08 02:21:35 +08:00
2012-10-02 22:40:07 +08:00
size_t size ( ) const ;
bool empty ( ) const ;
2013-06-20 01:50:01 +08:00
void clear ( ) ;
size_t dim ( ) const ;
2012-08-08 02:21:35 +08:00
2012-10-02 22:40:07 +08:00
void print ( string s ) const ;
bool equals ( const gtsam : : Values & other , double tol ) const ;
2012-08-08 02:21:35 +08:00
2013-06-20 01:50:01 +08:00
void insert ( const gtsam : : Values & values ) ;
void update ( const gtsam : : Values & values ) ;
void erase ( size_t j ) ;
void swap ( gtsam : : Values & values ) ;
2012-08-08 02:21:35 +08:00
2012-10-02 22:40:07 +08:00
bool exists ( size_t j ) const ;
gtsam : : KeyList keys ( ) const ;
2012-08-08 02:21:35 +08:00
2013-08-13 02:21:30 +08:00
gtsam : : VectorValues zeroVectors ( ) const ;
2013-06-20 01:50:01 +08:00
2013-08-13 02:21:30 +08:00
gtsam : : Values retract ( const gtsam : : VectorValues & delta ) const ;
gtsam : : VectorValues localCoordinates ( const gtsam : : Values & cp ) const ;
2012-08-08 02:21:35 +08:00
2013-06-20 01:50:01 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2014-11-10 23:43:49 +08:00
2014-11-11 20:42:13 +08:00
// New in 4.0, we have to specialize every insert/update/at to generate wrappers
2014-11-10 23:43:49 +08:00
// Instead of the old:
// void insert(size_t j, const gtsam::Value& value);
// void update(size_t j, const gtsam::Value& val);
2014-11-11 20:42:13 +08:00
// gtsam::Value at(size_t j) const;
2014-11-11 01:06:09 +08:00
void insert ( size_t j , const gtsam : : Point2 & t ) ;
void insert ( size_t j , const gtsam : : Point3 & t ) ;
void insert ( size_t j , const gtsam : : Rot2 & t ) ;
2014-11-10 23:43:49 +08:00
void insert ( size_t j , const gtsam : : Pose2 & t ) ;
2014-11-11 01:06:09 +08:00
void insert ( size_t j , const gtsam : : Rot3 & t ) ;
void insert ( size_t j , const gtsam : : Pose3 & t ) ;
void insert ( size_t j , const gtsam : : Cal3_S2 & t ) ;
void insert ( size_t j , const gtsam : : Cal3DS2 & t ) ;
void insert ( size_t j , const gtsam : : Cal3Bundler & t ) ;
void insert ( size_t j , const gtsam : : EssentialMatrix & t ) ;
2014-11-22 10:57:18 +08:00
void insert ( size_t j , const gtsam : : imuBias : : ConstantBias & t ) ;
2014-11-26 16:54:21 +08:00
void insert ( size_t j , Vector t ) ;
void insert ( size_t j , Matrix t ) ; //git/gtsam/gtsam/base/Manifold.h:254:1: error: invalid application of ‘ ’ ‘ ’
2014-11-11 20:42:13 +08:00
void update ( size_t j , const gtsam : : Point2 & t ) ;
void update ( size_t j , const gtsam : : Point3 & t ) ;
void update ( size_t j , const gtsam : : Rot2 & t ) ;
void update ( size_t j , const gtsam : : Pose2 & t ) ;
void update ( size_t j , const gtsam : : Rot3 & t ) ;
void update ( size_t j , const gtsam : : Pose3 & t ) ;
void update ( size_t j , const gtsam : : Cal3_S2 & t ) ;
void update ( size_t j , const gtsam : : Cal3DS2 & t ) ;
void update ( size_t j , const gtsam : : Cal3Bundler & t ) ;
2014-11-11 01:06:09 +08:00
void update ( size_t j , const gtsam : : EssentialMatrix & t ) ;
2014-11-22 10:57:18 +08:00
void update ( size_t j , const gtsam : : imuBias : : ConstantBias & t ) ;
2014-11-26 16:54:21 +08:00
void update ( size_t j , Vector t ) ;
void update ( size_t j , Matrix t ) ;
2014-11-10 23:44:48 +08:00
2014-11-13 08:26:06 +08:00
template < T = { gtsam : : Point2 , gtsam : : Point3 , gtsam : : Rot2 , gtsam : : Pose2 ,
2014-11-26 16:54:21 +08:00
gtsam : : Rot3 , gtsam : : Pose3 , gtsam : : Cal3_S2 , gtsam : : Cal3DS2 , gtsam : : imuBias : : ConstantBias , Vector , Matrix } > // Parse Error
2014-11-13 08:26:06 +08:00
T at ( size_t j ) ;
2012-05-22 06:32:40 +08:00
} ;
2012-06-14 00:38:51 +08:00
// Actually a FastList<Key>
2013-08-13 02:21:30 +08:00
# include <gtsam/inference/Key.h>
2012-06-14 00:38:51 +08:00
class KeyList {
2012-10-02 22:40:07 +08:00
KeyList ( ) ;
KeyList ( const gtsam : : KeyList & other ) ;
2012-06-14 00:38:51 +08:00
2012-10-02 22:40:07 +08:00
// Note: no print function
2012-07-03 01:32:47 +08:00
2012-10-02 22:40:07 +08:00
// common STL methods
size_t size ( ) const ;
bool empty ( ) const ;
void clear ( ) ;
2012-07-03 01:32:47 +08:00
2012-10-02 22:40:07 +08:00
// structure specific methods
size_t front ( ) const ;
size_t back ( ) const ;
void push_back ( size_t key ) ;
void push_front ( size_t key ) ;
2012-11-18 03:24:39 +08:00
void pop_back ( ) ;
void pop_front ( ) ;
2012-10-02 22:40:07 +08:00
void sort ( ) ;
void remove ( size_t key ) ;
2013-11-12 22:04:48 +08:00
void serialize ( ) const ;
2012-07-03 01:32:47 +08:00
} ;
// Actually a FastSet<Key>
class KeySet {
2012-10-02 22:40:07 +08:00
KeySet ( ) ;
KeySet ( const gtsam : : KeySet & other ) ;
KeySet ( const gtsam : : KeyVector & other ) ;
KeySet ( const gtsam : : KeyList & other ) ;
2012-07-03 01:32:47 +08:00
2012-10-02 22:40:07 +08:00
// Testable
void print ( string s ) const ;
bool equals ( const gtsam : : KeySet & other ) const ;
2012-07-03 01:32:47 +08:00
2012-10-02 22:40:07 +08:00
// common STL methods
size_t size ( ) const ;
bool empty ( ) const ;
void clear ( ) ;
2012-07-03 01:32:47 +08:00
2012-10-02 22:40:07 +08:00
// structure specific methods
void insert ( size_t key ) ;
2014-05-06 23:08:33 +08:00
void merge ( gtsam : : KeySet & other ) ;
2012-10-02 22:40:07 +08:00
bool erase ( size_t key ) ; // returns true if value was removed
bool count ( size_t key ) const ; // returns true if value exists
2013-11-12 22:04:48 +08:00
void serialize ( ) const ;
2012-06-14 00:38:51 +08:00
} ;
2013-08-13 02:21:30 +08:00
// Actually a vector<Key>
2012-06-14 00:38:51 +08:00
class KeyVector {
2012-10-02 22:40:07 +08:00
KeyVector ( ) ;
KeyVector ( const gtsam : : KeyVector & other ) ;
KeyVector ( const gtsam : : KeySet & other ) ;
KeyVector ( const gtsam : : KeyList & other ) ;
2012-06-14 00:38:51 +08:00
2012-10-02 22:40:07 +08:00
// Note: no print function
2012-07-03 01:32:47 +08:00
2012-10-02 22:40:07 +08:00
// common STL methods
size_t size ( ) const ;
bool empty ( ) const ;
void clear ( ) ;
2012-07-03 01:32:47 +08:00
2012-10-02 22:40:07 +08:00
// structure specific methods
size_t at ( size_t i ) const ;
size_t front ( ) const ;
size_t back ( ) const ;
void push_back ( size_t key ) const ;
2013-11-12 22:04:48 +08:00
void serialize ( ) const ;
2012-06-14 00:38:51 +08:00
} ;
2013-12-04 22:36:13 +08:00
// Actually a FastMap<Key,int>
class KeyGroupMap {
KeyGroupMap ( ) ;
// Note: no print function
// common STL methods
size_t size ( ) const ;
bool empty ( ) const ;
void clear ( ) ;
// structure specific methods
size_t at ( size_t key ) const ;
int erase ( size_t key ) ;
bool insert2 ( size_t key , int val ) ;
} ;
2012-07-23 23:33:39 +08:00
# include <gtsam/nonlinear/Marginals.h>
2012-05-22 06:32:40 +08:00
class Marginals {
2012-10-02 22:40:07 +08:00
Marginals ( const gtsam : : NonlinearFactorGraph & graph ,
const gtsam : : Values & solution ) ;
2014-10-15 06:08:26 +08:00
2012-10-02 22:40:07 +08:00
void print ( string s ) const ;
Matrix marginalCovariance ( size_t variable ) const ;
Matrix marginalInformation ( size_t variable ) const ;
gtsam : : JointMarginal jointMarginalCovariance ( const gtsam : : KeyVector & variables ) const ;
gtsam : : JointMarginal jointMarginalInformation ( const gtsam : : KeyVector & variables ) const ;
2012-07-22 07:54:55 +08:00
} ;
class JointMarginal {
2012-10-02 22:40:07 +08:00
Matrix at ( size_t iVariable , size_t jVariable ) const ;
Matrix fullMatrix ( ) const ;
void print ( string s ) const ;
void print ( ) const ;
2012-05-22 06:32:40 +08:00
} ;
2012-05-22 05:53:56 +08:00
2012-11-22 03:41:24 +08:00
# include <gtsam/nonlinear/LinearContainerFactor.h>
virtual class LinearContainerFactor : gtsam : : NonlinearFactor {
2013-08-13 02:21:30 +08:00
LinearContainerFactor ( gtsam : : GaussianFactor * factor , const gtsam : : Values & linearizationPoint ) ;
2012-11-22 03:41:24 +08:00
LinearContainerFactor ( gtsam : : GaussianFactor * factor ) ;
gtsam : : GaussianFactor * factor ( ) const ;
// const boost::optional<Values>& linearizationPoint() const;
bool isJacobian ( ) const ;
gtsam : : JacobianFactor * toJacobian ( ) const ;
gtsam : : HessianFactor * toHessian ( ) const ;
static gtsam : : NonlinearFactorGraph convertLinearGraph ( const gtsam : : GaussianFactorGraph & linear_graph ,
2013-08-13 02:21:30 +08:00
const gtsam : : Values & linearizationPoint ) ;
2012-11-22 03:41:24 +08:00
2013-08-13 02:21:30 +08:00
static gtsam : : NonlinearFactorGraph convertLinearGraph ( const gtsam : : GaussianFactorGraph & linear_graph ) ;
2012-11-22 03:41:24 +08:00
2013-06-20 01:50:07 +08:00
// enabling serialization functionality
void serializable ( ) const ;
2012-11-22 03:41:24 +08:00
} ; // \class LinearContainerFactor
2013-05-08 18:13:57 +08:00
// Summarization functionality
2013-08-20 01:06:28 +08:00
//#include <gtsam/nonlinear/summarization.h>
//
//// Uses partial QR approach by default
//gtsam::GaussianFactorGraph summarize(
// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values,
// const gtsam::KeySet& saved_keys);
//
//gtsam::NonlinearFactorGraph summarizeAsNonlinearContainer(
// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values,
// const gtsam::KeySet& saved_keys);
2013-05-08 18:13:57 +08:00
2012-07-19 07:35:36 +08:00
//*************************************************************************
// Nonlinear optimizers
//*************************************************************************
# include <gtsam/nonlinear/NonlinearOptimizer.h>
2013-10-26 03:47:22 +08:00
# include <gtsam/nonlinear/NonlinearOptimizerParams.h>
2012-07-19 07:35:36 +08:00
virtual class NonlinearOptimizerParams {
2012-10-02 22:40:07 +08:00
NonlinearOptimizerParams ( ) ;
void print ( string s ) const ;
2012-07-04 23:41:19 +08:00
2013-11-19 22:04:52 +08:00
int getMaxIterations ( ) const ;
2012-10-02 22:40:07 +08:00
double getRelativeErrorTol ( ) const ;
double getAbsoluteErrorTol ( ) const ;
double getErrorTol ( ) const ;
string getVerbosity ( ) const ;
2012-07-19 07:35:36 +08:00
2013-11-19 22:04:52 +08:00
void setMaxIterations ( int value ) ;
2012-10-02 22:40:07 +08:00
void setRelativeErrorTol ( double value ) ;
void setAbsoluteErrorTol ( double value ) ;
void setErrorTol ( double value ) ;
void setVerbosity ( string s ) ;
2012-07-04 23:41:19 +08:00
2012-10-02 22:40:07 +08:00
string getLinearSolverType ( ) const ;
void setLinearSolverType ( string solver ) ;
void setOrdering ( const gtsam : : Ordering & ordering ) ;
2014-06-18 01:47:43 +08:00
void setIterativeParams ( gtsam : : IterativeOptimizationParameters * params ) ;
2012-07-24 23:48:33 +08:00
2012-07-04 23:41:19 +08:00
bool isMultifrontal ( ) const ;
bool isSequential ( ) const ;
bool isCholmod ( ) const ;
2014-06-04 11:52:35 +08:00
bool isIterative ( ) const ;
2012-07-19 07:35:36 +08:00
} ;
2013-10-26 03:47:22 +08:00
bool checkConvergence ( double relativeErrorTreshold ,
double absoluteErrorTreshold , double errorThreshold ,
double currentError , double newError ) ;
2012-07-24 23:48:33 +08:00
# include <gtsam/nonlinear/GaussNewtonOptimizer.h>
2013-10-26 03:47:22 +08:00
virtual class GaussNewtonParams : gtsam : : NonlinearOptimizerParams {
2012-10-02 22:40:07 +08:00
GaussNewtonParams ( ) ;
2012-07-24 23:48:33 +08:00
} ;
2012-07-19 07:35:36 +08:00
# include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
2013-10-26 03:47:22 +08:00
virtual class LevenbergMarquardtParams : gtsam : : NonlinearOptimizerParams {
2012-10-02 22:40:07 +08:00
LevenbergMarquardtParams ( ) ;
2012-07-04 23:41:19 +08:00
2012-07-19 07:35:36 +08:00
double getlambdaInitial ( ) const ;
double getlambdaFactor ( ) const ;
2012-06-24 04:07:03 +08:00
double getlambdaUpperBound ( ) const ;
2012-07-19 07:35:36 +08:00
string getVerbosityLM ( ) const ;
2012-06-24 04:07:03 +08:00
void setlambdaInitial ( double value ) ;
void setlambdaFactor ( double value ) ;
void setlambdaUpperBound ( double value ) ;
void setVerbosityLM ( string s ) ;
2012-06-21 22:28:08 +08:00
} ;
2012-07-19 07:35:36 +08:00
# include <gtsam/nonlinear/DoglegOptimizer.h>
2013-10-26 03:47:22 +08:00
virtual class DoglegParams : gtsam : : NonlinearOptimizerParams {
2012-10-02 22:40:07 +08:00
DoglegParams ( ) ;
2012-07-19 07:35:36 +08:00
2012-10-02 22:40:07 +08:00
double getDeltaInitial ( ) const ;
string getVerbosityDL ( ) const ;
2012-07-19 07:35:36 +08:00
2012-10-02 22:40:07 +08:00
void setDeltaInitial ( double deltaInitial ) const ;
void setVerbosityDL ( string verbosityDL ) const ;
2012-07-19 07:35:36 +08:00
} ;
virtual class NonlinearOptimizer {
2012-10-02 22:40:07 +08:00
gtsam : : Values optimize ( ) ;
gtsam : : Values optimizeSafely ( ) ;
double error ( ) const ;
int iterations ( ) const ;
gtsam : : Values values ( ) const ;
void iterate ( ) const ;
2012-07-19 07:35:36 +08:00
} ;
2012-07-24 23:48:33 +08:00
virtual class GaussNewtonOptimizer : gtsam : : NonlinearOptimizer {
2012-10-02 22:40:07 +08:00
GaussNewtonOptimizer ( const gtsam : : NonlinearFactorGraph & graph , const gtsam : : Values & initialValues ) ;
GaussNewtonOptimizer ( const gtsam : : NonlinearFactorGraph & graph , const gtsam : : Values & initialValues , const gtsam : : GaussNewtonParams & params ) ;
2012-07-24 23:48:33 +08:00
} ;
2012-07-19 07:35:36 +08:00
virtual class DoglegOptimizer : gtsam : : NonlinearOptimizer {
2012-10-02 22:40:07 +08:00
DoglegOptimizer ( const gtsam : : NonlinearFactorGraph & graph , const gtsam : : Values & initialValues ) ;
DoglegOptimizer ( const gtsam : : NonlinearFactorGraph & graph , const gtsam : : Values & initialValues , const gtsam : : DoglegParams & params ) ;
double getDelta ( ) const ;
2012-07-19 07:35:36 +08:00
} ;
virtual class LevenbergMarquardtOptimizer : gtsam : : NonlinearOptimizer {
2012-10-02 22:40:07 +08:00
LevenbergMarquardtOptimizer ( const gtsam : : NonlinearFactorGraph & graph , const gtsam : : Values & initialValues ) ;
LevenbergMarquardtOptimizer ( const gtsam : : NonlinearFactorGraph & graph , const gtsam : : Values & initialValues , const gtsam : : LevenbergMarquardtParams & params ) ;
double lambda ( ) const ;
2012-08-05 04:32:23 +08:00
void print ( string str ) const ;
2012-07-19 07:35:36 +08:00
} ;
2012-07-20 03:50:00 +08:00
# include <gtsam/nonlinear/ISAM2.h>
class ISAM2GaussNewtonParams {
ISAM2GaussNewtonParams ( ) ;
void print ( string str ) const ;
/** Getters and Setters for all properties */
double getWildfireThreshold ( ) const ;
void setWildfireThreshold ( double wildfireThreshold ) ;
} ;
class ISAM2DoglegParams {
ISAM2DoglegParams ( ) ;
void print ( string str ) const ;
/** Getters and Setters for all properties */
double getWildfireThreshold ( ) const ;
void setWildfireThreshold ( double wildfireThreshold ) ;
double getInitialDelta ( ) const ;
void setInitialDelta ( double initialDelta ) ;
string getAdaptationMode ( ) const ;
void setAdaptationMode ( string adaptationMode ) ;
bool isVerbose ( ) const ;
void setVerbose ( bool verbose ) ;
} ;
2012-07-31 05:19:24 +08:00
class ISAM2ThresholdMapValue {
ISAM2ThresholdMapValue ( char c , Vector thresholds ) ;
ISAM2ThresholdMapValue ( const gtsam : : ISAM2ThresholdMapValue & other ) ;
} ;
class ISAM2ThresholdMap {
ISAM2ThresholdMap ( ) ;
ISAM2ThresholdMap ( const gtsam : : ISAM2ThresholdMap & other ) ;
// Note: no print function
// common STL methods
size_t size ( ) const ;
bool empty ( ) const ;
void clear ( ) ;
// structure specific methods
void insert ( const gtsam : : ISAM2ThresholdMapValue & value ) const ;
} ;
2012-07-20 03:50:00 +08:00
class ISAM2Params {
ISAM2Params ( ) ;
void print ( string str ) const ;
/** Getters and Setters for all properties */
void setOptimizationParams ( const gtsam : : ISAM2GaussNewtonParams & params ) ;
void setOptimizationParams ( const gtsam : : ISAM2DoglegParams & params ) ;
void setRelinearizeThreshold ( double relinearizeThreshold ) ;
2012-07-31 05:19:24 +08:00
void setRelinearizeThreshold ( const gtsam : : ISAM2ThresholdMap & relinearizeThreshold ) ;
2012-07-20 03:50:00 +08:00
int getRelinearizeSkip ( ) const ;
void setRelinearizeSkip ( int relinearizeSkip ) ;
bool isEnableRelinearization ( ) const ;
void setEnableRelinearization ( bool enableRelinearization ) ;
bool isEvaluateNonlinearError ( ) const ;
void setEvaluateNonlinearError ( bool evaluateNonlinearError ) ;
string getFactorization ( ) const ;
void setFactorization ( string factorization ) ;
bool isCacheLinearizedFactors ( ) const ;
void setCacheLinearizedFactors ( bool cacheLinearizedFactors ) ;
bool isEnableDetailedResults ( ) const ;
void setEnableDetailedResults ( bool enableDetailedResults ) ;
bool isEnablePartialRelinearizationCheck ( ) const ;
void setEnablePartialRelinearizationCheck ( bool enablePartialRelinearizationCheck ) ;
} ;
2013-08-13 02:21:30 +08:00
class ISAM2Clique {
2012-09-09 03:51:23 +08:00
//Constructors
2013-08-13 02:21:30 +08:00
ISAM2Clique ( ) ;
2012-09-09 03:51:23 +08:00
//Standard Interface
Vector gradientContribution ( ) const ;
void print ( string s ) ;
} ;
2012-07-20 03:50:00 +08:00
class ISAM2Result {
ISAM2Result ( ) ;
void print ( string str ) const ;
/** Getters and Setters for all properties */
size_t getVariablesRelinearized ( ) const ;
size_t getVariablesReeliminated ( ) const ;
size_t getCliques ( ) const ;
} ;
2013-08-13 02:21:30 +08:00
class ISAM2 {
2012-07-20 03:50:00 +08:00
ISAM2 ( ) ;
ISAM2 ( const gtsam : : ISAM2Params & params ) ;
2013-03-14 02:17:45 +08:00
ISAM2 ( const gtsam : : ISAM2 & other ) ;
2012-07-20 03:50:00 +08:00
bool equals ( const gtsam : : ISAM2 & other , double tol ) const ;
void print ( string s ) const ;
2012-09-04 12:17:55 +08:00
void printStats ( ) const ;
void saveGraph ( string s ) const ;
2012-07-20 03:50:00 +08:00
gtsam : : ISAM2Result update ( ) ;
gtsam : : ISAM2Result update ( const gtsam : : NonlinearFactorGraph & newFactors , const gtsam : : Values & newTheta ) ;
gtsam : : ISAM2Result update ( const gtsam : : NonlinearFactorGraph & newFactors , const gtsam : : Values & newTheta , const gtsam : : KeyVector & removeFactorIndices ) ;
2013-12-04 22:36:13 +08:00
gtsam : : ISAM2Result update ( const gtsam : : NonlinearFactorGraph & newFactors , const gtsam : : Values & newTheta , const gtsam : : KeyVector & removeFactorIndices , const gtsam : : KeyGroupMap & constrainedKeys ) ;
2012-07-20 03:50:00 +08:00
// TODO: wrap the full version of update
2012-09-08 07:10:49 +08:00
//void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys);
2012-07-20 03:50:00 +08:00
//void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys, bool force_relinearize);
gtsam : : Values getLinearizationPoint ( ) const ;
gtsam : : Values calculateEstimate ( ) const ;
2013-08-03 00:04:17 +08:00
gtsam : : Value calculateEstimate ( size_t key ) const ;
2012-07-20 03:50:00 +08:00
gtsam : : Values calculateBestEstimate ( ) const ;
2013-08-03 00:04:17 +08:00
Matrix marginalCovariance ( size_t key ) const ;
2012-07-20 03:50:00 +08:00
gtsam : : VectorValues getDelta ( ) const ;
gtsam : : NonlinearFactorGraph getFactorsUnsafe ( ) const ;
gtsam : : VariableIndex getVariableIndex ( ) const ;
gtsam : : ISAM2Params params ( ) const ;
} ;
2012-08-04 04:23:08 +08:00
# include <gtsam/nonlinear/NonlinearISAM.h>
class NonlinearISAM {
2012-10-02 22:40:07 +08:00
NonlinearISAM ( ) ;
NonlinearISAM ( int reorderInterval ) ;
2012-08-04 04:23:08 +08:00
void print ( string s ) const ;
void printStats ( ) const ;
void saveGraph ( string s ) const ;
2012-10-02 22:40:07 +08:00
gtsam : : Values estimate ( ) const ;
2012-08-04 04:23:08 +08:00
Matrix marginalCovariance ( size_t key ) const ;
int reorderInterval ( ) const ;
int reorderCounter ( ) const ;
void update ( const gtsam : : NonlinearFactorGraph & newFactors , const gtsam : : Values & initialValues ) ;
void reorder_relinearize ( ) ;
// These might be expensive as instead of a reference the wrapper will make a copy
gtsam : : GaussianISAM bayesTree ( ) const ;
gtsam : : Values getLinearizationPoint ( ) const ;
gtsam : : NonlinearFactorGraph getFactorsUnsafe ( ) const ;
} ;
2012-07-12 05:43:19 +08:00
//*************************************************************************
// Nonlinear factor types
//*************************************************************************
2012-07-23 23:33:39 +08:00
# include <gtsam/geometry/Cal3_S2.h>
# include <gtsam/geometry/Cal3DS2.h>
# include <gtsam/geometry/Cal3_S2Stereo.h>
# include <gtsam/geometry/SimpleCamera.h>
# include <gtsam/geometry/CalibratedCamera.h>
# include <gtsam/geometry/StereoPoint2.h>
# include <gtsam/slam/PriorFactor.h>
2014-11-26 16:54:21 +08:00
template < T = { gtsam : : Point2 , gtsam : : StereoPoint2 , gtsam : : Point3 , gtsam : : Rot2 ,
gtsam : : Rot3 , gtsam : : Pose2 , gtsam : : Pose3 , gtsam : : Cal3_S2 ,
gtsam : : CalibratedCamera , gtsam : : SimpleCamera , gtsam : : imuBias : : ConstantBias , Vector , Matrix } > // Parse Error
2013-08-09 00:08:35 +08:00
virtual class PriorFactor : gtsam : : NoiseModelFactor {
2012-10-02 22:40:07 +08:00
PriorFactor ( size_t key , const T & prior , const gtsam : : noiseModel : : Base * noiseModel ) ;
2013-06-11 04:49:47 +08:00
T prior ( ) const ;
2013-06-20 01:50:05 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2012-07-12 05:43:19 +08:00
} ;
2012-07-12 10:11:32 +08:00
2012-07-23 23:33:39 +08:00
# include <gtsam/slam/BetweenFactor.h>
2014-11-26 16:58:49 +08:00
template < T = { gtsam : : Point2 , gtsam : : Point3 , gtsam : : Rot2 , gtsam : : Rot3 , gtsam : : Pose2 , gtsam : : Pose3 , gtsam : : imuBias : : ConstantBias , Vector , Matrix } > // Parse Error
2013-08-09 00:08:35 +08:00
virtual class BetweenFactor : gtsam : : NoiseModelFactor {
2012-10-02 22:40:07 +08:00
BetweenFactor ( size_t key1 , size_t key2 , const T & relativePose , const gtsam : : noiseModel : : Base * noiseModel ) ;
2013-06-11 04:49:47 +08:00
T measured ( ) const ;
2013-06-20 01:50:07 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2012-07-12 05:43:19 +08:00
} ;
2012-07-12 10:11:32 +08:00
2014-09-27 05:21:43 +08:00
2012-07-23 23:33:39 +08:00
# include <gtsam/nonlinear/NonlinearEquality.h>
2014-11-08 05:41:21 +08:00
template < T = { gtsam : : Point2 , gtsam : : StereoPoint2 , gtsam : : Point3 , gtsam : : Rot2 , gtsam : : Rot3 , gtsam : : Pose2 , gtsam : : Pose3 , gtsam : : Cal3_S2 , gtsam : : CalibratedCamera , gtsam : : SimpleCamera , gtsam : : imuBias : : ConstantBias } >
2013-08-09 00:08:35 +08:00
virtual class NonlinearEquality : gtsam : : NoiseModelFactor {
2012-10-02 22:40:07 +08:00
// Constructor - forces exact evaluation
NonlinearEquality ( size_t j , const T & feasible ) ;
// Constructor - allows inexact evaluation
NonlinearEquality ( size_t j , const T & feasible , double error_gain ) ;
2013-06-20 01:50:07 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2012-07-14 05:55:02 +08:00
} ;
2012-07-23 23:33:39 +08:00
# include <gtsam/slam/RangeFactor.h>
2012-07-12 10:11:32 +08:00
template < POSE , POINT >
2013-06-21 03:44:50 +08:00
virtual class RangeFactor : gtsam : : NoiseModelFactor {
2012-10-02 22:40:07 +08:00
RangeFactor ( size_t key1 , size_t key2 , double measured , const gtsam : : noiseModel : : Base * noiseModel ) ;
2012-07-12 10:11:32 +08:00
} ;
2012-07-19 23:11:11 +08:00
typedef gtsam : : RangeFactor < gtsam : : Pose2 , gtsam : : Point2 > RangeFactorPosePoint2 ;
typedef gtsam : : RangeFactor < gtsam : : Pose3 , gtsam : : Point3 > RangeFactorPosePoint3 ;
typedef gtsam : : RangeFactor < gtsam : : Pose2 , gtsam : : Pose2 > RangeFactorPose2 ;
typedef gtsam : : RangeFactor < gtsam : : Pose3 , gtsam : : Pose3 > RangeFactorPose3 ;
typedef gtsam : : RangeFactor < gtsam : : CalibratedCamera , gtsam : : Point3 > RangeFactorCalibratedCameraPoint ;
typedef gtsam : : RangeFactor < gtsam : : SimpleCamera , gtsam : : Point3 > RangeFactorSimpleCameraPoint ;
typedef gtsam : : RangeFactor < gtsam : : CalibratedCamera , gtsam : : CalibratedCamera > RangeFactorCalibratedCamera ;
typedef gtsam : : RangeFactor < gtsam : : SimpleCamera , gtsam : : SimpleCamera > RangeFactorSimpleCamera ;
2012-07-12 10:11:32 +08:00
2012-07-28 03:02:11 +08:00
# include <gtsam/slam/BearingFactor.h>
2012-07-24 03:20:56 +08:00
template < POSE , POINT , ROTATION >
2013-06-21 03:44:50 +08:00
virtual class BearingFactor : gtsam : : NoiseModelFactor {
2012-10-02 22:40:07 +08:00
BearingFactor ( size_t key1 , size_t key2 , const ROTATION & measured , const gtsam : : noiseModel : : Base * noiseModel ) ;
2013-06-20 01:50:07 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2012-07-12 10:11:32 +08:00
} ;
typedef gtsam : : BearingFactor < gtsam : : Pose2 , gtsam : : Point2 , gtsam : : Rot2 > BearingFactor2D ;
2012-07-28 03:02:11 +08:00
# include <gtsam/slam/BearingRangeFactor.h>
2012-07-24 03:20:56 +08:00
template < POSE , POINT , ROTATION >
2013-06-21 03:44:50 +08:00
virtual class BearingRangeFactor : gtsam : : NoiseModelFactor {
2012-10-02 22:40:07 +08:00
BearingRangeFactor ( size_t poseKey , size_t pointKey , const ROTATION & measuredBearing , double measuredRange , const gtsam : : noiseModel : : Base * noiseModel ) ;
2013-06-20 01:50:07 +08:00
2014-05-01 10:32:59 +08:00
pair < ROTATION , double > measured ( ) const ;
2013-06-20 01:50:07 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2012-07-24 03:20:56 +08:00
} ;
typedef gtsam : : BearingRangeFactor < gtsam : : Pose2 , gtsam : : Point2 , gtsam : : Rot2 > BearingRangeFactor2D ;
2012-07-18 02:30:02 +08:00
# include <gtsam/slam/ProjectionFactor.h>
2012-07-12 10:11:32 +08:00
template < POSE , LANDMARK , CALIBRATION >
2013-06-21 03:44:50 +08:00
virtual class GenericProjectionFactor : gtsam : : NoiseModelFactor {
2012-10-02 22:40:07 +08:00
GenericProjectionFactor ( const gtsam : : Point2 & measured , const gtsam : : noiseModel : : Base * noiseModel ,
size_t poseKey , size_t pointKey , const CALIBRATION * k ) ;
2012-12-18 06:30:54 +08:00
GenericProjectionFactor ( const gtsam : : Point2 & measured , const gtsam : : noiseModel : : Base * noiseModel ,
size_t poseKey , size_t pointKey , const CALIBRATION * k , const POSE & body_P_sensor ) ;
2013-02-22 05:21:07 +08:00
GenericProjectionFactor ( const gtsam : : Point2 & measured , const gtsam : : noiseModel : : Base * noiseModel ,
size_t poseKey , size_t pointKey , const CALIBRATION * k , bool throwCheirality , bool verboseCheirality ) ;
GenericProjectionFactor ( const gtsam : : Point2 & measured , const gtsam : : noiseModel : : Base * noiseModel ,
size_t poseKey , size_t pointKey , const CALIBRATION * k , bool throwCheirality , bool verboseCheirality ,
const POSE & body_P_sensor ) ;
2012-10-02 22:40:07 +08:00
gtsam : : Point2 measured ( ) const ;
CALIBRATION * calibration ( ) const ;
2013-02-22 05:21:07 +08:00
bool verboseCheirality ( ) const ;
bool throwCheirality ( ) const ;
2013-06-20 01:50:07 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2012-07-12 10:11:32 +08:00
} ;
typedef gtsam : : GenericProjectionFactor < gtsam : : Pose3 , gtsam : : Point3 , gtsam : : Cal3_S2 > GenericProjectionFactorCal3_S2 ;
2013-02-16 01:27:38 +08:00
typedef gtsam : : GenericProjectionFactor < gtsam : : Pose3 , gtsam : : Point3 , gtsam : : Cal3DS2 > GenericProjectionFactorCal3DS2 ;
2012-07-24 05:27:42 +08:00
# include <gtsam/slam/GeneralSFMFactor.h>
template < CAMERA , LANDMARK >
2013-06-21 03:44:50 +08:00
virtual class GeneralSFMFactor : gtsam : : NoiseModelFactor {
2012-10-02 22:40:07 +08:00
GeneralSFMFactor ( const gtsam : : Point2 & measured , const gtsam : : noiseModel : : Base * model , size_t cameraKey , size_t landmarkKey ) ;
gtsam : : Point2 measured ( ) const ;
2012-07-24 05:27:42 +08:00
} ;
typedef gtsam : : GeneralSFMFactor < gtsam : : SimpleCamera , gtsam : : Point3 > GeneralSFMFactorCal3_S2 ;
2013-02-16 01:27:38 +08:00
typedef gtsam : : GeneralSFMFactor < gtsam : : PinholeCameraCal3DS2 , gtsam : : Point3 > GeneralSFMFactorCal3DS2 ;
2012-07-24 05:27:42 +08:00
template < CALIBRATION = { gtsam : : Cal3_S2 } >
2013-06-21 03:44:50 +08:00
virtual class GeneralSFMFactor2 : gtsam : : NoiseModelFactor {
2012-10-02 22:40:07 +08:00
GeneralSFMFactor2 ( const gtsam : : Point2 & measured , const gtsam : : noiseModel : : Base * model , size_t poseKey , size_t landmarkKey , size_t calibKey ) ;
gtsam : : Point2 measured ( ) const ;
2013-06-20 01:50:07 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2012-07-24 05:27:42 +08:00
} ;
2012-07-12 10:11:32 +08:00
2014-06-21 01:41:51 +08:00
# include <gtsam/slam/SmartProjectionPoseFactor.h>
2014-11-26 16:34:37 +08:00
template < POSE , CALIBRATION >
2014-06-21 01:41:51 +08:00
virtual class SmartProjectionPoseFactor : gtsam : : NonlinearFactor {
SmartProjectionPoseFactor ( double rankTol , double linThreshold ,
bool manageDegeneracy , bool enableEPI , const POSE & body_P_sensor ) ;
SmartProjectionPoseFactor ( double rankTol ) ;
SmartProjectionPoseFactor ( ) ;
void add ( const gtsam : : Point2 & measured_i , size_t poseKey_i , const gtsam : : noiseModel : : Base * noise_i ,
const CALIBRATION * K_i ) ;
// enabling serialization functionality
//void serialize() const;
} ;
2014-11-26 16:34:37 +08:00
typedef gtsam : : SmartProjectionPoseFactor < gtsam : : Pose3 , gtsam : : Cal3_S2 > SmartProjectionPose3Factor ;
2014-06-21 01:41:51 +08:00
2012-07-24 22:23:36 +08:00
# include <gtsam/slam/StereoFactor.h>
template < POSE , LANDMARK >
2013-06-21 03:44:50 +08:00
virtual class GenericStereoFactor : gtsam : : NoiseModelFactor {
2012-10-02 22:40:07 +08:00
GenericStereoFactor ( const gtsam : : StereoPoint2 & measured , const gtsam : : noiseModel : : Base * noiseModel ,
size_t poseKey , size_t landmarkKey , const gtsam : : Cal3_S2Stereo * K ) ;
gtsam : : StereoPoint2 measured ( ) const ;
gtsam : : Cal3_S2Stereo * calibration ( ) const ;
2013-06-20 01:50:07 +08:00
// enabling serialization functionality
2013-10-24 22:38:04 +08:00
void serialize ( ) const ;
2012-07-24 22:23:36 +08:00
} ;
typedef gtsam : : GenericStereoFactor < gtsam : : Pose3 , gtsam : : Point3 > GenericStereoFactor3D ;
2013-05-01 01:50:48 +08:00
# include <gtsam/slam/PoseTranslationPrior.h>
template < POSE >
2013-06-21 03:44:50 +08:00
virtual class PoseTranslationPrior : gtsam : : NoiseModelFactor {
2013-05-01 01:50:48 +08:00
PoseTranslationPrior ( size_t key , const POSE & pose_z , const gtsam : : noiseModel : : Base * noiseModel ) ;
} ;
typedef gtsam : : PoseTranslationPrior < gtsam : : Pose2 > PoseTranslationPrior2D ;
typedef gtsam : : PoseTranslationPrior < gtsam : : Pose3 > PoseTranslationPrior3D ;
# include <gtsam/slam/PoseRotationPrior.h>
template < POSE >
2013-06-21 03:44:50 +08:00
virtual class PoseRotationPrior : gtsam : : NoiseModelFactor {
2013-05-01 01:50:48 +08:00
PoseRotationPrior ( size_t key , const POSE & pose_z , const gtsam : : noiseModel : : Base * noiseModel ) ;
} ;
typedef gtsam : : PoseRotationPrior < gtsam : : Pose2 > PoseRotationPrior2D ;
typedef gtsam : : PoseRotationPrior < gtsam : : Pose3 > PoseRotationPrior3D ;
2013-12-18 00:18:31 +08:00
# include <gtsam/slam/EssentialMatrixFactor.h>
virtual class EssentialMatrixFactor : gtsam : : NoiseModelFactor {
EssentialMatrixFactor ( size_t key , const gtsam : : Point2 & pA , const gtsam : : Point2 & pB ,
const gtsam : : noiseModel : : Base * noiseModel ) ;
} ;
2012-07-26 09:40:25 +08:00
# include <gtsam/slam/dataset.h>
2012-07-28 03:02:11 +08:00
pair < gtsam : : NonlinearFactorGraph * , gtsam : : Values * > load2D ( string filename ,
2012-07-26 09:40:25 +08:00
gtsam : : noiseModel : : Diagonal * model , int maxID , bool addNoise , bool smart ) ;
2012-07-31 02:34:47 +08:00
pair < gtsam : : NonlinearFactorGraph * , gtsam : : Values * > load2D ( string filename ,
2012-10-02 22:40:07 +08:00
gtsam : : noiseModel : : Diagonal * model , int maxID , bool addNoise ) ;
2012-07-31 02:34:47 +08:00
pair < gtsam : : NonlinearFactorGraph * , gtsam : : Values * > load2D ( string filename ,
2012-10-02 22:40:07 +08:00
gtsam : : noiseModel : : Diagonal * model , int maxID ) ;
2012-07-31 02:34:47 +08:00
pair < gtsam : : NonlinearFactorGraph * , gtsam : : Values * > load2D ( string filename ,
2012-10-02 22:40:07 +08:00
gtsam : : noiseModel : : Diagonal * model ) ;
2013-12-06 03:56:46 +08:00
pair < gtsam : : NonlinearFactorGraph * , gtsam : : Values * > load2D ( string filename ) ;
2013-05-21 05:46:30 +08:00
pair < gtsam : : NonlinearFactorGraph * , gtsam : : Values * > load2D_robust ( string filename ,
gtsam : : noiseModel : : Base * model ) ;
2014-06-01 23:46:23 +08:00
void save2D ( const gtsam : : NonlinearFactorGraph & graph ,
const gtsam : : Values & config , gtsam : : noiseModel : : Diagonal * model ,
string filename ) ;
pair < gtsam : : NonlinearFactorGraph * , gtsam : : Values * > readG2o ( string filename ) ;
void writeG2o ( const gtsam : : NonlinearFactorGraph & graph ,
const gtsam : : Values & estimate , string filename ) ;
2012-07-30 23:40:58 +08:00
2013-08-03 00:04:17 +08:00
//*************************************************************************
// Navigation
//*************************************************************************
namespace imuBias {
# include <gtsam/navigation/ImuBias.h>
2014-11-07 23:41:43 +08:00
class ConstantBias {
2013-08-03 00:04:17 +08:00
// Standard Constructor
ConstantBias ( ) ;
ConstantBias ( Vector biasAcc , Vector biasGyro ) ;
// Testable
void print ( string s ) const ;
bool equals ( const gtsam : : imuBias : : ConstantBias & expected , double tol ) const ;
// Group
static gtsam : : imuBias : : ConstantBias identity ( ) ;
gtsam : : imuBias : : ConstantBias inverse ( ) const ;
gtsam : : imuBias : : ConstantBias compose ( const gtsam : : imuBias : : ConstantBias & b ) const ;
gtsam : : imuBias : : ConstantBias between ( const gtsam : : imuBias : : ConstantBias & b ) const ;
// Manifold
static size_t Dim ( ) ;
size_t dim ( ) const ;
gtsam : : imuBias : : ConstantBias retract ( Vector v ) const ;
Vector localCoordinates ( const gtsam : : imuBias : : ConstantBias & b ) const ;
// Lie Group
static gtsam : : imuBias : : ConstantBias Expmap ( Vector v ) ;
static Vector Logmap ( const gtsam : : imuBias : : ConstantBias & b ) ;
// Standard Interface
Vector vector ( ) const ;
Vector accelerometer ( ) const ;
Vector gyroscope ( ) const ;
Vector correctAccelerometer ( Vector measurement ) const ;
Vector correctGyroscope ( Vector measurement ) const ;
} ;
} ///\namespace imuBias
2013-08-14 05:04:31 +08:00
# include <gtsam/navigation/ImuFactor.h>
2014-11-22 10:57:18 +08:00
class PoseVelocity {
PoseVelocity ( const gtsam : : Pose3 & pose , const gtsam : : Vector3 velocity ) ;
} ;
2013-08-14 05:04:31 +08:00
class ImuFactorPreintegratedMeasurements {
// Standard Constructor
2014-04-18 04:23:29 +08:00
ImuFactorPreintegratedMeasurements ( const gtsam : : imuBias : : ConstantBias & bias , Matrix measuredAccCovariance , Matrix measuredOmegaCovariance , Matrix integrationErrorCovariance , bool use2ndOrderIntegration ) ;
ImuFactorPreintegratedMeasurements ( const gtsam : : imuBias : : ConstantBias & bias , Matrix measuredAccCovariance , Matrix measuredOmegaCovariance , Matrix integrationErrorCovariance ) ;
2013-08-14 05:04:31 +08:00
ImuFactorPreintegratedMeasurements ( const gtsam : : ImuFactorPreintegratedMeasurements & rhs ) ;
// Testable
void print ( string s ) const ;
bool equals ( const gtsam : : ImuFactorPreintegratedMeasurements & expected , double tol ) ;
2014-11-22 10:57:18 +08:00
2014-11-14 02:46:00 +08:00
Matrix measurementCovariance ( ) const ;
Matrix deltaRij ( ) const ;
double deltaTij ( ) const ;
Vector deltaPij ( ) const ;
Vector deltaVij ( ) const ;
Vector biasHat ( ) const ;
2014-11-22 10:57:18 +08:00
Matrix delPdelBiasAcc ( ) const ;
Matrix delPdelBiasOmega ( ) const ;
Matrix delVdelBiasAcc ( ) const ;
Matrix delVdelBiasOmega ( ) const ;
Matrix delRdelBiasOmega ( ) const ;
2014-11-23 18:36:05 +08:00
Matrix preintMeasCov ( ) const ;
2014-10-15 06:08:26 +08:00
2013-08-14 05:04:31 +08:00
// Standard Interface
void integrateMeasurement ( Vector measuredAcc , Vector measuredOmega , double deltaT ) ;
void integrateMeasurement ( Vector measuredAcc , Vector measuredOmega , double deltaT , const gtsam : : Pose3 & body_P_sensor ) ;
} ;
virtual class ImuFactor : gtsam : : NonlinearFactor {
ImuFactor ( size_t pose_i , size_t vel_i , size_t pose_j , size_t vel_j , size_t bias ,
const gtsam : : ImuFactorPreintegratedMeasurements & preintegratedMeasurements , Vector gravity , Vector omegaCoriolis ) ;
ImuFactor ( size_t pose_i , size_t vel_i , size_t pose_j , size_t vel_j , size_t bias ,
const gtsam : : ImuFactorPreintegratedMeasurements & preintegratedMeasurements , Vector gravity , Vector omegaCoriolis ,
const gtsam : : Pose3 & body_P_sensor ) ;
// Standard Interface
gtsam : : ImuFactorPreintegratedMeasurements preintegratedMeasurements ( ) const ;
2014-11-22 10:57:18 +08:00
gtsam : : PoseVelocity Predict ( const gtsam : : Pose3 & pose_i , const gtsam : : Vector3 & vel_i , const gtsam : : imuBias : : ConstantBias & bias ,
2013-08-14 05:04:31 +08:00
const gtsam : : ImuFactorPreintegratedMeasurements & preintegratedMeasurements ,
Vector gravity , Vector omegaCoriolis ) const ;
} ;
2014-07-15 12:14:13 +08:00
# include <gtsam/navigation/AHRSFactor.h>
class AHRSFactorPreintegratedMeasurements {
// Standard Constructor
2014-11-25 03:14:56 +08:00
AHRSFactorPreintegratedMeasurements ( Vector bias , Matrix measuredOmegaCovariance ) ;
AHRSFactorPreintegratedMeasurements ( Vector bias , Matrix measuredOmegaCovariance ) ;
2014-07-15 12:14:13 +08:00
AHRSFactorPreintegratedMeasurements ( const gtsam : : AHRSFactorPreintegratedMeasurements & rhs ) ;
// Testable
void print ( string s ) const ;
bool equals ( const gtsam : : AHRSFactorPreintegratedMeasurements & expected , double tol ) ;
2014-07-19 04:46:58 +08:00
// get Data
2014-11-14 02:46:00 +08:00
Matrix measurementCovariance ( ) const ;
Matrix deltaRij ( ) const ;
double deltaTij ( ) const ;
Vector biasHat ( ) const ;
2014-07-19 04:46:58 +08:00
2014-07-15 12:14:13 +08:00
// Standard Interface
void integrateMeasurement ( Vector measuredOmega , double deltaT ) ;
void integrateMeasurement ( Vector measuredOmega , double deltaT , const gtsam : : Pose3 & body_P_sensor ) ;
2014-10-15 06:08:26 +08:00
void resetIntegration ( ) ;
2014-07-15 12:14:13 +08:00
} ;
virtual class AHRSFactor : gtsam : : NonlinearFactor {
AHRSFactor ( size_t rot_i , size_t rot_j , size_t bias ,
const gtsam : : AHRSFactorPreintegratedMeasurements & preintegratedMeasurements , Vector omegaCoriolis ) ;
AHRSFactor ( size_t rot_i , size_t rot_j , size_t bias ,
const gtsam : : AHRSFactorPreintegratedMeasurements & preintegratedMeasurements , Vector omegaCoriolis ,
const gtsam : : Pose3 & body_P_sensor ) ;
// Standard Interface
gtsam : : AHRSFactorPreintegratedMeasurements preintegratedMeasurements ( ) const ;
2014-10-15 06:08:26 +08:00
Vector evaluateError ( const gtsam : : Rot3 & rot_i , const gtsam : : Rot3 & rot_j ,
2014-11-25 03:14:56 +08:00
Vector bias ) const ;
gtsam : : Rot3 predict ( const gtsam : : Rot3 & rot_i , Vector bias ,
2014-07-15 12:14:13 +08:00
const gtsam : : AHRSFactorPreintegratedMeasurements & preintegratedMeasurements ,
Vector omegaCoriolis ) const ;
} ;
2013-08-14 05:04:31 +08:00
# include <gtsam/navigation/CombinedImuFactor.h>
2014-11-22 10:57:18 +08:00
class PoseVelocityBias {
PoseVelocityBias ( const gtsam : : Pose3 & pose , const gtsam : : Vector3 velocity , const gtsam : : imuBias : : ConstantBias & bias ) ;
} ;
2013-08-14 05:04:31 +08:00
class CombinedImuFactorPreintegratedMeasurements {
// Standard Constructor
CombinedImuFactorPreintegratedMeasurements (
const gtsam : : imuBias : : ConstantBias & bias ,
Matrix measuredAccCovariance ,
Matrix measuredOmegaCovariance ,
Matrix integrationErrorCovariance ,
Matrix biasAccCovariance ,
Matrix biasOmegaCovariance ,
Matrix biasAccOmegaInit ) ;
2014-04-25 03:52:11 +08:00
CombinedImuFactorPreintegratedMeasurements (
const gtsam : : imuBias : : ConstantBias & bias ,
Matrix measuredAccCovariance ,
Matrix measuredOmegaCovariance ,
Matrix integrationErrorCovariance ,
Matrix biasAccCovariance ,
Matrix biasOmegaCovariance ,
Matrix biasAccOmegaInit ,
bool use2ndOrderIntegration ) ;
2013-08-14 05:04:31 +08:00
CombinedImuFactorPreintegratedMeasurements ( const gtsam : : CombinedImuFactorPreintegratedMeasurements & rhs ) ;
// Testable
void print ( string s ) const ;
bool equals ( const gtsam : : CombinedImuFactorPreintegratedMeasurements & expected , double tol ) ;
// Standard Interface
void integrateMeasurement ( Vector measuredAcc , Vector measuredOmega , double deltaT ) ;
void integrateMeasurement ( Vector measuredAcc , Vector measuredOmega , double deltaT , const gtsam : : Pose3 & body_P_sensor ) ;
2014-10-15 06:08:26 +08:00
2014-11-22 10:57:18 +08:00
Matrix measurementCovariance ( ) const ;
2014-11-14 02:46:00 +08:00
Matrix deltaRij ( ) const ;
double deltaTij ( ) const ;
Vector deltaPij ( ) const ;
Vector deltaVij ( ) const ;
Vector biasHat ( ) const ;
2014-11-22 10:57:18 +08:00
Matrix delPdelBiasAcc ( ) const ;
Matrix delPdelBiasOmega ( ) const ;
Matrix delVdelBiasAcc ( ) const ;
Matrix delVdelBiasOmega ( ) const ;
Matrix delRdelBiasOmega ( ) const ;
Matrix PreintMeasCov ( ) const ;
2013-08-14 05:04:31 +08:00
} ;
virtual class CombinedImuFactor : gtsam : : NonlinearFactor {
CombinedImuFactor ( size_t pose_i , size_t vel_i , size_t pose_j , size_t vel_j , size_t bias_i , size_t bias_j ,
2014-04-30 03:13:28 +08:00
const gtsam : : CombinedImuFactorPreintegratedMeasurements & CombinedPreintegratedMeasurements , Vector gravity , Vector omegaCoriolis ) ;
2013-08-14 05:04:31 +08:00
// Standard Interface
gtsam : : CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements ( ) const ;
2014-11-22 10:57:18 +08:00
gtsam : : PoseVelocityBias Predict ( const gtsam : : Pose3 & pose_i , const gtsam : : Vector3 & vel_i , const gtsam : : imuBias : : ConstantBias & bias_i ,
2014-10-15 06:08:26 +08:00
const gtsam : : CombinedImuFactorPreintegratedMeasurements & preintegratedMeasurements ,
Vector gravity , Vector omegaCoriolis ) ;
2013-08-14 05:04:31 +08:00
} ;
2014-07-19 04:46:58 +08:00
# include <gtsam/navigation/AttitudeFactor.h>
//virtual class AttitudeFactor : gtsam::NonlinearFactor {
// AttitudeFactor(const Unit3& nZ, const Unit3& bRef);
// AttitudeFactor();
//};
virtual class Rot3AttitudeFactor : gtsam : : NonlinearFactor {
Rot3AttitudeFactor ( size_t key , const gtsam : : Unit3 & nZ , const gtsam : : noiseModel : : Diagonal * model ,
const gtsam : : Unit3 & bRef ) ;
Rot3AttitudeFactor ( size_t key , const gtsam : : Unit3 & nZ , const gtsam : : noiseModel : : Diagonal * model ) ;
Rot3AttitudeFactor ( ) ;
void print ( string s ) const ;
bool equals ( const gtsam : : NonlinearFactor & expected , double tol ) const ;
gtsam : : Unit3 nZ ( ) const ;
gtsam : : Unit3 bRef ( ) const ;
} ;
2014-10-15 06:08:26 +08:00
virtual class Pose3AttitudeFactor : gtsam : : NonlinearFactor {
Pose3AttitudeFactor ( size_t key , const gtsam : : Unit3 & nZ , const gtsam : : noiseModel : : Diagonal * model ,
const gtsam : : Unit3 & bRef ) ;
Pose3AttitudeFactor ( size_t key , const gtsam : : Unit3 & nZ , const gtsam : : noiseModel : : Diagonal * model ) ;
Pose3AttitudeFactor ( ) ;
void print ( string s ) const ;
bool equals ( const gtsam : : NonlinearFactor & expected , double tol ) const ;
gtsam : : Unit3 nZ ( ) const ;
gtsam : : Unit3 bRef ( ) const ;
} ;
2012-07-30 23:40:58 +08:00
//*************************************************************************
// Utilities
//*************************************************************************
namespace utilities {
# include <matlab.h>
2014-05-25 23:15:17 +08:00
gtsam : : KeyList createKeyList ( Vector I ) ;
gtsam : : KeyList createKeyList ( string s , Vector I ) ;
gtsam : : KeyVector createKeyVector ( Vector I ) ;
gtsam : : KeyVector createKeyVector ( string s , Vector I ) ;
gtsam : : KeySet createKeySet ( Vector I ) ;
gtsam : : KeySet createKeySet ( string s , Vector I ) ;
2012-07-30 23:40:58 +08:00
Matrix extractPoint2 ( const gtsam : : Values & values ) ;
Matrix extractPoint3 ( const gtsam : : Values & values ) ;
Matrix extractPose2 ( const gtsam : : Values & values ) ;
2012-11-18 03:24:39 +08:00
gtsam : : Values allPose3s ( gtsam : : Values & values ) ;
2012-07-30 23:40:58 +08:00
Matrix extractPose3 ( const gtsam : : Values & values ) ;
void perturbPoint2 ( gtsam : : Values & values , double sigma , int seed ) ;
2014-05-06 12:23:20 +08:00
void perturbPose2 ( gtsam : : Values & values , double sigmaT , double sigmaR , int seed ) ;
2012-07-30 23:40:58 +08:00
void perturbPoint3 ( gtsam : : Values & values , double sigma , int seed ) ;
void insertBackprojections ( gtsam : : Values & values , const gtsam : : SimpleCamera & c , Vector J , Matrix Z , double depth ) ;
2012-08-06 07:52:47 +08:00
void insertProjectionFactors ( gtsam : : NonlinearFactorGraph & graph , size_t i , Vector J , Matrix Z , const gtsam : : noiseModel : : Base * model , const gtsam : : Cal3_S2 * K ) ;
2012-12-18 06:30:54 +08:00
void insertProjectionFactors ( gtsam : : NonlinearFactorGraph & graph , size_t i , Vector J , Matrix Z , const gtsam : : noiseModel : : Base * model , const gtsam : : Cal3_S2 * K , const gtsam : : Pose3 & body_P_sensor ) ;
2012-07-30 23:40:58 +08:00
Matrix reprojectionErrors ( const gtsam : : NonlinearFactorGraph & graph , const gtsam : : Values & values ) ;
2014-05-27 12:42:03 +08:00
gtsam : : Values localToWorld ( const gtsam : : Values & local , const gtsam : : Pose2 & base ) ;
gtsam : : Values localToWorld ( const gtsam : : Values & local , const gtsam : : Pose2 & base , const gtsam : : KeyVector & keys ) ;
2012-07-30 23:40:58 +08:00
} //\namespace utilities
2012-07-28 03:02:11 +08:00
}
