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
|
|
|
};
|
|
|
|
|
|
2012-08-11 10:53:56 +08:00
|
|
|
#include <gtsam/base/LieScalar.h>
|
|
|
|
|
virtual class LieScalar : gtsam::Value {
|
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>
|
2012-07-10 04:19:37 +08:00
|
|
|
virtual class LieVector : gtsam::Value {
|
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
|
|
|
|
|
void serialize() const;
|
2012-06-19 02:12:19 +08:00
|
|
|
};
|
|
|
|
|
|
2012-07-18 02:30:02 +08:00
|
|
|
#include <gtsam/base/LieMatrix.h>
|
2012-07-17 23:51:32 +08:00
|
|
|
virtual class LieMatrix : gtsam::Value {
|
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
|
|
|
|
|
void serialize() const;
|
2012-07-17 23:51:32 +08:00
|
|
|
};
|
|
|
|
|
|
2012-01-18 04:04:37 +08:00
|
|
|
//*************************************************************************
|
|
|
|
|
// geometry
|
|
|
|
|
//*************************************************************************
|
|
|
|
|
|
2012-07-10 04:19:37 +08:00
|
|
|
virtual class Point2 : gtsam::Value {
|
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
|
|
|
|
|
void serialize() const;
|
2011-10-14 02:41:56 +08:00
|
|
|
};
|
|
|
|
|
|
2012-07-10 04:19:37 +08:00
|
|
|
virtual class StereoPoint2 : gtsam::Value {
|
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
|
|
|
|
|
void serialize() const;
|
2012-06-04 10:41:14 +08:00
|
|
|
};
|
|
|
|
|
|
2012-07-10 04:19:37 +08:00
|
|
|
virtual class Point3 : gtsam::Value {
|
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
|
|
|
|
|
void serialize() const;
|
2011-10-21 13:07:35 +08:00
|
|
|
};
|
2011-10-14 02:41:56 +08:00
|
|
|
|
2012-07-10 04:19:37 +08:00
|
|
|
virtual class Rot2 : gtsam::Value {
|
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
|
|
|
|
|
void serialize() const;
|
2011-10-14 12:43:40 +08:00
|
|
|
};
|
|
|
|
|
|
2012-07-10 04:19:37 +08:00
|
|
|
virtual class Rot3 : gtsam::Value {
|
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
|
|
|
|
|
void serialize() const;
|
2011-12-01 09:59:34 +08:00
|
|
|
};
|
|
|
|
|
|
2012-07-10 04:19:37 +08:00
|
|
|
virtual class Pose2 : gtsam::Value {
|
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
|
|
|
|
|
void serialize() const;
|
2011-10-21 13:07:35 +08:00
|
|
|
};
|
|
|
|
|
|
2012-07-10 04:19:37 +08:00
|
|
|
virtual class Pose3 : gtsam::Value {
|
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
|
|
|
|
|
void serialize() const;
|
2011-12-01 09:59:34 +08:00
|
|
|
};
|
|
|
|
|
|
2012-07-10 04:19:37 +08:00
|
|
|
virtual class Cal3_S2 : gtsam::Value {
|
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
|
|
|
|
|
void serialize() const;
|
2012-06-04 10:41:14 +08:00
|
|
|
};
|
|
|
|
|
|
2012-07-18 02:30:02 +08:00
|
|
|
#include <gtsam/geometry/Cal3DS2.h>
|
2012-07-12 10:11:32 +08:00
|
|
|
virtual class Cal3DS2 : gtsam::Value {
|
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
|
|
|
|
|
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
|
|
|
};
|
|
|
|
|
|
2012-07-10 04:19:37 +08:00
|
|
|
virtual class CalibratedCamera : gtsam::Value {
|
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
|
|
|
|
|
void serialize() const;
|
2012-06-05 04:17:24 +08:00
|
|
|
};
|
2012-06-03 13:25:50 +08:00
|
|
|
|
2012-07-10 04:19:37 +08:00
|
|
|
virtual class SimpleCamera : gtsam::Value {
|
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
|
|
|
|
|
void serialize() const;
|
2013-02-16 01:27:38 +08:00
|
|
|
};
|
|
|
|
|
|
|
|
|
|
template<CALIBRATION = {gtsam::Cal3DS2}>
|
|
|
|
|
virtual class PinholeCamera : gtsam::Value {
|
|
|
|
|
// 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
|
|
|
|
|
void serialize() const;
|
2013-02-16 01:27:38 +08:00
|
|
|
};
|
2012-07-24 05:27:42 +08:00
|
|
|
|
2013-07-23 23:32:38 +08:00
|
|
|
virtual class StereoCamera : gtsam::Value {
|
|
|
|
|
// 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
|
|
|
|
|
void serialize() const;
|
|
|
|
|
};
|
|
|
|
|
|
2012-01-18 04:04:37 +08:00
|
|
|
//*************************************************************************
|
|
|
|
|
// inference
|
|
|
|
|
//*************************************************************************
|
2012-09-08 07:10:49 +08:00
|
|
|
#include <gtsam/inference/Permutation.h>
|
2012-06-22 07:53:49 +08:00
|
|
|
class Permutation {
|
2012-10-02 22:40:07 +08:00
|
|
|
// Standard Constructors and Named Constructors
|
|
|
|
|
Permutation();
|
|
|
|
|
Permutation(size_t nVars);
|
|
|
|
|
static gtsam::Permutation Identity(size_t nVars);
|
|
|
|
|
|
|
|
|
|
// Testable
|
|
|
|
|
void print(string s) const;
|
|
|
|
|
bool equals(const gtsam::Permutation& rhs, double tol) const;
|
|
|
|
|
|
|
|
|
|
// Standard interface
|
|
|
|
|
size_t at(size_t variable) const;
|
|
|
|
|
size_t size() const;
|
|
|
|
|
bool empty() const;
|
|
|
|
|
void resize(size_t newSize);
|
|
|
|
|
gtsam::Permutation* permute(const gtsam::Permutation& permutation) const;
|
|
|
|
|
gtsam::Permutation* inverse() const;
|
|
|
|
|
// FIXME: Cannot currently wrap std::vector
|
|
|
|
|
//static gtsam::Permutation PullToFront(const vector<size_t>& toFront, size_t size, bool filterDuplicates);
|
|
|
|
|
//static gtsam::Permutation PushToBack(const vector<size_t>& toBack, size_t size, bool filterDuplicates = false);
|
2012-06-22 07:53:49 +08:00
|
|
|
};
|
|
|
|
|
|
|
|
|
|
class IndexFactor {
|
|
|
|
|
// Standard Constructors and Named Constructors
|
2012-09-08 07:10:49 +08:00
|
|
|
IndexFactor(const gtsam::IndexFactor& f);
|
|
|
|
|
IndexFactor(const gtsam::IndexConditional& c);
|
2012-06-22 07:53:49 +08:00
|
|
|
IndexFactor();
|
|
|
|
|
IndexFactor(size_t j);
|
|
|
|
|
IndexFactor(size_t j1, size_t j2);
|
|
|
|
|
IndexFactor(size_t j1, size_t j2, size_t j3);
|
|
|
|
|
IndexFactor(size_t j1, size_t j2, size_t j3, size_t j4);
|
|
|
|
|
IndexFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5);
|
|
|
|
|
IndexFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, size_t j6);
|
|
|
|
|
// FIXME: Must wrap std::set<Index> for this to work
|
|
|
|
|
//IndexFactor(const std::set<Index>& js);
|
2012-09-08 07:10:49 +08:00
|
|
|
void permuteWithInverse(const gtsam::Permutation& inversePermutation);
|
2012-06-22 07:53:49 +08:00
|
|
|
|
|
|
|
|
// From Factor
|
|
|
|
|
size_t size() const;
|
|
|
|
|
void print(string s) const;
|
|
|
|
|
bool equals(const gtsam::IndexFactor& other, double tol) const;
|
|
|
|
|
// FIXME: Need to wrap std::vector<KeyType>
|
|
|
|
|
//std::vector<KeyType>& keys();
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
class IndexConditional {
|
|
|
|
|
// Standard Constructors and Named Constructors
|
|
|
|
|
IndexConditional();
|
|
|
|
|
IndexConditional(size_t key);
|
|
|
|
|
IndexConditional(size_t key, size_t parent);
|
|
|
|
|
IndexConditional(size_t key, size_t parent1, size_t parent2);
|
|
|
|
|
IndexConditional(size_t key, size_t parent1, size_t parent2, size_t parent3);
|
|
|
|
|
// FIXME: Must wrap std::vector<KeyType> for this to work
|
|
|
|
|
//IndexFactor(size_t key, const std::vector<KeyType>& parents);
|
|
|
|
|
//IndexConditional(const std::vector<Index>& keys, size_t nrFrontals);
|
|
|
|
|
//template<class KEYS> static shared_ptr FromKeys(const KEYS& keys, size_t nrFrontals);
|
|
|
|
|
|
|
|
|
|
// Testable
|
|
|
|
|
void print(string s) const;
|
|
|
|
|
bool equals(const gtsam::IndexConditional& other, double tol) const;
|
|
|
|
|
|
|
|
|
|
// Standard interface
|
|
|
|
|
size_t nrFrontals() const;
|
|
|
|
|
size_t nrParents() const;
|
|
|
|
|
gtsam::IndexFactor* toFactor() const;
|
2012-09-08 07:10:49 +08:00
|
|
|
|
|
|
|
|
//Advanced interface
|
|
|
|
|
void permuteWithInverse(const gtsam::Permutation& inversePermutation);
|
2012-06-22 07:53:49 +08:00
|
|
|
};
|
|
|
|
|
|
2012-09-15 03:59:10 +08:00
|
|
|
#include <gtsam/inference/BayesNet.h>
|
|
|
|
|
template<CONDITIONAL>
|
|
|
|
|
virtual class BayesNet {
|
2012-10-02 22:40:07 +08:00
|
|
|
// Testable
|
|
|
|
|
void print(string s) const;
|
|
|
|
|
bool equals(const This& other, double tol) const;
|
2012-09-15 03:59:10 +08:00
|
|
|
|
2012-10-02 22:40:07 +08:00
|
|
|
// Standard interface
|
|
|
|
|
size_t size() const;
|
|
|
|
|
void printStats(string s) const;
|
2012-12-18 06:30:54 +08:00
|
|
|
void saveGraph(string s) const;
|
2012-10-02 22:40:07 +08:00
|
|
|
CONDITIONAL* front() const;
|
|
|
|
|
CONDITIONAL* back() const;
|
|
|
|
|
void push_back(CONDITIONAL* conditional);
|
|
|
|
|
void push_front(CONDITIONAL* conditional);
|
|
|
|
|
void push_back(This& conditional);
|
|
|
|
|
void push_front(This& conditional);
|
|
|
|
|
void pop_front();
|
|
|
|
|
void permuteWithInverse(const gtsam::Permutation& inversePermutation);
|
2012-09-15 03:59:10 +08:00
|
|
|
};
|
|
|
|
|
|
|
|
|
|
#include <gtsam/inference/BayesTree.h>
|
|
|
|
|
template<CONDITIONAL, CLIQUE>
|
|
|
|
|
virtual class BayesTree {
|
|
|
|
|
|
|
|
|
|
//Constructors
|
|
|
|
|
BayesTree();
|
|
|
|
|
|
|
|
|
|
// Testable
|
|
|
|
|
void print(string s);
|
|
|
|
|
bool equals(const This& other, double tol) const;
|
|
|
|
|
|
|
|
|
|
//Standard Interface
|
2012-10-02 22:40:07 +08:00
|
|
|
//size_t findParentClique(const gtsam::IndexVector& parents) const;
|
2012-09-15 03:59:10 +08:00
|
|
|
size_t size();
|
2013-05-23 01:27:42 +08:00
|
|
|
size_t nrNodes() const;
|
2012-09-15 04:52:22 +08:00
|
|
|
void saveGraph(string s) const;
|
2012-09-15 03:59:10 +08:00
|
|
|
CLIQUE* root() const;
|
|
|
|
|
void clear();
|
2012-10-09 08:12:51 +08:00
|
|
|
void deleteCachedShortcuts();
|
2012-09-15 03:59:10 +08:00
|
|
|
void insert(const CLIQUE* subtree);
|
2012-10-02 22:40:07 +08:00
|
|
|
size_t numCachedSeparatorMarginals() const;
|
2013-02-05 01:55:55 +08:00
|
|
|
CLIQUE* clique(size_t j) const;
|
2012-09-15 03:59:10 +08:00
|
|
|
};
|
|
|
|
|
|
|
|
|
|
template<CONDITIONAL>
|
|
|
|
|
virtual class BayesTreeClique {
|
|
|
|
|
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;
|
2012-10-02 22:40:07 +08:00
|
|
|
size_t numCachedSeparatorMarginals() const;
|
2012-09-15 03:59:10 +08:00
|
|
|
|
|
|
|
|
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(); }
|
|
|
|
|
|
|
|
|
|
void permuteWithInverse(const gtsam::Permutation& inversePermutation);
|
|
|
|
|
|
|
|
|
|
// 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;
|
|
|
|
|
|
2012-10-09 08:12:51 +08:00
|
|
|
void deleteCachedShortcuts();
|
2012-09-15 03:59:10 +08:00
|
|
|
};
|
|
|
|
|
|
2012-06-22 07:53:49 +08:00
|
|
|
#include <gtsam/inference/SymbolicFactorGraph.h>
|
2012-09-15 03:59:10 +08:00
|
|
|
typedef gtsam::BayesNet<gtsam::IndexConditional> SymbolicBayesNetBase;
|
2012-12-18 06:30:54 +08:00
|
|
|
|
2012-09-15 03:59:10 +08:00
|
|
|
virtual class SymbolicBayesNet : gtsam::SymbolicBayesNetBase {
|
2012-06-22 07:53:49 +08:00
|
|
|
// Standard Constructors and Named Constructors
|
|
|
|
|
SymbolicBayesNet();
|
|
|
|
|
SymbolicBayesNet(const gtsam::SymbolicBayesNet& bn);
|
|
|
|
|
SymbolicBayesNet(const gtsam::IndexConditional* conditional);
|
|
|
|
|
|
|
|
|
|
// Standard interface
|
2012-09-08 07:10:49 +08:00
|
|
|
//TODO:Throws parse error
|
|
|
|
|
//void push_back(const gtsam::SymbolicBayesNet bn);
|
|
|
|
|
//TODO:Throws parse error
|
|
|
|
|
//void push_front(const gtsam::SymbolicBayesNet bn);
|
|
|
|
|
|
|
|
|
|
//Advanced Interface
|
2012-06-22 07:53:49 +08:00
|
|
|
void pop_front();
|
|
|
|
|
void permuteWithInverse(const gtsam::Permutation& inversePermutation);
|
|
|
|
|
};
|
|
|
|
|
|
2012-09-15 04:52:22 +08:00
|
|
|
typedef gtsam::BayesTreeClique<gtsam::IndexConditional> SymbolicBayesTreeClique;
|
|
|
|
|
typedef gtsam::BayesTree<gtsam::IndexConditional, gtsam::SymbolicBayesTreeClique> SymbolicBayesTreeBase;
|
|
|
|
|
virtual class SymbolicBayesTree : gtsam::SymbolicBayesTreeBase {
|
2012-06-22 07:53:49 +08:00
|
|
|
// Standard Constructors and Named Constructors
|
|
|
|
|
SymbolicBayesTree();
|
|
|
|
|
SymbolicBayesTree(const gtsam::SymbolicBayesNet& bn);
|
|
|
|
|
SymbolicBayesTree(const gtsam::SymbolicBayesTree& other);
|
|
|
|
|
// FIXME: wrap needs to understand std::list
|
|
|
|
|
//SymbolicBayesTree(const gtsam::SymbolicBayesNet& bayesNet, std::list<gtsam::SymbolicBayesTree> subtrees);
|
|
|
|
|
|
|
|
|
|
// Standard interface
|
2012-09-08 07:10:49 +08:00
|
|
|
size_t findParentClique(const gtsam::IndexConditional& parents) const;
|
2012-06-22 07:53:49 +08:00
|
|
|
// TODO: There are many other BayesTree member functions which might be of use
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
class SymbolicFactorGraph {
|
|
|
|
|
// Standard Constructors and Named Constructors
|
|
|
|
|
SymbolicFactorGraph();
|
|
|
|
|
SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet);
|
|
|
|
|
SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree);
|
|
|
|
|
|
|
|
|
|
// From FactorGraph
|
|
|
|
|
void push_back(gtsam::IndexFactor* factor);
|
|
|
|
|
void print(string s) const;
|
|
|
|
|
bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const;
|
|
|
|
|
size_t size() const;
|
2013-06-07 02:07:55 +08:00
|
|
|
bool exists(size_t i) const;
|
2012-06-22 07:53:49 +08:00
|
|
|
|
|
|
|
|
// Standard interface
|
|
|
|
|
// FIXME: Must wrap FastSet<Index> for this to work
|
|
|
|
|
//FastSet<Index> keys() const;
|
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);
|
|
|
|
|
|
|
|
|
|
pair<gtsam::IndexConditional*, gtsam::SymbolicFactorGraph> eliminateFrontals(size_t nFrontals) const;
|
2012-10-29 04:48:17 +08:00
|
|
|
pair<gtsam::IndexConditional*, gtsam::SymbolicFactorGraph> eliminateOne(size_t j);
|
2012-06-22 07:53:49 +08:00
|
|
|
};
|
|
|
|
|
|
2012-07-18 02:30:02 +08:00
|
|
|
#include <gtsam/inference/SymbolicSequentialSolver.h>
|
2012-06-22 07:53:49 +08:00
|
|
|
class SymbolicSequentialSolver {
|
|
|
|
|
// Standard Constructors and Named Constructors
|
|
|
|
|
SymbolicSequentialSolver(const gtsam::SymbolicFactorGraph& factorGraph);
|
|
|
|
|
SymbolicSequentialSolver(const gtsam::SymbolicFactorGraph* factorGraph, const gtsam::VariableIndex* variableIndex);
|
|
|
|
|
|
|
|
|
|
// Testable
|
|
|
|
|
void print(string s) const;
|
|
|
|
|
bool equals(const gtsam::SymbolicSequentialSolver& rhs, double tol) const;
|
|
|
|
|
|
|
|
|
|
// Standard interface
|
|
|
|
|
gtsam::SymbolicBayesNet* eliminate() const;
|
2012-09-08 07:10:49 +08:00
|
|
|
gtsam::IndexFactor* marginalFactor(size_t j) const;
|
2012-06-22 07:53:49 +08:00
|
|
|
};
|
|
|
|
|
|
2012-07-18 02:30:02 +08:00
|
|
|
#include <gtsam/inference/SymbolicMultifrontalSolver.h>
|
2012-06-22 07:53:49 +08:00
|
|
|
class SymbolicMultifrontalSolver {
|
|
|
|
|
// Standard Constructors and Named Constructors
|
|
|
|
|
SymbolicMultifrontalSolver(const gtsam::SymbolicFactorGraph& factorGraph);
|
|
|
|
|
SymbolicMultifrontalSolver(const gtsam::SymbolicFactorGraph* factorGraph, const gtsam::VariableIndex* variableIndex);
|
|
|
|
|
|
|
|
|
|
// Testable
|
|
|
|
|
void print(string s) const;
|
|
|
|
|
bool equals(const gtsam::SymbolicMultifrontalSolver& rhs, double tol) const;
|
|
|
|
|
|
|
|
|
|
// Standard interface
|
|
|
|
|
gtsam::SymbolicBayesTree* eliminate() const;
|
2012-09-08 07:10:49 +08:00
|
|
|
gtsam::IndexFactor* marginalFactor(size_t j) const;
|
2012-06-22 07:53:49 +08:00
|
|
|
};
|
|
|
|
|
|
|
|
|
|
#include <gtsam/inference/SymbolicFactorGraph.h>
|
|
|
|
|
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);
|
|
|
|
|
VariableIndex(const gtsam::SymbolicFactorGraph& factorGraph, size_t nVariables);
|
2012-09-08 07:10:49 +08:00
|
|
|
VariableIndex(const gtsam::GaussianFactorGraph& factorGraph);
|
|
|
|
|
VariableIndex(const gtsam::GaussianFactorGraph& factorGraph, size_t nVariables);
|
|
|
|
|
VariableIndex(const gtsam::NonlinearFactorGraph& factorGraph);
|
|
|
|
|
VariableIndex(const gtsam::NonlinearFactorGraph& factorGraph, size_t nVariables);
|
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-07-01 01:36:25 +08:00
|
|
|
void permuteInPlace(const gtsam::Permutation& permutation);
|
2012-06-22 07:53:49 +08:00
|
|
|
};
|
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
|
|
|
};
|
|
|
|
|
|
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);
|
|
|
|
|
VectorValues(size_t nVars, size_t varDim);
|
|
|
|
|
|
|
|
|
|
//Named Constructors
|
|
|
|
|
static gtsam::VectorValues Zero(const gtsam::VectorValues& model);
|
|
|
|
|
static gtsam::VectorValues Zero(size_t nVars, size_t varDim);
|
|
|
|
|
static gtsam::VectorValues SameStructure(const gtsam::VectorValues& other);
|
|
|
|
|
|
|
|
|
|
//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);
|
2012-12-19 08:59:03 +08:00
|
|
|
Vector asVector() const;
|
2012-10-02 22:40:07 +08:00
|
|
|
Vector at(size_t j) const;
|
|
|
|
|
|
|
|
|
|
//Advanced Interface
|
|
|
|
|
void resizeLike(const gtsam::VectorValues& other);
|
|
|
|
|
void resize(size_t nVars, size_t varDim);
|
|
|
|
|
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;
|
|
|
|
|
gtsam::VectorValues scale(double a, const gtsam::VectorValues& c) const;
|
|
|
|
|
|
2012-09-08 07:10:49 +08:00
|
|
|
//FIXME: Parse errors with vector()
|
2012-10-02 22:40:07 +08:00
|
|
|
//const Vector& vector() const;
|
|
|
|
|
//Vector& vector();
|
|
|
|
|
bool hasSameStructure(const gtsam::VectorValues& other) const;
|
2012-09-08 07:10:49 +08:00
|
|
|
double dot(const gtsam::VectorValues& V) const;
|
|
|
|
|
double norm() const;
|
2013-06-20 01:50:05 +08:00
|
|
|
|
|
|
|
|
// enabling serialization functionality
|
|
|
|
|
void serialize() const;
|
2011-10-21 13:07:35 +08:00
|
|
|
};
|
|
|
|
|
|
2011-10-23 03:42:02 +08:00
|
|
|
class GaussianConditional {
|
2012-09-08 07:10:49 +08:00
|
|
|
//Constructors
|
2012-10-02 22:40:07 +08:00
|
|
|
GaussianConditional(size_t key, Vector d, Matrix R, Vector sigmas);
|
|
|
|
|
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
|
|
|
|
|
Vector sigmas);
|
|
|
|
|
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
|
|
|
|
|
size_t name2, Matrix T, Vector sigmas);
|
|
|
|
|
|
|
|
|
|
//Standard Interface
|
|
|
|
|
void print(string s) const;
|
|
|
|
|
bool equals(const gtsam::GaussianConditional &cg, double tol) const;
|
|
|
|
|
size_t dim() const;
|
|
|
|
|
|
|
|
|
|
//Advanced Interface
|
2012-10-09 06:40:37 +08:00
|
|
|
Matrix information() const;
|
|
|
|
|
Matrix augmentedInformation() const;
|
|
|
|
|
gtsam::JacobianFactor* toFactor() const;
|
|
|
|
|
void solveInPlace(gtsam::VectorValues& x) const;
|
|
|
|
|
void solveTransposeInPlace(gtsam::VectorValues& gy) const;
|
|
|
|
|
void scaleFrontalsBySigma(gtsam::VectorValues& gy) const;
|
2013-06-20 01:50:05 +08:00
|
|
|
|
|
|
|
|
// enabling serialization functionality
|
|
|
|
|
void serialize() const;
|
2011-10-23 03:42:02 +08:00
|
|
|
};
|
|
|
|
|
|
2012-01-28 06:20:43 +08:00
|
|
|
class GaussianDensity {
|
2012-09-08 07:10:49 +08:00
|
|
|
//Constructors
|
2012-10-02 22:40:07 +08:00
|
|
|
GaussianDensity(size_t key, Vector d, Matrix R, Vector sigmas);
|
2012-09-08 07:10:49 +08:00
|
|
|
|
2012-10-02 22:40:07 +08:00
|
|
|
//Standard Interface
|
|
|
|
|
void print(string s) const;
|
|
|
|
|
Vector mean() const;
|
|
|
|
|
Matrix information() const;
|
|
|
|
|
Matrix covariance() const;
|
2012-01-28 06:20:43 +08:00
|
|
|
};
|
|
|
|
|
|
2012-09-15 03:59:10 +08:00
|
|
|
typedef gtsam::BayesNet<gtsam::GaussianConditional> GaussianBayesNetBase;
|
|
|
|
|
virtual class GaussianBayesNet : gtsam::GaussianBayesNetBase {
|
2012-09-08 07:10:49 +08:00
|
|
|
//Constructors
|
2012-10-02 22:40:07 +08:00
|
|
|
GaussianBayesNet();
|
|
|
|
|
GaussianBayesNet(const gtsam::GaussianConditional* conditional);
|
2011-10-23 03:42:02 +08:00
|
|
|
};
|
|
|
|
|
|
2012-09-08 07:10:49 +08:00
|
|
|
//Non-Class methods found in GaussianBayesNet.h
|
|
|
|
|
//FIXME: No MATLAB documentation for these functions!
|
2013-02-05 05:00:43 +08:00
|
|
|
//gtsam::GaussianBayesNet scalarGaussian(size_t key, double mu, double sigma);
|
|
|
|
|
//gtsam::GaussianBayesNet simpleGaussian(size_t key, const Vector& mu, double sigma);
|
|
|
|
|
//void push_front(gtsam::GaussianBayesNet& bn, size_t key, Vector d, Matrix R, size_t name1, Matrix S, Vector sigmas);
|
|
|
|
|
//void push_front(gtsam::GaussianBayesNet& bn, size_t key, Vector d, Matrix R, size_t name1, Matrix S, size_t name2, Matrix T, Vector sigmas);
|
|
|
|
|
//gtsam::VectorValues* allocateVectorValues(const gtsam::GaussianBayesNet& bn);
|
|
|
|
|
//gtsam::VectorValues optimize(const gtsam::GaussianBayesNet& bn);
|
|
|
|
|
//void optimizeInPlace(const gtsam::GaussianBayesNet& bn, gtsam::VectorValues& x);
|
|
|
|
|
//gtsam::VectorValues optimizeGradientSearch(const gtsam::GaussianBayesNet& bn);
|
|
|
|
|
//void optimizeGradientSearchInPlace(const gtsam::GaussianBayesNet& bn, gtsam::VectorValues& grad);
|
|
|
|
|
//gtsam::VectorValues backSubstitute(const gtsam::GaussianBayesNet& bn, const gtsam::VectorValues& gx);
|
|
|
|
|
//gtsam::VectorValues backSubstituteTranspose(const gtsam::GaussianBayesNet& bn, const gtsam::VectorValues& gx);
|
|
|
|
|
//pair<Matrix, Vector> matrix(const gtsam::GaussianBayesNet& bn);
|
2012-09-08 07:10:49 +08:00
|
|
|
double determinant(const gtsam::GaussianBayesNet& bayesNet);
|
2013-02-05 05:00:43 +08:00
|
|
|
//gtsam::VectorValues gradient(const gtsam::GaussianBayesNet& bayesNet, const gtsam::VectorValues& x0);
|
|
|
|
|
//void gradientAtZero(const gtsam::GaussianBayesNet& bayesNet, const gtsam::VectorValues& g);
|
2012-09-08 07:10:49 +08:00
|
|
|
|
2012-09-15 04:52:22 +08:00
|
|
|
#include <gtsam/linear/GaussianBayesTree.h>
|
|
|
|
|
typedef gtsam::BayesTreeClique<gtsam::GaussianConditional> GaussianBayesTreeClique;
|
|
|
|
|
typedef gtsam::BayesTree<gtsam::GaussianConditional, gtsam::GaussianBayesTreeClique> GaussianBayesTreeBase;
|
|
|
|
|
virtual class GaussianBayesTree : gtsam::GaussianBayesTreeBase {
|
|
|
|
|
// Standard Constructors and Named Constructors
|
2012-10-02 22:40:07 +08:00
|
|
|
GaussianBayesTree();
|
|
|
|
|
GaussianBayesTree(const gtsam::GaussianBayesNet& bn);
|
|
|
|
|
GaussianBayesTree(const gtsam::GaussianBayesNet& other);
|
2013-06-21 03:44:50 +08:00
|
|
|
bool equals(const gtsam::GaussianBayesTree& other, double tol) const;
|
|
|
|
|
void print(string s);
|
|
|
|
|
size_t size() const;
|
|
|
|
|
size_t nrNodes() const;
|
|
|
|
|
bool empty() const;
|
|
|
|
|
gtsam::GaussianBayesTreeClique* root() const;
|
|
|
|
|
gtsam::GaussianBayesTreeClique* clique(size_t j) const;
|
|
|
|
|
size_t numCachedSeparatorMarginals() const;
|
|
|
|
|
void saveGraph(string s) const;
|
2012-09-15 04:52:22 +08:00
|
|
|
};
|
|
|
|
|
|
2013-01-29 03:26:52 +08:00
|
|
|
// namespace functions for GaussianBayesTree
|
|
|
|
|
gtsam::VectorValues optimize(const gtsam::GaussianBayesTree& bayesTree);
|
|
|
|
|
gtsam::VectorValues optimizeGradientSearch(const gtsam::GaussianBayesTree& bayesTree);
|
|
|
|
|
gtsam::VectorValues gradient(const gtsam::GaussianBayesTree& bayesTree, const gtsam::VectorValues& x0);
|
2013-02-05 01:55:55 +08:00
|
|
|
gtsam::VectorValues* allocateVectorValues(const gtsam::GaussianBayesTree& bt);
|
2013-02-05 05:00:43 +08:00
|
|
|
double determinant(const gtsam::GaussianBayesTree& bayesTree);
|
2013-01-29 03:26:52 +08:00
|
|
|
|
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;
|
|
|
|
|
gtsam::GaussianFactor* negate() const;
|
2012-10-09 06:40:37 +08:00
|
|
|
Matrix augmentedInformation() const;
|
|
|
|
|
Matrix information() const;
|
2012-10-02 22:40:07 +08:00
|
|
|
size_t size() const;
|
2011-10-23 03:42:02 +08:00
|
|
|
};
|
2011-10-21 13:07:35 +08:00
|
|
|
|
2012-07-12 02:18:57 +08:00
|
|
|
virtual class JacobianFactor : gtsam::GaussianFactor {
|
2012-09-08 07:10:49 +08:00
|
|
|
//Constructors
|
2012-10-02 22:40:07 +08:00
|
|
|
JacobianFactor();
|
|
|
|
|
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);
|
|
|
|
|
JacobianFactor(const gtsam::GaussianFactor& factor);
|
|
|
|
|
|
|
|
|
|
//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
|
|
|
bool empty() const;
|
|
|
|
|
Matrix getA() const;
|
|
|
|
|
Vector getb() const;
|
|
|
|
|
size_t rows() const;
|
|
|
|
|
size_t cols() const;
|
|
|
|
|
size_t numberOfRows() const;
|
|
|
|
|
bool isConstrained() const;
|
|
|
|
|
pair<Matrix, Vector> matrix() const;
|
|
|
|
|
Matrix matrix_augmented() const;
|
|
|
|
|
|
|
|
|
|
gtsam::GaussianConditional* eliminateFirst();
|
|
|
|
|
gtsam::GaussianConditional* eliminate(size_t nrFrontals);
|
|
|
|
|
gtsam::GaussianFactor* negate() const;
|
|
|
|
|
void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const;
|
|
|
|
|
gtsam::JacobianFactor whiten() const;
|
|
|
|
|
gtsam::GaussianConditional* eliminateFirst();
|
|
|
|
|
gtsam::GaussianConditional* eliminate(size_t nFrontals);
|
|
|
|
|
gtsam::GaussianConditional* splitConditional(size_t nFrontals);
|
|
|
|
|
|
|
|
|
|
void setModel(bool anyConstrained, const Vector& sigmas);
|
|
|
|
|
void assertInvariants() const;
|
|
|
|
|
|
|
|
|
|
//gtsam::SharedDiagonal& get_model();
|
2013-06-20 01:50:05 +08:00
|
|
|
|
|
|
|
|
// enabling serialization functionality
|
|
|
|
|
void serialize() const;
|
2011-10-23 03:42:02 +08:00
|
|
|
};
|
2011-10-21 13:07:35 +08:00
|
|
|
|
2012-07-12 02:18:57 +08:00
|
|
|
virtual class HessianFactor : gtsam::GaussianFactor {
|
2012-09-08 07:10:49 +08:00
|
|
|
//Constructors
|
2012-10-02 22:40:07 +08:00
|
|
|
HessianFactor(const gtsam::HessianFactor& gf);
|
|
|
|
|
HessianFactor();
|
|
|
|
|
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);
|
|
|
|
|
HessianFactor(const gtsam::GaussianConditional& cg);
|
|
|
|
|
HessianFactor(const gtsam::GaussianFactor& factor);
|
|
|
|
|
|
|
|
|
|
//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;
|
|
|
|
|
|
|
|
|
|
//Advanced Interface
|
|
|
|
|
void partialCholesky(size_t nrFrontals);
|
2012-10-09 06:40:37 +08:00
|
|
|
gtsam::GaussianConditional* splitEliminatedFactor(size_t nrFrontals);
|
|
|
|
|
void assertInvariants() const;
|
2013-06-20 01:50:05 +08:00
|
|
|
|
|
|
|
|
// enabling serialization functionality
|
|
|
|
|
void serialize() const;
|
2012-01-20 12:35:12 +08:00
|
|
|
};
|
|
|
|
|
|
2011-10-23 03:42:02 +08:00
|
|
|
class GaussianFactorGraph {
|
2012-10-02 22:40:07 +08:00
|
|
|
GaussianFactorGraph();
|
|
|
|
|
GaussianFactorGraph(const gtsam::GaussianBayesNet& CBN);
|
|
|
|
|
|
|
|
|
|
// 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-06-07 02:07:55 +08:00
|
|
|
bool exists(size_t idx) const;
|
2012-10-02 22:40:07 +08:00
|
|
|
|
|
|
|
|
// Inference
|
|
|
|
|
pair<gtsam::GaussianConditional*, gtsam::GaussianFactorGraph> eliminateFrontals(size_t nFrontals) const;
|
2012-10-28 03:45:28 +08:00
|
|
|
pair<gtsam::GaussianConditional*, gtsam::GaussianFactorGraph> eliminateOne(size_t j) const;
|
2012-10-02 22:40:07 +08:00
|
|
|
|
|
|
|
|
// Building the graph
|
|
|
|
|
void push_back(gtsam::GaussianFactor* factor);
|
|
|
|
|
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-09-08 07:10:49 +08:00
|
|
|
//Permutations
|
2012-10-02 22:40:07 +08:00
|
|
|
void permuteWithInverse(const gtsam::Permutation& inversePermutation);
|
2012-09-08 07:10:49 +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
|
|
|
|
2012-10-02 22:40:07 +08:00
|
|
|
// combining
|
|
|
|
|
static gtsam::GaussianFactorGraph combine2(
|
|
|
|
|
const gtsam::GaussianFactorGraph& lfg1,
|
|
|
|
|
const gtsam::GaussianFactorGraph& lfg2);
|
|
|
|
|
void combine(const gtsam::GaussianFactorGraph& lfg);
|
2012-01-20 12:35:12 +08:00
|
|
|
|
2012-10-02 22:40:07 +08:00
|
|
|
// Conversion to matrices
|
|
|
|
|
Matrix sparseJacobian_() const;
|
|
|
|
|
Matrix augmentedJacobian() const;
|
|
|
|
|
pair<Matrix,Vector> jacobian() const;
|
|
|
|
|
Matrix augmentedHessian() const;
|
|
|
|
|
pair<Matrix,Vector> hessian() const;
|
2013-06-20 01:50:05 +08:00
|
|
|
|
|
|
|
|
// enabling serialization functionality
|
|
|
|
|
void serialize() const;
|
2011-10-23 03:42:02 +08:00
|
|
|
};
|
2011-10-14 02:41:56 +08:00
|
|
|
|
2012-09-08 07:10:49 +08:00
|
|
|
//Non-Class functions in GaussianFactorGraph.h
|
|
|
|
|
/*void multiplyInPlace(const gtsam::GaussianFactorGraph& fg, const gtsam::VectorValues& x, gtsam::Errors& e);
|
|
|
|
|
gtsam::VectorValues gradient(const gtsam::GaussianFactorGraph& fg, const gtsam::VectorValues& x0);
|
|
|
|
|
void gradientAtZero(const gtsam::GaussianFactorGraph& fg, gtsam::VectorValues& g);
|
|
|
|
|
void residual(const gtsam::GaussianFactorGraph& fg, const gtsam::VectorValues& x, gtsam::VectorValues& r);
|
|
|
|
|
void multiply(const gtsam::GaussianFactorGraph& fg, const gtsam::VectorValues& x, gtsam::VectorValues& r);
|
|
|
|
|
void transposeMultiply(const gtsam::GaussianFactorGraph& fg, const gtsam::VectorValues& x, gtsam::VectorValues& r);*/
|
|
|
|
|
|
|
|
|
|
class Errors {
|
|
|
|
|
//Constructors
|
|
|
|
|
Errors();
|
|
|
|
|
Errors(const gtsam::VectorValues& V);
|
|
|
|
|
|
|
|
|
|
//Testable
|
|
|
|
|
void print(string s);
|
|
|
|
|
bool equals(const gtsam::Errors& expected, double tol) const;
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
//Non-Class functions for Errors
|
|
|
|
|
//double dot(const gtsam::Errors& A, const gtsam::Errors& b);
|
|
|
|
|
|
2012-09-11 01:00:02 +08:00
|
|
|
virtual class GaussianISAM : gtsam::GaussianBayesTree {
|
2012-09-08 07:10:49 +08:00
|
|
|
//Constructor
|
2012-06-07 13:19:43 +08:00
|
|
|
GaussianISAM();
|
2012-09-08 07:10:49 +08:00
|
|
|
|
|
|
|
|
//Standard Interface
|
2012-06-07 21:17:46 +08:00
|
|
|
void saveGraph(string s) const;
|
2012-06-07 13:19:43 +08:00
|
|
|
gtsam::GaussianFactor* marginalFactor(size_t j) const;
|
|
|
|
|
gtsam::GaussianBayesNet* marginalBayesNet(size_t key) const;
|
|
|
|
|
Matrix marginalCovariance(size_t key) const;
|
|
|
|
|
gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const;
|
2012-09-08 07:10:49 +08:00
|
|
|
void clear();
|
2012-06-07 13:19:43 +08:00
|
|
|
};
|
|
|
|
|
|
2012-07-18 02:30:02 +08:00
|
|
|
#include <gtsam/linear/GaussianSequentialSolver.h>
|
2011-12-10 00:36:50 +08:00
|
|
|
class GaussianSequentialSolver {
|
2012-09-08 07:10:49 +08:00
|
|
|
//Constructors
|
2012-10-02 22:40:07 +08:00
|
|
|
GaussianSequentialSolver(const gtsam::GaussianFactorGraph& graph,
|
|
|
|
|
bool useQR);
|
2012-09-08 07:10:49 +08:00
|
|
|
|
2012-10-02 22:40:07 +08:00
|
|
|
//Standard Interface
|
|
|
|
|
void replaceFactors(const gtsam::GaussianFactorGraph* factorGraph);
|
|
|
|
|
gtsam::GaussianBayesNet* eliminate() const;
|
|
|
|
|
gtsam::VectorValues* optimize() const;
|
|
|
|
|
gtsam::GaussianFactor* marginalFactor(size_t j) const;
|
|
|
|
|
Matrix marginalCovariance(size_t j) const;
|
2011-12-10 00:36:50 +08:00
|
|
|
};
|
|
|
|
|
|
2012-10-28 03:45:30 +08:00
|
|
|
#include <gtsam/linear/GaussianMultifrontalSolver.h>
|
|
|
|
|
class GaussianMultifrontalSolver {
|
|
|
|
|
//Constructors
|
|
|
|
|
GaussianMultifrontalSolver(const gtsam::GaussianFactorGraph& graph,
|
|
|
|
|
bool useQR);
|
|
|
|
|
|
|
|
|
|
//Standard Interface
|
|
|
|
|
void replaceFactors(const gtsam::GaussianFactorGraph* factorGraph);
|
|
|
|
|
gtsam::GaussianBayesTree* eliminate() const;
|
|
|
|
|
gtsam::VectorValues* optimize() const;
|
|
|
|
|
gtsam::GaussianFactor* marginalFactor(size_t j) const;
|
|
|
|
|
Matrix marginalCovariance(size_t j) const;
|
|
|
|
|
};
|
|
|
|
|
|
2012-09-04 05:50:22 +08:00
|
|
|
#include <gtsam/linear/IterativeSolver.h>
|
|
|
|
|
virtual class IterativeOptimizationParameters {
|
|
|
|
|
string getKernel() const ;
|
|
|
|
|
string getVerbosity() const;
|
|
|
|
|
void setKernel(string s) ;
|
|
|
|
|
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();
|
|
|
|
|
size_t getMinIterations() const ;
|
|
|
|
|
size_t getMaxIterations() const ;
|
|
|
|
|
size_t getReset() const;
|
|
|
|
|
double getEpsilon_rel() const;
|
|
|
|
|
double getEpsilon_abs() const;
|
|
|
|
|
|
|
|
|
|
void setMinIterations(size_t value);
|
|
|
|
|
void setMaxIterations(size_t value);
|
|
|
|
|
void setReset(size_t value);
|
|
|
|
|
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
|
|
|
};
|
|
|
|
|
|
|
|
|
|
class SubgraphSolver {
|
|
|
|
|
SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters);
|
2012-09-08 07:10:49 +08:00
|
|
|
SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters ¶meters);
|
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
|
|
|
|
|
//*************************************************************************
|
|
|
|
|
|
2012-07-18 02:30:02 +08:00
|
|
|
#include <gtsam/nonlinear/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
|
|
|
|
|
void printKeySet(const gtsam::KeySet& keys);
|
|
|
|
|
void printKeySet(const gtsam::KeySet& keys, string s);
|
|
|
|
|
|
|
|
|
|
#include <gtsam/nonlinear/LabeledSymbol.h>
|
|
|
|
|
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;
|
|
|
|
|
|
|
|
|
|
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);
|
|
|
|
|
|
2012-07-18 02:30:02 +08:00
|
|
|
#include <gtsam/nonlinear/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();
|
|
|
|
|
|
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;
|
2013-02-06 05:52:50 +08:00
|
|
|
size_t key(size_t index) const;
|
2012-06-22 07:53:49 +08:00
|
|
|
bool exists(size_t key) const;
|
|
|
|
|
void insert(size_t key, size_t order);
|
|
|
|
|
void push_back(size_t key);
|
2013-02-06 05:52:50 +08:00
|
|
|
void permuteInPlace(const gtsam::Permutation& permutation);
|
|
|
|
|
void permuteInPlace(const gtsam::Permutation& selector, const gtsam::Permutation& permutation);
|
2013-06-20 01:50:05 +08:00
|
|
|
|
|
|
|
|
// enabling serialization functionality
|
|
|
|
|
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;
|
|
|
|
|
bool exists(size_t idx) const;
|
2012-10-02 22:40:07 +08:00
|
|
|
void push_back(const gtsam::NonlinearFactorGraph& factors);
|
|
|
|
|
|
|
|
|
|
// NonlinearFactorGraph
|
|
|
|
|
double error(const gtsam::Values& values) const;
|
|
|
|
|
double probPrime(const gtsam::Values& values) const;
|
|
|
|
|
void add(const gtsam::NonlinearFactor* factor);
|
|
|
|
|
gtsam::Ordering* orderingCOLAMD(const gtsam::Values& values) const;
|
|
|
|
|
// Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map<gtsam::Key,int>& constraints) const;
|
|
|
|
|
gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values,
|
|
|
|
|
const gtsam::Ordering& ordering) const;
|
|
|
|
|
gtsam::SymbolicFactorGraph* symbolic(const gtsam::Ordering& ordering) const;
|
|
|
|
|
gtsam::NonlinearFactorGraph clone() const;
|
2013-06-20 01:50:05 +08:00
|
|
|
|
|
|
|
|
// enabling serialization functionality
|
|
|
|
|
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;
|
|
|
|
|
gtsam::GaussianFactor* linearize(const gtsam::Values& c, const gtsam::Ordering& ordering) const;
|
|
|
|
|
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
|
|
|
|
2012-10-02 22:40:07 +08:00
|
|
|
void insert(size_t j, const gtsam::Value& value);
|
2013-06-20 01:50:01 +08:00
|
|
|
void insert(const gtsam::Values& values);
|
|
|
|
|
void update(size_t j, const gtsam::Value& val);
|
|
|
|
|
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::Value at(size_t j) const;
|
|
|
|
|
gtsam::KeyList keys() const;
|
2012-08-08 02:21:35 +08:00
|
|
|
|
2013-06-20 01:50:01 +08:00
|
|
|
gtsam::VectorValues zeroVectors(const gtsam::Ordering& ordering) const;
|
|
|
|
|
gtsam::Ordering* orderingArbitrary(size_t firstVar) const;
|
|
|
|
|
|
|
|
|
|
gtsam::Values retract(const gtsam::VectorValues& delta, const gtsam::Ordering& ordering) const;
|
|
|
|
|
gtsam::VectorValues localCoordinates(const gtsam::Values& cp, const gtsam::Ordering& ordering) const;
|
|
|
|
|
void localCoordinates(const gtsam::Values& cp, const gtsam::Ordering& ordering, gtsam::VectorValues& delta) const;
|
2012-08-08 02:21:35 +08:00
|
|
|
|
2013-06-20 01:50:01 +08:00
|
|
|
// enabling serialization functionality
|
|
|
|
|
void serialize() const;
|
2012-05-22 06:32:40 +08:00
|
|
|
};
|
|
|
|
|
|
2012-06-14 00:38:51 +08:00
|
|
|
// Actually a FastList<Key>
|
2012-07-03 01:32:47 +08:00
|
|
|
#include <gtsam/nonlinear/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);
|
2012-07-03 01:32:47 +08:00
|
|
|
};
|
|
|
|
|
|
|
|
|
|
// Actually a FastSet<Key>
|
|
|
|
|
#include <gtsam/nonlinear/Key.h>
|
|
|
|
|
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);
|
|
|
|
|
bool erase(size_t key); // returns true if value was removed
|
|
|
|
|
bool count(size_t key) const; // returns true if value exists
|
2012-06-14 00:38:51 +08:00
|
|
|
};
|
|
|
|
|
|
2012-07-03 01:32:47 +08:00
|
|
|
// Actually a FastVector<Key>
|
|
|
|
|
#include <gtsam/nonlinear/Key.h>
|
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;
|
2012-06-14 00:38:51 +08:00
|
|
|
};
|
|
|
|
|
|
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);
|
|
|
|
|
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 {
|
|
|
|
|
|
|
|
|
|
LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Ordering& ordering,
|
|
|
|
|
const gtsam::Values& linearizationPoint);
|
|
|
|
|
LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint);
|
|
|
|
|
|
|
|
|
|
LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Ordering& ordering);
|
|
|
|
|
LinearContainerFactor(gtsam::GaussianFactor* factor);
|
|
|
|
|
|
|
|
|
|
gtsam::GaussianFactor* factor() const;
|
|
|
|
|
// const boost::optional<Values>& linearizationPoint() const;
|
|
|
|
|
|
|
|
|
|
gtsam::GaussianFactor* order(const gtsam::Ordering& ordering) const;
|
|
|
|
|
gtsam::GaussianFactor* negate(const gtsam::Ordering& ordering) const;
|
|
|
|
|
gtsam::NonlinearFactor* negate() const;
|
|
|
|
|
|
|
|
|
|
bool isJacobian() const;
|
|
|
|
|
gtsam::JacobianFactor* toJacobian() const;
|
|
|
|
|
gtsam::HessianFactor* toHessian() const;
|
|
|
|
|
|
|
|
|
|
static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
|
|
|
|
|
const gtsam::Ordering& ordering, const gtsam::Values& linearizationPoint);
|
|
|
|
|
|
|
|
|
|
static gtsam::NonlinearFactorGraph convertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
|
|
|
|
|
const gtsam::Ordering& ordering);
|
|
|
|
|
|
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
|
|
|
|
|
#include <gtsam/nonlinear/summarization.h>
|
|
|
|
|
|
2013-05-08 18:14:27 +08:00
|
|
|
// Uses partial QR approach by default
|
|
|
|
|
pair<gtsam::GaussianFactorGraph,gtsam::Ordering> summarize(
|
2013-05-08 18:13:57 +08:00
|
|
|
const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values,
|
2013-05-08 18:14:27 +08:00
|
|
|
const gtsam::KeySet& saved_keys);
|
|
|
|
|
|
2013-05-08 18:13:57 +08:00
|
|
|
gtsam::NonlinearFactorGraph summarizeAsNonlinearContainer(
|
|
|
|
|
const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values,
|
2013-05-08 18:14:27 +08:00
|
|
|
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>
|
|
|
|
|
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
|
|
|
|
2012-10-02 22:40:07 +08:00
|
|
|
size_t getMaxIterations() const;
|
|
|
|
|
double getRelativeErrorTol() const;
|
|
|
|
|
double getAbsoluteErrorTol() const;
|
|
|
|
|
double getErrorTol() const;
|
|
|
|
|
string getVerbosity() const;
|
2012-07-19 07:35:36 +08:00
|
|
|
|
2012-10-02 22:40:07 +08:00
|
|
|
void setMaxIterations(size_t value);
|
|
|
|
|
void setRelativeErrorTol(double value);
|
|
|
|
|
void setAbsoluteErrorTol(double value);
|
|
|
|
|
void setErrorTol(double value);
|
|
|
|
|
void setVerbosity(string s);
|
2012-07-19 07:35:36 +08:00
|
|
|
};
|
2012-07-04 23:41:19 +08:00
|
|
|
|
2012-07-19 07:35:36 +08:00
|
|
|
#include <gtsam/nonlinear/SuccessiveLinearizationOptimizer.h>
|
|
|
|
|
virtual class SuccessiveLinearizationParams : gtsam::NonlinearOptimizerParams {
|
|
|
|
|
SuccessiveLinearizationParams();
|
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);
|
|
|
|
|
void setIterativeParams(const gtsam::SubgraphSolverParameters ¶ms);
|
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;
|
|
|
|
|
bool isCG() const;
|
2012-07-19 07:35:36 +08:00
|
|
|
};
|
|
|
|
|
|
2012-07-24 23:48:33 +08:00
|
|
|
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
|
|
|
|
virtual class GaussNewtonParams : gtsam::SuccessiveLinearizationParams {
|
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>
|
|
|
|
|
virtual class LevenbergMarquardtParams : gtsam::SuccessiveLinearizationParams {
|
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>
|
|
|
|
|
virtual class DoglegParams : gtsam::SuccessiveLinearizationParams {
|
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);
|
|
|
|
|
};
|
|
|
|
|
|
2012-09-09 03:51:23 +08:00
|
|
|
virtual class ISAM2Clique {
|
|
|
|
|
|
|
|
|
|
//Constructors
|
|
|
|
|
ISAM2Clique(const gtsam::GaussianConditional* conditional);
|
|
|
|
|
|
|
|
|
|
//Standard Interface
|
|
|
|
|
Vector gradientContribution() const;
|
|
|
|
|
gtsam::ISAM2Clique* clone() const;
|
|
|
|
|
void print(string s);
|
|
|
|
|
|
|
|
|
|
void permuteWithInverse(const gtsam::Permutation& inversePermutation);
|
|
|
|
|
};
|
|
|
|
|
|
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;
|
|
|
|
|
};
|
|
|
|
|
|
2012-09-15 03:59:10 +08:00
|
|
|
typedef gtsam::BayesTree<gtsam::GaussianConditional, gtsam::ISAM2Clique> ISAM2BayesTree;
|
2012-09-09 03:51:23 +08:00
|
|
|
virtual class ISAM2 : gtsam::ISAM2BayesTree {
|
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);
|
|
|
|
|
// 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::Ordering getOrdering() 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();
|
|
|
|
|
void addKey(size_t key);
|
|
|
|
|
void setOrdering(const gtsam::Ordering& new_ordering);
|
|
|
|
|
|
|
|
|
|
// These might be expensive as instead of a reference the wrapper will make a copy
|
|
|
|
|
gtsam::GaussianISAM bayesTree() const;
|
|
|
|
|
gtsam::Values getLinearizationPoint() const;
|
|
|
|
|
gtsam::Ordering getOrdering() 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>
|
2013-08-03 00:04:17 +08:00
|
|
|
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, 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 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
|
|
|
|
|
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>
|
2013-08-03 00:04:17 +08:00
|
|
|
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
|
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
|
|
|
|
|
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/nonlinear/NonlinearEquality.h>
|
2013-08-03 00:04:17 +08:00
|
|
|
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, 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
|
|
|
|
|
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
|
|
|
|
|
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
|
|
|
|
|
|
|
|
// enabling serialization functionality
|
|
|
|
|
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
|
|
|
|
|
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
|
|
|
|
|
void serialize() const;
|
2012-07-24 05:27:42 +08:00
|
|
|
};
|
2012-07-12 10:11:32 +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
|
|
|
|
|
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;
|
|
|
|
|
|
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-05-21 05:46:30 +08:00
|
|
|
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(string filename,
|
|
|
|
|
gtsam::noiseModel::Base* model);
|
2012-07-30 23:40:58 +08:00
|
|
|
|
2013-08-03 00:04:17 +08:00
|
|
|
//*************************************************************************
|
|
|
|
|
// Navigation
|
|
|
|
|
//*************************************************************************
|
|
|
|
|
namespace imuBias {
|
|
|
|
|
#include <gtsam/navigation/ImuBias.h>
|
|
|
|
|
|
|
|
|
|
virtual class ConstantBias : gtsam::Value {
|
|
|
|
|
// 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:03:48 +08:00
|
|
|
#include <gtsam/navigation/ImuFactor.h>
|
|
|
|
|
class ImuFactorPreintegratedMeasurements {
|
|
|
|
|
// Standard Constructor
|
|
|
|
|
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance, Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
|
|
|
|
|
ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
|
|
|
|
|
|
|
|
|
|
// Testable
|
|
|
|
|
void print(string s) const;
|
|
|
|
|
bool equals(const gtsam::ImuFactorPreintegratedMeasurements& 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);
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
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;
|
|
|
|
|
void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
|
|
|
|
|
const gtsam::imuBias::ConstantBias& bias,
|
|
|
|
|
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
|
|
|
|
|
Vector gravity, Vector omegaCoriolis) const;
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
#include <gtsam/navigation/CombinedImuFactor.h>
|
|
|
|
|
class CombinedImuFactorPreintegratedMeasurements {
|
|
|
|
|
// Standard Constructor
|
|
|
|
|
CombinedImuFactorPreintegratedMeasurements(
|
|
|
|
|
const gtsam::imuBias::ConstantBias& bias,
|
|
|
|
|
Matrix measuredAccCovariance,
|
|
|
|
|
Matrix measuredOmegaCovariance,
|
|
|
|
|
Matrix integrationErrorCovariance,
|
|
|
|
|
Matrix biasAccCovariance,
|
|
|
|
|
Matrix biasOmegaCovariance,
|
|
|
|
|
Matrix biasAccOmegaInit);
|
|
|
|
|
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);
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
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,
|
|
|
|
|
const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
|
|
|
|
|
const gtsam::noiseModel::Base* model);
|
|
|
|
|
|
|
|
|
|
// Standard Interface
|
|
|
|
|
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
|
|
|
|
|
void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
|
|
|
|
|
const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j,
|
|
|
|
|
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
|
|
|
|
|
Vector gravity, Vector omegaCoriolis) const;
|
|
|
|
|
};
|
|
|
|
|
|
2012-07-30 23:40:58 +08:00
|
|
|
//*************************************************************************
|
|
|
|
|
// Utilities
|
|
|
|
|
//*************************************************************************
|
|
|
|
|
|
|
|
|
|
namespace utilities {
|
|
|
|
|
|
|
|
|
|
#include <matlab.h>
|
|
|
|
|
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);
|
|
|
|
|
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);
|
|
|
|
|
|
|
|
|
|
} //\namespace utilities
|
|
|
|
|
|
2012-07-28 03:02:11 +08:00
|
|
|
}
|
