1 Star 0 Fork 1

zerolover / gtsam

Create your Gitee Account
Explore and code with more than 6 million developers,Free private repositories !:)
Sign up
Clone or download
gtsam.h 91.49 KB
Copy Edit Web IDE Raw Blame History
12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754175517561757175817591760176117621763176417651766176717681769177017711772177317741775177617771778177917801781178217831784178517861787178817891790179117921793179417951796179717981799180018011802180318041805180618071808180918101811181218131814181518161817181818191820182118221823182418251826182718281829183018311832183318341835183618371838183918401841184218431844184518461847184818491850185118521853185418551856185718581859186018611862186318641865186618671868186918701871187218731874187518761877187818791880188118821883188418851886188718881889189018911892189318941895189618971898189919001901190219031904190519061907190819091910191119121913191419151916191719181919192019211922192319241925192619271928192919301931193219331934193519361937193819391940194119421943194419451946194719481949195019511952195319541955195619571958195919601961196219631964196519661967196819691970197119721973197419751976197719781979198019811982198319841985198619871988198919901991199219931994199519961997199819992000200120022003200420052006200720082009201020112012201320142015201620172018201920202021202220232024202520262027202820292030203120322033203420352036203720382039204020412042204320442045204620472048204920502051205220532054205520562057205820592060206120622063206420652066206720682069207020712072207320742075207620772078207920802081208220832084208520862087208820892090209120922093209420952096209720982099210021012102210321042105210621072108210921102111211221132114211521162117211821192120212121222123212421252126212721282129213021312132213321342135213621372138213921402141214221432144214521462147214821492150215121522153215421552156215721582159216021612162216321642165216621672168216921702171217221732174217521762177217821792180218121822183218421852186218721882189219021912192219321942195219621972198219922002201220222032204220522062207220822092210221122122213221422152216221722182219222022212222222322242225222622272228222922302231223222332234223522362237223822392240224122422243224422452246224722482249225022512252225322542255225622572258225922602261226222632264226522662267226822692270227122722273227422752276227722782279228022812282228322842285228622872288228922902291229222932294229522962297229822992300230123022303230423052306230723082309231023112312231323142315231623172318231923202321232223232324232523262327232823292330233123322333233423352336233723382339234023412342234323442345234623472348234923502351235223532354235523562357235823592360236123622363236423652366236723682369237023712372237323742375237623772378237923802381238223832384238523862387238823892390239123922393239423952396239723982399240024012402240324042405240624072408240924102411241224132414241524162417241824192420242124222423242424252426242724282429243024312432243324342435243624372438243924402441244224432444244524462447244824492450245124522453245424552456245724582459246024612462246324642465246624672468246924702471247224732474247524762477247824792480248124822483248424852486248724882489249024912492249324942495249624972498249925002501250225032504250525062507250825092510251125122513251425152516251725182519252025212522252325242525252625272528252925302531253225332534253525362537253825392540254125422543254425452546254725482549255025512552255325542555255625572558255925602561256225632564256525662567256825692570257125722573257425752576257725782579258025812582258325842585258625872588258925902591259225932594259525962597259825992600260126022603260426052606260726082609261026112612261326142615261626172618261926202621262226232624262526262627262826292630263126322633263426352636263726382639264026412642264326442645264626472648264926502651265226532654265526562657265826592660266126622663
/**
* GTSAM Wrap Module Definition
*
* These are the current classes available through the matlab toolbox interface,
* add more functions/classes as they are available.
*
* Requirements:
* Classes must start with an uppercase letter
* - Can wrap a typedef
* Only one Method/Constructor per line, though methods/constructors can extend across multiple lines
* Methods can return
* - Eigen types: Matrix, Vector
* - C/C++ basic types: string, bool, size_t, int, double, char, unsigned char
* - void
* - Any class with which be copied with boost::make_shared()
* - boost::shared_ptr of any object type
* Constructors
* - Overloads are supported
* - A class with no constructors can be returned from other functions but not allocated directly in MATLAB
* Methods
* - Constness has no effect
* - Specify by-value (not reference) return types, even if C++ method returns reference
* - Must start with a letter (upper or lowercase)
* - Overloads are supported
* Static methods
* - Must start with a letter (upper or lowercase) and use the "static" keyword
* - The first letter will be made uppercase in the generated MATLAB interface
* - Overloads are supported
* Arguments to functions any of
* - Eigen types: Matrix, Vector
* - Eigen types and classes as an optionally const reference
* - C/C++ basic types: string, bool, size_t, size_t, double, char, unsigned char
* - 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
* Namespace definitions
* - Names of namespaces must start with a lowercase letter
* - start a namespace with "namespace {"
* - end a namespace with exactly "}"
* - Namespaces can be nested
* Namespace usage
* - 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"
* Includes in C++ wrappers
* - All includes will be collected and added in a single file
* - All namespaces must have angle brackets: <path>
* - No default includes will be added
* 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
* Using classes defined in other modules
* - If you are using a class 'OtherClass' not wrapped in this definition file, add "class OtherClass;" to avoid a dependency error
* Virtual inheritance
* - 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;"
* - 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).
* - Signature of clone function - will be called virtually, so must appear at least at the top of the inheritance tree
* virtual boost::shared_ptr<CLASS_NAME> clone() const;
* Class Templates
* - 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;
* - 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();'
* - 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;
* 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.
*/
/**
* Status:
* - TODO: default values for arguments
* - WORKAROUND: make multiple versions of the same function for different configurations of default arguments
* - TODO: Handle gtsam::Rot3M conversions to quaternions
* - 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
*/
namespace std {
#include <vector>
template<T>
class vector
{
//Do we need these?
//Capacity
/*size_t size() const;
size_t max_size() const;
//void resize(size_t sz);
size_t capacity() const;
bool empty() const;
void reserve(size_t n);
//Element access
T* at(size_t n);
T* front();
T* back();
//Modifiers
void assign(size_t n, const T& u);
void push_back(const T& x);
void pop_back();*/
};
//typedef std::vector
#include<list>
template<T>
class list
{
};
}
namespace gtsam {
//*************************************************************************
// base
//*************************************************************************
/** gtsam namespace functions */
bool linear_independent(Matrix A, Matrix B, double tol);
virtual class Value {
// No constructors because this is an abstract class
// Testable
void print(string s) const;
// Manifold
size_t dim() const;
};
#include <gtsam/base/deprecated/LieScalar.h>
class LieScalar {
// Standard constructors
LieScalar();
LieScalar(double d);
// Standard interface
double value() const;
// Testable
void print(string s) const;
bool equals(const gtsam::LieScalar& expected, double tol) const;
// 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;
// Manifold
size_t dim() const;
gtsam::LieScalar retract(Vector v) const;
Vector localCoordinates(const gtsam::LieScalar& t2) const;
// Lie group
static gtsam::LieScalar Expmap(Vector v);
static Vector Logmap(const gtsam::LieScalar& p);
};
#include <gtsam/base/deprecated/LieVector.h>
class LieVector {
// Standard constructors
LieVector();
LieVector(Vector v);
// Standard interface
Vector vector() const;
// Testable
void print(string s) const;
bool equals(const gtsam::LieVector& expected, double tol) const;
// 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;
// Manifold
size_t dim() const;
gtsam::LieVector retract(Vector v) const;
Vector localCoordinates(const gtsam::LieVector& t2) const;
// Lie group
static gtsam::LieVector Expmap(Vector v);
static Vector Logmap(const gtsam::LieVector& p);
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/base/deprecated/LieMatrix.h>
class LieMatrix {
// Standard constructors
LieMatrix();
LieMatrix(Matrix v);
// Standard interface
Matrix matrix() const;
// Testable
void print(string s) const;
bool equals(const gtsam::LieMatrix& expected, double tol) const;
// 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;
// Manifold
size_t dim() const;
gtsam::LieMatrix retract(Vector v) const;
Vector localCoordinates(const gtsam::LieMatrix & t2) const;
// Lie group
static gtsam::LieMatrix Expmap(Vector v);
static Vector Logmap(const gtsam::LieMatrix& p);
// enabling serialization functionality
void serialize() const;
};
//*************************************************************************
// geometry
//*************************************************************************
class Point2 {
// Standard Constructors
Point2();
Point2(double x, double y);
Point2(Vector v);
// Testable
void print(string s) const;
bool equals(const gtsam::Point2& pose, double tol) const;
// Group
static gtsam::Point2 identity();
// Standard Interface
double x() const;
double y() const;
Vector vector() const;
double distance(const gtsam::Point2& p2) const;
double norm() const;
// enabling serialization functionality
void serialize() const;
};
// std::vector<gtsam::Point2>
class Point2Vector
{
// Constructors
Point2Vector();
Point2Vector(const gtsam::Point2Vector& v);
//Capacity
size_t size() const;
size_t max_size() const;
void resize(size_t sz);
size_t capacity() const;
bool empty() const;
void reserve(size_t n);
//Element access
gtsam::Point2 at(size_t n) const;
gtsam::Point2 front() const;
gtsam::Point2 back() const;
//Modifiers
void assign(size_t n, const gtsam::Point2& u);
void push_back(const gtsam::Point2& x);
void pop_back();
};
class StereoPoint2 {
// Standard Constructors
StereoPoint2();
StereoPoint2(double uL, double uR, double v);
// 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
gtsam::StereoPoint2 retract(Vector v) const;
Vector localCoordinates(const gtsam::StereoPoint2& p) const;
// Lie Group
static gtsam::StereoPoint2 Expmap(Vector v);
static Vector Logmap(const gtsam::StereoPoint2& p);
// Standard Interface
Vector vector() const;
double uL() const;
double uR() const;
double v() const;
// enabling serialization functionality
void serialize() const;
};
class Point3 {
// Standard Constructors
Point3();
Point3(double x, double y, double z);
Point3(Vector v);
// Testable
void print(string s) const;
bool equals(const gtsam::Point3& p, double tol) const;
// Group
static gtsam::Point3 identity();
// Standard Interface
Vector vector() const;
double x() const;
double y() const;
double z() const;
// enabling serialization functionality
void serialize() const;
};
class Rot2 {
// Standard Constructors and Named Constructors
Rot2();
Rot2(double theta);
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();
gtsam::Rot2 inverse();
gtsam::Rot2 compose(const gtsam::Rot2& p2) const;
gtsam::Rot2 between(const gtsam::Rot2& p2) const;
// Manifold
gtsam::Rot2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Rot2& p) const;
// Lie Group
static gtsam::Rot2 Expmap(Vector v);
static Vector Logmap(const gtsam::Rot2& p);
// Group Action on Point2
gtsam::Point2 rotate(const gtsam::Point2& point) const;
gtsam::Point2 unrotate(const gtsam::Point2& point) const;
// Standard Interface
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;
Matrix matrix() const;
// enabling serialization functionality
void serialize() const;
};
class Rot3 {
// Standard Constructors and Named Constructors
Rot3();
Rot3(Matrix R);
static gtsam::Rot3 Rx(double t);
static gtsam::Rot3 Ry(double t);
static gtsam::Rot3 Rz(double t);
static gtsam::Rot3 RzRyRx(double x, double y, double z);
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 Rodrigues(Vector v);
// Testable
void print(string s) const;
bool equals(const gtsam::Rot3& rot, double tol) const;
// Group
static gtsam::Rot3 identity();
gtsam::Rot3 inverse() const;
gtsam::Rot3 compose(const gtsam::Rot3& p2) const;
gtsam::Rot3 between(const gtsam::Rot3& p2) const;
// Manifold
//gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options
gtsam::Rot3 retract(Vector v) const;
Vector localCoordinates(const gtsam::Rot3& p) const;
// Group Action on Point3
gtsam::Point3 rotate(const gtsam::Point3& p) const;
gtsam::Point3 unrotate(const gtsam::Point3& p) const;
// Standard Interface
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;
// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly
Vector quaternion() const;
// enabling serialization functionality
void serialize() const;
};
class Pose2 {
// Standard Constructor
Pose2();
Pose2(const gtsam::Pose2& pose);
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);
// 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
gtsam::Pose2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Pose2& p) const;
// Lie Group
static gtsam::Pose2 Expmap(Vector v);
static Vector Logmap(const gtsam::Pose2& p);
Matrix AdjointMap() const;
Vector Adjoint(const Vector& xi) const;
static Matrix wedge(double vx, double vy, double w);
// Group Actions on Point2
gtsam::Point2 transform_from(const gtsam::Point2& p) const;
gtsam::Point2 transform_to(const gtsam::Point2& p) const;
// Standard Interface
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;
Matrix matrix() const;
// enabling serialization functionality
void serialize() const;
};
class Pose3 {
// 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
gtsam::Pose3 retract(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);
Matrix AdjointMap() const;
Vector Adjoint(Vector xi) const;
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);
// enabling serialization functionality
void serialize() const;
};
// std::vector<gtsam::Pose3>
class Pose3Vector
{
Pose3Vector();
size_t size() const;
bool empty() const;
gtsam::Pose3 at(size_t n) const;
void push_back(const gtsam::Pose3& x);
};
#include <gtsam/geometry/Unit3.h>
class Unit3 {
// Standard Constructors
Unit3();
Unit3(const gtsam::Point3& pose);
// Testable
void print(string s) const;
bool equals(const gtsam::Unit3& pose, double tol) const;
// Other functionality
Matrix basis() const;
Matrix skew() const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Unit3 retract(Vector v) const;
Vector localCoordinates(const gtsam::Unit3& s) const;
};
#include <gtsam/geometry/EssentialMatrix.h>
class EssentialMatrix {
// Standard Constructors
EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
// Testable
void print(string s) const;
bool equals(const gtsam::EssentialMatrix& pose, double tol) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::EssentialMatrix retract(Vector v) const;
Vector localCoordinates(const gtsam::EssentialMatrix& s) const;
// Other methods:
gtsam::Rot3 rotation() const;
gtsam::Unit3 direction() const;
Matrix matrix() const;
double error(Vector vA, Vector vB);
};
class Cal3_S2 {
// Standard Constructors
Cal3_S2();
Cal3_S2(double fx, double fy, double s, double u0, double v0);
Cal3_S2(Vector v);
Cal3_S2(double fov, int w, int h);
// Testable
void print(string s) const;
bool equals(const gtsam::Cal3_S2& rhs, double tol) const;
// 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;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3DS2_Base.h>
virtual class Cal3DS2_Base {
// Standard Constructors
Cal3DS2_Base();
// Testable
void print(string s) const;
// Standard Interface
double fx() const;
double fy() const;
double skew() const;
double px() const;
double py() const;
double k1() const;
double k2() const;
// Action on Point2
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3DS2.h>
virtual class Cal3DS2 : gtsam::Cal3DS2_Base {
// Standard Constructors
Cal3DS2();
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2);
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2);
Cal3DS2(Vector v);
// Testable
bool equals(const gtsam::Cal3DS2& rhs, double tol) const;
// Manifold
size_t dim() const;
static size_t Dim();
gtsam::Cal3DS2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3DS2& c) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3Unified.h>
virtual class Cal3Unified : gtsam::Cal3DS2_Base {
// Standard Constructors
Cal3Unified();
Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2);
Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi);
Cal3Unified(Vector v);
// Testable
bool equals(const gtsam::Cal3Unified& rhs, double tol) const;
// Standard Interface
double xi() const;
gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const;
gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const;
// Manifold
size_t dim() const;
static size_t Dim();
gtsam::Cal3Unified retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3Unified& c) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/geometry/Cal3_S2Stereo.h>
class Cal3_S2Stereo {
// Standard Constructors
Cal3_S2Stereo();
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b);
Cal3_S2Stereo(Vector v);
// Testable
void print(string s) const;
bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const;
// Standard Interface
double fx() const;
double fy() const;
double skew() const;
double px() const;
double py() const;
gtsam::Point2 principalPoint() const;
double baseline() const;
};
#include <gtsam/geometry/Cal3Bundler.h>
class Cal3Bundler {
// Standard Constructors
Cal3Bundler();
Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
// Testable
void print(string s) const;
bool equals(const gtsam::Cal3Bundler& rhs, double tol) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Cal3Bundler retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
// Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
// Standard Interface
double fx() const;
double fy() const;
double k1() const;
double k2() const;
double u0() const;
double v0() const;
Vector vector() const;
Vector k() const;
//Matrix K() const; //FIXME: Uppercase
// enabling serialization functionality
void serialize() const;
};
class CalibratedCamera {
// Standard Constructors and Named Constructors
CalibratedCamera();
CalibratedCamera(const gtsam::Pose3& pose);
CalibratedCamera(const Vector& v);
static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height);
// Testable
void print(string s) const;
bool equals(const gtsam::CalibratedCamera& camera, double tol) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::CalibratedCamera retract(const Vector& d) const;
Vector localCoordinates(const gtsam::CalibratedCamera& T2) const;
// Action on Point3
gtsam::Point2 project(const gtsam::Point3& point) const;
static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
// Standard Interface
gtsam::Pose3 pose() const;
double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods
// enabling serialization functionality
void serialize() const;
};
template<CALIBRATION>
class PinholeCamera {
// Standard Constructors and Named Constructors
PinholeCamera();
PinholeCamera(const gtsam::Pose3& pose);
PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K);
static This Level(const CALIBRATION& 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 CALIBRATION& 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(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);
// enabling serialization functionality
void serialize() const;
};
virtual class SimpleCamera {
// Standard Constructors and Named Constructors
SimpleCamera();
SimpleCamera(const gtsam::Pose3& pose);
SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K);
static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height);
static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height);
static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
const gtsam::Point3& upVector, const gtsam::Cal3_S2& K);
// Testable
void print(string s) const;
bool equals(const gtsam::SimpleCamera& camera, double tol) const;
// Standard Interface
gtsam::Pose3 pose() const;
gtsam::Cal3_S2 calibration() const;
// Manifold
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(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);
// enabling serialization functionality
void serialize() const;
};
// Some typedefs for common camera types
// PinholeCameraCal3_S2 is the same as SimpleCamera above
typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
class StereoCamera {
// 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;
};
#include <gtsam/geometry/triangulation.h>
// Templates appear not yet supported for free functions
gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize);
gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements,
double rank_tol, bool optimize);
//*************************************************************************
// Symbolic
//*************************************************************************
#include <gtsam/symbolic/SymbolicFactor.h>
virtual class SymbolicFactor {
// Standard Constructors and Named Constructors
SymbolicFactor(const gtsam::SymbolicFactor& f);
SymbolicFactor();
SymbolicFactor(size_t j);
SymbolicFactor(size_t j1, size_t j2);
SymbolicFactor(size_t j1, size_t j2, size_t j3);
SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4);
SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5);
SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, size_t j6);
static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js);
// From Factor
size_t size() const;
void print(string s) const;
bool equals(const gtsam::SymbolicFactor& other, double tol) const;
gtsam::KeyVector keys();
};
#include <gtsam/symbolic/SymbolicFactorGraph.h>
virtual class SymbolicFactorGraph {
SymbolicFactorGraph();
SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet);
SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree);
// From FactorGraph
void push_back(gtsam::SymbolicFactor* factor);
void print(string s) const;
bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const;
size_t size() const;
bool exists(size_t idx) const;
// Standard interface
gtsam::KeySet keys() const;
void push_back(gtsam::SymbolicFactor* factor);
void push_back(const gtsam::SymbolicFactorGraph& graph);
void push_back(const gtsam::SymbolicBayesNet& bayesNet);
void push_back(const gtsam::SymbolicBayesTree& bayesTree);
//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);
gtsam::SymbolicBayesNet* eliminateSequential();
gtsam::SymbolicBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
gtsam::SymbolicBayesTree* eliminateMultifrontal();
gtsam::SymbolicBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering);
pair<gtsam::SymbolicBayesNet*, gtsam::SymbolicFactorGraph*> eliminatePartialSequential(
const gtsam::Ordering& ordering);
pair<gtsam::SymbolicBayesNet*, gtsam::SymbolicFactorGraph*> eliminatePartialSequential(
const gtsam::KeyVector& keys);
pair<gtsam::SymbolicBayesTree*, gtsam::SymbolicFactorGraph*> eliminatePartialMultifrontal(
const gtsam::Ordering& ordering);
pair<gtsam::SymbolicBayesTree*, gtsam::SymbolicFactorGraph*> eliminatePartialMultifrontal(
const gtsam::KeyVector& keys);
gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables);
gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables);
gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables,
const gtsam::Ordering& marginalizedVariableOrdering);
gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables,
const gtsam::Ordering& marginalizedVariableOrdering);
gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& variables);
};
#include <gtsam/symbolic/SymbolicConditional.h>
virtual class SymbolicConditional : gtsam::SymbolicFactor {
// Standard Constructors and Named Constructors
SymbolicConditional();
SymbolicConditional(const gtsam::SymbolicConditional& other);
SymbolicConditional(size_t key);
SymbolicConditional(size_t key, size_t parent);
SymbolicConditional(size_t key, size_t parent1, size_t parent2);
SymbolicConditional(size_t key, size_t parent1, size_t parent2, size_t parent3);
static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals);
// Testable
void print(string s) const;
bool equals(const gtsam::SymbolicConditional& other, double tol) const;
// Standard interface
size_t nrFrontals() const;
size_t nrParents() const;
};
#include <gtsam/symbolic/SymbolicBayesNet.h>
class SymbolicBayesNet {
SymbolicBayesNet();
SymbolicBayesNet(const gtsam::SymbolicBayesNet& other);
// Testable
void print(string s) const;
bool equals(const gtsam::SymbolicBayesNet& other, double tol) const;
// Standard interface
size_t size() const;
void saveGraph(string s) const;
gtsam::SymbolicConditional* at(size_t idx) const;
gtsam::SymbolicConditional* front() const;
gtsam::SymbolicConditional* back() const;
void push_back(gtsam::SymbolicConditional* conditional);
void push_back(const gtsam::SymbolicBayesNet& bayesNet);
};
#include <gtsam/symbolic/SymbolicBayesTree.h>
class SymbolicBayesTree {
//Constructors
SymbolicBayesTree();
SymbolicBayesTree(const gtsam::SymbolicBayesTree& other);
// Testable
void print(string s);
bool equals(const gtsam::SymbolicBayesTree& other, double tol) const;
//Standard Interface
//size_t findParentClique(const gtsam::IndexVector& parents) const;
size_t size();
void saveGraph(string s) const;
void clear();
void deleteCachedShortcuts();
size_t numCachedSeparatorMarginals() const;
gtsam::SymbolicConditional* marginalFactor(size_t key) const;
gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const;
gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const;
};
// class SymbolicBayesTreeClique {
// BayesTreeClique();
// BayesTreeClique(CONDITIONAL* conditional);
// // BayesTreeClique(const std::pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
//
// bool equals(const This& other, double tol) const;
// void print(string s) const;
// void printTree() const; // Default indent of ""
// void printTree(string indent) const;
// size_t numCachedSeparatorMarginals() const;
//
// CONDITIONAL* conditional() const;
// bool isRoot() const;
// size_t treeSize() const;
// // const std::list<derived_ptr>& children() const { return children_; }
// // derived_ptr parent() const { return parent_.lock(); }
//
// // FIXME: need wrapped versions graphs, BayesNet
// // BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;
// // FactorGraph<FactorType> marginal(derived_ptr root, Eliminate function) const;
// // FactorGraph<FactorType> joint(derived_ptr C2, derived_ptr root, Eliminate function) const;
//
// void deleteCachedShortcuts();
// };
#include <gtsam/inference/VariableIndex.h>
class VariableIndex {
// Standard Constructors and Named Constructors
VariableIndex();
// TODO: Templetize constructor when wrap supports it
//template<T = {gtsam::FactorGraph}>
//VariableIndex(const T& factorGraph, size_t nVariables);
//VariableIndex(const T& factorGraph);
VariableIndex(const gtsam::SymbolicFactorGraph& factorGraph);
VariableIndex(const gtsam::GaussianFactorGraph& factorGraph);
VariableIndex(const gtsam::NonlinearFactorGraph& factorGraph);
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;
};
//*************************************************************************
// linear
//*************************************************************************
namespace noiseModel {
#include <gtsam/linear/NoiseModel.h>
virtual class Base {
};
virtual class Gaussian : gtsam::noiseModel::Base {
static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R);
static gtsam::noiseModel::Gaussian* Covariance(Matrix R);
Matrix R() const;
bool equals(gtsam::noiseModel::Base& expected, double tol);
void print(string s) const;
// enabling serialization functionality
void serializable() const;
};
virtual class Diagonal : gtsam::noiseModel::Gaussian {
static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas);
static gtsam::noiseModel::Diagonal* Variances(Vector variances);
static gtsam::noiseModel::Diagonal* Precisions(Vector precisions);
Matrix R() const;
void print(string s) const;
// enabling serialization functionality
void serializable() const;
};
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);
static gtsam::noiseModel::Constrained* All(size_t dim, double mu);
gtsam::noiseModel::Constrained* unit() const;
// enabling serialization functionality
void serializable() const;
};
virtual class Isotropic : gtsam::noiseModel::Diagonal {
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;
// enabling serialization functionality
void serializable() const;
};
virtual class Unit : gtsam::noiseModel::Isotropic {
static gtsam::noiseModel::Unit* Create(size_t dim);
void print(string s) const;
// enabling serialization functionality
void serializable() const;
};
namespace mEstimator {
virtual class Base {
};
virtual class Null: gtsam::noiseModel::mEstimator::Base {
Null();
void print(string s) const;
static gtsam::noiseModel::mEstimator::Null* Create();
// enabling serialization functionality
void serializable() const;
};
virtual class Fair: gtsam::noiseModel::mEstimator::Base {
Fair(double c);
void print(string s) const;
static gtsam::noiseModel::mEstimator::Fair* Create(double c);
// enabling serialization functionality
void serializable() const;
};
virtual class Huber: gtsam::noiseModel::mEstimator::Base {
Huber(double k);
void print(string s) const;
static gtsam::noiseModel::mEstimator::Huber* Create(double k);
// enabling serialization functionality
void serializable() const;
};
virtual class Tukey: gtsam::noiseModel::mEstimator::Base {
Tukey(double k);
void print(string s) const;
static gtsam::noiseModel::mEstimator::Tukey* Create(double k);
// enabling serialization functionality
void serializable() const;
};
}///\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;
// enabling serialization functionality
void serializable() const;
};
}///\namespace noiseModel
#include <gtsam/linear/Sampler.h>
class Sampler {
//Constructors
Sampler(gtsam::noiseModel::Diagonal* model, int seed);
Sampler(Vector sigmas, int seed);
Sampler(int seed);
//Standard Interface
size_t dim() const;
Vector sigmas() const;
gtsam::noiseModel::Diagonal* model() const;
Vector sample();
Vector sampleNewModel(gtsam::noiseModel::Diagonal* model);
};
#include <gtsam/linear/VectorValues.h>
class VectorValues {
//Constructors
VectorValues();
VectorValues(const gtsam::VectorValues& other);
//Named Constructors
static gtsam::VectorValues Zero(const gtsam::VectorValues& model);
//Standard Interface
size_t size() const;
size_t dim(size_t j) const;
bool exists(size_t j) const;
void print(string s) const;
bool equals(const gtsam::VectorValues& expected, double tol) const;
void insert(size_t j, Vector value);
Vector vector() const;
Vector at(size_t j) const;
void update(const gtsam::VectorValues& values);
//Advanced Interface
void setZero();
gtsam::VectorValues add(const gtsam::VectorValues& c) const;
void addInPlace(const gtsam::VectorValues& c);
gtsam::VectorValues subtract(const gtsam::VectorValues& c) const;
gtsam::VectorValues scale(double a) const;
void scaleInPlace(double a);
bool hasSameStructure(const gtsam::VectorValues& other) const;
double dot(const gtsam::VectorValues& V) const;
double norm() const;
double squaredNorm() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/linear/GaussianFactor.h>
virtual class GaussianFactor {
gtsam::KeyVector keys() const;
void print(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const;
double error(const gtsam::VectorValues& c) const;
gtsam::GaussianFactor* clone() const;
gtsam::GaussianFactor* negate() const;
Matrix augmentedInformation() const;
Matrix information() const;
Matrix augmentedJacobian() const;
pair<Matrix, Vector> jacobian() const;
size_t size() const;
bool empty() const;
};
#include <gtsam/linear/JacobianFactor.h>
virtual class JacobianFactor : gtsam::GaussianFactor {
//Constructors
JacobianFactor();
JacobianFactor(const gtsam::GaussianFactor& factor);
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::GaussianFactorGraph& graph);
//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
Matrix getA() const;
Vector getb() const;
size_t rows() const;
size_t cols() const;
bool isConstrained() const;
pair<Matrix, Vector> jacobianUnweighted() const;
Matrix augmentedJacobianUnweighted() const;
void transposeMultiplyAdd(double alpha, const Vector& e, gtsam::VectorValues& x) const;
gtsam::JacobianFactor whiten() const;
pair<gtsam::GaussianConditional*, gtsam::JacobianFactor*> eliminate(const gtsam::Ordering& keys) const;
void setModel(bool anyConstrained, const Vector& sigmas);
gtsam::noiseModel::Diagonal* get_model() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/linear/HessianFactor.h>
virtual class HessianFactor : gtsam::GaussianFactor {
//Constructors
HessianFactor();
HessianFactor(const gtsam::GaussianFactor& factor);
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::GaussianFactorGraph& factors);
//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 information() const;
double constantTerm() const;
Vector linearTerm() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/linear/GaussianFactorGraph.h>
class GaussianFactorGraph {
GaussianFactorGraph();
GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet);
GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree);
// 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;
gtsam::KeySet keys() const;
bool exists(size_t idx) const;
// Building the graph
void push_back(const gtsam::GaussianFactor* factor);
void push_back(const gtsam::GaussianConditional* factor);
void push_back(const gtsam::GaussianFactorGraph& graph);
void push_back(const gtsam::GaussianBayesNet& bayesNet);
void push_back(const gtsam::GaussianBayesTree& bayesTree);
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);
// error and probability
double error(const gtsam::VectorValues& c) const;
double probPrime(const gtsam::VectorValues& c) const;
gtsam::GaussianFactorGraph clone() const;
gtsam::GaussianFactorGraph negate() const;
// Optimizing and linear algebra
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const;
gtsam::VectorValues optimizeGradientSearch() const;
gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
gtsam::VectorValues gradientAtZero() const;
// Elimination and marginals
gtsam::GaussianBayesNet* eliminateSequential();
gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering);
gtsam::GaussianBayesTree* eliminateMultifrontal();
gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering);
pair<gtsam::GaussianBayesNet*, gtsam::GaussianFactorGraph*> eliminatePartialSequential(
const gtsam::Ordering& ordering);
pair<gtsam::GaussianBayesNet*, gtsam::GaussianFactorGraph*> eliminatePartialSequential(
const gtsam::KeyVector& keys);
pair<gtsam::GaussianBayesTree*, gtsam::GaussianFactorGraph*> eliminatePartialMultifrontal(
const gtsam::Ordering& ordering);
pair<gtsam::GaussianBayesTree*, gtsam::GaussianFactorGraph*> eliminatePartialMultifrontal(
const gtsam::KeyVector& keys);
gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables);
gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables);
gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& variables,
const gtsam::Ordering& marginalizedVariableOrdering);
gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& variables,
const gtsam::Ordering& marginalizedVariableOrdering);
gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& variables);
// Conversion to matrices
Matrix sparseJacobian_() const;
Matrix augmentedJacobian() const;
Matrix augmentedJacobian(const gtsam::Ordering& ordering) const;
pair<Matrix,Vector> jacobian() const;
pair<Matrix,Vector> jacobian(const gtsam::Ordering& ordering) const;
Matrix augmentedHessian() const;
Matrix augmentedHessian(const gtsam::Ordering& ordering) const;
pair<Matrix,Vector> hessian() const;
pair<Matrix,Vector> hessian(const gtsam::Ordering& ordering) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/linear/GaussianConditional.h>
virtual class GaussianConditional : gtsam::GaussianFactor {
//Constructors
GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas);
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
const gtsam::noiseModel::Diagonal* sigmas);
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas);
//Constructors with no noise model
GaussianConditional(size_t key, Vector d, Matrix R);
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S);
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S,
size_t name2, Matrix T);
//Standard Interface
void print(string s) const;
bool equals(const gtsam::GaussianConditional &cg, double tol) const;
//Advanced Interface
gtsam::VectorValues solve(const gtsam::VectorValues& parents) const;
gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, const gtsam::VectorValues& rhs) const;
void solveTransposeInPlace(gtsam::VectorValues& gy) const;
void scaleFrontalsBySigma(gtsam::VectorValues& gy) const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/linear/GaussianDensity.h>
virtual class GaussianDensity : gtsam::GaussianConditional {
//Constructors
GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas);
//Standard Interface
void print(string s) const;
bool equals(const gtsam::GaussianDensity &cg, double tol) const;
Vector mean() const;
Matrix covariance() const;
};
#include <gtsam/linear/GaussianBayesNet.h>
virtual class GaussianBayesNet {
//Constructors
GaussianBayesNet();
GaussianBayesNet(const gtsam::GaussianConditional* conditional);
// Testable
void print(string s) const;
bool equals(const gtsam::GaussianBayesNet& other, double tol) const;
size_t size() const;
// FactorGraph derived interface
size_t size() const;
gtsam::GaussianConditional* at(size_t idx) const;
gtsam::KeySet keys() const;
bool exists(size_t idx) const;
gtsam::GaussianConditional* front() const;
gtsam::GaussianConditional* back() const;
void push_back(gtsam::GaussianConditional* conditional);
void push_back(const gtsam::GaussianBayesNet& bayesNet);
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const;
gtsam::VectorValues optimizeGradientSearch() const;
gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
gtsam::VectorValues gradientAtZero() const;
double error(const gtsam::VectorValues& x) const;
double determinant() const;
double logDeterminant() const;
gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const;
gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const;
};
#include <gtsam/linear/GaussianBayesTree.h>
virtual class GaussianBayesTree {
// Standard Constructors and Named Constructors
GaussianBayesTree();
GaussianBayesTree(const gtsam::GaussianBayesTree& other);
bool equals(const gtsam::GaussianBayesTree& other, double tol) const;
void print(string s);
size_t size() const;
bool empty() const;
size_t numCachedSeparatorMarginals() const;
void saveGraph(string s) const;
gtsam::VectorValues optimize() const;
gtsam::VectorValues optimizeGradientSearch() const;
gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const;
gtsam::VectorValues gradientAtZero() const;
double error(const gtsam::VectorValues& x) const;
double determinant() const;
double logDeterminant() const;
Matrix marginalCovariance(size_t key) const;
gtsam::GaussianConditional* marginalFactor(size_t key) const;
gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const;
gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const;
};
class Errors {
//Constructors
Errors();
Errors(const gtsam::VectorValues& V);
//Testable
void print(string s);
bool equals(const gtsam::Errors& expected, double tol) const;
};
class GaussianISAM {
//Constructor
GaussianISAM();
//Standard Interface
void update(const gtsam::GaussianFactorGraph& newFactors);
void saveGraph(string s) const;
void clear();
};
#include <gtsam/linear/IterativeSolver.h>
virtual class IterativeOptimizationParameters {
string getVerbosity() const;
void setVerbosity(string s) ;
void print() const;
};
//virtual class IterativeSolver {
// IterativeSolver();
// gtsam::VectorValues optimize ();
//};
#include <gtsam/linear/ConjugateGradientSolver.h>
virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters {
ConjugateGradientParameters();
int getMinIterations() const ;
int getMaxIterations() const ;
int getReset() const;
double getEpsilon_rel() const;
double getEpsilon_abs() const;
void setMinIterations(int value);
void setMaxIterations(int value);
void setReset(int value);
void setEpsilon_rel(double value);
void setEpsilon_abs(double value);
void print();
};
#include <gtsam/linear/SubgraphSolver.h>
virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
SubgraphSolverParameters();
void print() const;
};
virtual class SubgraphSolver {
SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
gtsam::VectorValues optimize() const;
};
#include <gtsam/linear/KalmanFilter.h>
class KalmanFilter {
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;
static size_t step(gtsam::GaussianDensity* p);
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);
};
//*************************************************************************
// nonlinear
//*************************************************************************
#include <gtsam/inference/Symbol.h>
size_t symbol(char chr, size_t index);
char symbolChr(size_t key);
size_t symbolIndex(size_t key);
// Default keyformatter
void PrintKeyList (const gtsam::KeyList& keys);
void PrintKeyList (const gtsam::KeyList& keys, string s);
void PrintKeyVector(const gtsam::KeyVector& keys);
void PrintKeyVector(const gtsam::KeyVector& keys, string s);
void PrintKeySet (const gtsam::KeySet& keys);
void PrintKeySet (const gtsam::KeySet& keys, string s);
#include <gtsam/inference/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;
gtsam::LabeledSymbol newChr(unsigned char c) const;
gtsam::LabeledSymbol newLabel(unsigned char label) const;
void print(string s) const;
};
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);
#include <gtsam/inference/Ordering.h>
class Ordering {
// Standard Constructors and Named Constructors
Ordering();
Ordering(const gtsam::Ordering& other);
// Testable
void print(string s) const;
bool equals(const gtsam::Ordering& ord, double tol) const;
// Standard interface
size_t size() const;
size_t at(size_t key) const;
void push_back(size_t key);
// enabling serialization functionality
void serialize() const;
};
class NonlinearFactorGraph {
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;
gtsam::NonlinearFactor* at(size_t idx) const;
void push_back(const gtsam::NonlinearFactorGraph& factors);
void push_back(gtsam::NonlinearFactor* factor);
void add(gtsam::NonlinearFactor* factor);
bool exists(size_t idx) const;
gtsam::KeySet keys() const;
// NonlinearFactorGraph
double error(const gtsam::Values& values) const;
double probPrime(const gtsam::Values& values) const;
gtsam::Ordering orderingCOLAMD() const;
// Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map<gtsam::Key,int>& constraints) const;
gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const;
gtsam::NonlinearFactorGraph clone() const;
// enabling serialization functionality
void serialize() const;
};
virtual class NonlinearFactor {
// Factor base class
size_t size() const;
gtsam::KeyVector keys() const;
void print(string s) const;
void printKeys(string s) const;
// NonlinearFactor
void equals(const gtsam::NonlinearFactor& other, double tol) const;
double error(const gtsam::Values& c) const;
size_t dim() const;
bool active(const gtsam::Values& c) const;
gtsam::GaussianFactor* linearize(const gtsam::Values& c) const;
gtsam::NonlinearFactor* clone() const;
// gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen
};
virtual class NoiseModelFactor: gtsam::NonlinearFactor {
void equals(const gtsam::NoiseModelFactor& other, double tol) const;
gtsam::noiseModel::Base* get_noiseModel() const; // deprecated by below
gtsam::noiseModel::Base* noiseModel() const;
Vector unwhitenedError(const gtsam::Values& x) const;
Vector whitenedError(const gtsam::Values& x) const;
};
#include <gtsam/nonlinear/Values.h>
class Values {
Values();
Values(const gtsam::Values& other);
size_t size() const;
bool empty() const;
void clear();
size_t dim() const;
void print(string s) const;
bool equals(const gtsam::Values& other, double tol) const;
void insert(const gtsam::Values& values);
void update(const gtsam::Values& values);
void erase(size_t j);
void swap(gtsam::Values& values);
bool exists(size_t j) const;
gtsam::KeyVector keys() const;
gtsam::VectorValues zeroVectors() const;
gtsam::Values retract(const gtsam::VectorValues& delta) const;
gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const;
// enabling serialization functionality
void serialize() const;
// New in 4.0, we have to specialize every insert/update/at to generate wrappers
// Instead of the old:
// void insert(size_t j, const gtsam::Value& value);
// void update(size_t j, const gtsam::Value& val);
// gtsam::Value at(size_t j) const;
void insert(size_t j, const gtsam::Point2& t);
void insert(size_t j, const gtsam::Point3& t);
void insert(size_t j, const gtsam::Rot2& t);
void insert(size_t j, const gtsam::Pose2& t);
void insert(size_t j, const gtsam::Rot3& t);
void insert(size_t j, const gtsam::Pose3& t);
void insert(size_t j, const gtsam::Cal3_S2& t);
void insert(size_t j, const gtsam::Cal3DS2& t);
void insert(size_t j, const gtsam::Cal3Bundler& t);
void insert(size_t j, const gtsam::EssentialMatrix& t);
void insert(size_t j, const gtsam::SimpleCamera& t);
void insert(size_t j, const gtsam::imuBias::ConstantBias& t);
void insert(size_t j, Vector t);
void insert(size_t j, Matrix t);
void update(size_t j, const gtsam::Point2& t);
void update(size_t j, const gtsam::Point3& t);
void update(size_t j, const gtsam::Rot2& t);
void update(size_t j, const gtsam::Pose2& t);
void update(size_t j, const gtsam::Rot3& t);
void update(size_t j, const gtsam::Pose3& t);
void update(size_t j, const gtsam::Cal3_S2& t);
void update(size_t j, const gtsam::Cal3DS2& t);
void update(size_t j, const gtsam::Cal3Bundler& t);
void update(size_t j, const gtsam::EssentialMatrix& t);
void update(size_t j, const gtsam::imuBias::ConstantBias& t);
void update(size_t j, Vector t);
void update(size_t j, Matrix t);
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
T at(size_t j);
/// version for double
void insertDouble(size_t j, double c);
double atDouble(size_t j) const;
};
// Actually a FastList<Key>
#include <gtsam/inference/Key.h>
class KeyList {
KeyList();
KeyList(const gtsam::KeyList& other);
// Note: no print function
// common STL methods
size_t size() const;
bool empty() const;
void clear();
// structure specific methods
size_t front() const;
size_t back() const;
void push_back(size_t key);
void push_front(size_t key);
void pop_back();
void pop_front();
void sort();
void remove(size_t key);
void serialize() const;
};
// Actually a FastSet<Key>
class KeySet {
KeySet();
KeySet(const gtsam::KeySet& other);
KeySet(const gtsam::KeyVector& other);
KeySet(const gtsam::KeyList& other);
// Testable
void print(string s) const;
bool equals(const gtsam::KeySet& other) const;
// common STL methods
size_t size() const;
bool empty() const;
void clear();
// structure specific methods
void insert(size_t key);
void merge(gtsam::KeySet& other);
bool erase(size_t key); // returns true if value was removed
bool count(size_t key) const; // returns true if value exists
void serialize() const;
};
// Actually a vector<Key>
class KeyVector {
KeyVector();
KeyVector(const gtsam::KeyVector& other);
// Note: no print function
// common STL methods
size_t size() const;
bool empty() const;
void clear();
// 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;
void serialize() const;
};
// Actually a FastMap<Key,int>
class KeyGroupMap {
KeyGroupMap();
// Note: no print function
// common STL methods
size_t size() const;
bool empty() const;
void clear();
// structure specific methods
size_t at(size_t key) const;
int erase(size_t key);
bool insert2(size_t key, int val);
};
#include <gtsam/nonlinear/Marginals.h>
class Marginals {
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;
};
class JointMarginal {
Matrix at(size_t iVariable, size_t jVariable) const;
Matrix fullMatrix() const;
void print(string s) const;
void print() const;
};
#include <gtsam/nonlinear/LinearContainerFactor.h>
virtual class LinearContainerFactor : gtsam::NonlinearFactor {
LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint);
LinearContainerFactor(gtsam::GaussianFactor* factor);
gtsam::GaussianFactor* factor() const;
// const boost::optional<Values>& linearizationPoint() const;
bool isJacobian() const;
gtsam::JacobianFactor* toJacobian() const;
gtsam::HessianFactor* toHessian() const;
static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph,
const gtsam::Values& linearizationPoint);
static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph);
// enabling serialization functionality
void serializable() const;
}; // \class LinearContainerFactor
// Summarization functionality
//#include <gtsam/nonlinear/summarization.h>
//
//// Uses partial QR approach by default
//gtsam::GaussianFactorGraph summarize(
// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values,
// const gtsam::KeySet& saved_keys);
//
//gtsam::NonlinearFactorGraph summarizeAsNonlinearContainer(
// const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values,
// const gtsam::KeySet& saved_keys);
//*************************************************************************
// Nonlinear optimizers
//*************************************************************************
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/NonlinearOptimizerParams.h>
virtual class NonlinearOptimizerParams {
NonlinearOptimizerParams();
void print(string s) const;
int getMaxIterations() const;
double getRelativeErrorTol() const;
double getAbsoluteErrorTol() const;
double getErrorTol() const;
string getVerbosity() const;
void setMaxIterations(int value);
void setRelativeErrorTol(double value);
void setAbsoluteErrorTol(double value);
void setErrorTol(double value);
void setVerbosity(string s);
string getLinearSolverType() const;
void setLinearSolverType(string solver);
void setOrdering(const gtsam::Ordering& ordering);
void setIterativeParams(gtsam::IterativeOptimizationParameters* params);
bool isMultifrontal() const;
bool isSequential() const;
bool isCholmod() const;
bool isIterative() const;
};
bool checkConvergence(double relativeErrorTreshold,
double absoluteErrorTreshold, double errorThreshold,
double currentError, double newError);
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams {
GaussNewtonParams();
};
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams {
LevenbergMarquardtParams();
double getlambdaInitial() const;
double getlambdaFactor() const;
double getlambdaUpperBound() const;
string getVerbosityLM() const;
void setlambdaInitial(double value);
void setlambdaFactor(double value);
void setlambdaUpperBound(double value);
void setVerbosityLM(string s);
};
#include <gtsam/nonlinear/DoglegOptimizer.h>
virtual class DoglegParams : gtsam::NonlinearOptimizerParams {
DoglegParams();
double getDeltaInitial() const;
string getVerbosityDL() const;
void setDeltaInitial(double deltaInitial) const;
void setVerbosityDL(string verbosityDL) const;
};
virtual class NonlinearOptimizer {
gtsam::Values optimize();
gtsam::Values optimizeSafely();
double error() const;
int iterations() const;
gtsam::Values values() const;
void iterate() const;
};
virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer {
GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues);
GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::GaussNewtonParams& params);
};
virtual class DoglegOptimizer : gtsam::NonlinearOptimizer {
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;
};
virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer {
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;
void print(string str) const;
};
#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);
};
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;
};
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);
void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& relinearizeThreshold);
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);
};
class ISAM2Clique {
//Constructors
ISAM2Clique();
//Standard Interface
Vector gradientContribution() const;
void print(string s);
};
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;
};
class FactorIndices {};
class ISAM2 {
ISAM2();
ISAM2(const gtsam::ISAM2Params& params);
ISAM2(const gtsam::ISAM2& other);
bool equals(const gtsam::ISAM2& other, double tol) const;
void print(string s) const;
void printStats() const;
void saveGraph(string s) const;
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::FactorIndices& removeFactorIndices);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys);
// TODO: wrap the full version of update
//void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap<Key,int>& constrainedKeys);
//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;
gtsam::Value calculateEstimate(size_t key) const;
gtsam::Values calculateBestEstimate() const;
Matrix marginalCovariance(size_t key) const;
gtsam::VectorValues getDelta() const;
gtsam::NonlinearFactorGraph getFactorsUnsafe() const;
gtsam::VariableIndex getVariableIndex() const;
gtsam::ISAM2Params params() const;
};
#include <gtsam/nonlinear/NonlinearISAM.h>
class NonlinearISAM {
NonlinearISAM();
NonlinearISAM(int reorderInterval);
void print(string s) const;
void printStats() const;
void saveGraph(string s) const;
gtsam::Values estimate() const;
Matrix marginalCovariance(size_t key) const;
int reorderInterval() const;
int reorderCounter() const;
void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& initialValues);
void reorder_relinearize();
// These might be expensive as instead of a reference the wrapper will make a copy
gtsam::GaussianISAM bayesTree() const;
gtsam::Values getLinearizationPoint() const;
gtsam::NonlinearFactorGraph getFactorsUnsafe() const;
};
//*************************************************************************
// Nonlinear factor types
//*************************************************************************
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/slam/PriorFactor.h>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/slam/BetweenFactor.h>
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
virtual class BetweenFactor : gtsam::NoiseModelFactor {
BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
T measured() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/nonlinear/NonlinearEquality.h>
template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class NonlinearEquality : gtsam::NoiseModelFactor {
// 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);
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/sam/RangeFactor.h>
template<POSE, POINT>
virtual class RangeFactor : gtsam::NoiseModelFactor {
RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel);
};
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;
#include <gtsam/sam/BearingFactor.h>
template<POSE, POINT, BEARING>
virtual class BearingFactor : gtsam::NoiseModelFactor {
BearingFactor(size_t key1, size_t key2, const BEARING& measured, const gtsam::noiseModel::Base* noiseModel);
// enabling serialization functionality
void serialize() const;
};
typedef gtsam::BearingFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2> BearingFactor2D;
#include <gtsam/sam/BearingRangeFactor.h>
template<POSE, POINT, BEARING, RANGE>
virtual class BearingRangeFactor : gtsam::NoiseModelFactor {
BearingRangeFactor(size_t poseKey, size_t pointKey,
const BEARING& measuredBearing, const RANGE& measuredRange,
const gtsam::noiseModel::Base* noiseModel);
// enabling serialization functionality
void serialize() const;
};
typedef gtsam::BearingRangeFactor<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double> BearingRangeFactor2D;
#include <gtsam/slam/ProjectionFactor.h>
template<POSE, LANDMARK, CALIBRATION>
virtual class GenericProjectionFactor : gtsam::NoiseModelFactor {
GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
size_t poseKey, size_t pointKey, const CALIBRATION* k);
GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
size_t poseKey, size_t pointKey, const CALIBRATION* k, const POSE& body_P_sensor);
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);
gtsam::Point2 measured() const;
CALIBRATION* calibration() const;
bool verboseCheirality() const;
bool throwCheirality() const;
// enabling serialization functionality
void serialize() const;
};
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> GenericProjectionFactorCal3_S2;
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> GenericProjectionFactorCal3DS2;
#include <gtsam/slam/GeneralSFMFactor.h>
template<CAMERA, LANDMARK>
virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey);
gtsam::Point2 measured() const;
};
typedef gtsam::GeneralSFMFactor<gtsam::SimpleCamera, gtsam::Point3> GeneralSFMFactorCal3_S2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
template<CALIBRATION = {gtsam::Cal3_S2}>
virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey);
gtsam::Point2 measured() const;
// enabling serialization functionality
void serialize() const;
};
#include <gtsam/slam/SmartProjectionFactor.h>
class SmartProjectionParams {
SmartProjectionParams();
// TODO(frank): make these work:
// void setLinearizationMode(LinearizationMode linMode);
// void setDegeneracyMode(DegeneracyMode degMode);
void setRankTolerance(double rankTol);
void setEnableEPI(bool enableEPI);
void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold);
void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold);
};
#include <gtsam/slam/SmartProjectionPoseFactor.h>
template<CALIBRATION>
virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor {
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
const CALIBRATION* K);
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
const CALIBRATION* K,
const gtsam::Pose3& body_P_sensor);
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
const CALIBRATION* K,
const gtsam::Pose3& body_P_sensor,
const gtsam::SmartProjectionParams& params);
void add(const gtsam::Point2& measured_i, size_t poseKey_i);
// enabling serialization functionality
//void serialize() const;
};
typedef gtsam::SmartProjectionPoseFactor<gtsam::Cal3_S2> SmartProjectionPose3Factor;
#include <gtsam/slam/StereoFactor.h>
template<POSE, LANDMARK>
virtual class GenericStereoFactor : gtsam::NoiseModelFactor {
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;
// enabling serialization functionality
void serialize() const;
};
typedef gtsam::GenericStereoFactor<gtsam::Pose3, gtsam::Point3> GenericStereoFactor3D;
#include <gtsam/slam/PoseTranslationPrior.h>
template<POSE>
virtual class PoseTranslationPrior : gtsam::NoiseModelFactor {
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>
virtual class PoseRotationPrior : gtsam::NoiseModelFactor {
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;
#include <gtsam/slam/EssentialMatrixFactor.h>
virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, const gtsam::Point2& pB,
const gtsam::noiseModel::Base* noiseModel);
};
#include <gtsam/slam/dataset.h>
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxID);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(string filename,
gtsam::noiseModel::Base* model);
void save2D(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
void writeG2o(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& estimate, string filename);
//*************************************************************************
// Navigation
//*************************************************************************
namespace imuBias {
#include <gtsam/navigation/ImuBias.h>
class ConstantBias {
// Constructors
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
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
#include <gtsam/navigation/NavState.h>
class NavState {
// Constructors
NavState();
NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v);
NavState(const gtsam::Pose3& pose, Vector v);
// Testable
void print(string s) const;
bool equals(const gtsam::NavState& expected, double tol) const;
// Access
gtsam::Rot3 attitude() const;
gtsam::Point3 position() const;
Vector velocity() const;
gtsam::Pose3 pose() const;
};
#include <gtsam/navigation/PreintegratedRotation.h>
virtual class PreintegratedRotationParams {
PreintegratedRotationParams();
void setGyroscopeCovariance(Matrix cov);
void setOmegaCoriolis(Vector omega);
void setBodyPSensor(const gtsam::Pose3& pose);
Matrix getGyroscopeCovariance() const;
// TODO(frank): allow optional
// boost::optional<Vector3> getOmegaCoriolis() const;
// boost::optional<Pose3> getBodyPSensor() const;
};
#include <gtsam/navigation/PreintegrationParams.h>
virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
PreintegrationParams(Vector n_gravity);
void setAccelerometerCovariance(Matrix cov);
void setIntegrationCovariance(Matrix cov);
void setUse2ndOrderCoriolis(bool flag);
Matrix getAccelerometerCovariance() const;
Matrix getIntegrationCovariance() const;
bool getUse2ndOrderCoriolis() const;
};
#include <gtsam/navigation/ImuFactor.h>
class PreintegratedImuMeasurements {
// Constructors
PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params);
PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params,
const gtsam::imuBias::ConstantBias& bias);
// Testable
void print(string s) const;
bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol);
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
double deltaT);
void resetIntegration();
Matrix preintMeasCov() const;
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;
};
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::PreintegratedImuMeasurements& preintegratedMeasurements);
// Standard Interface
gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i,
const gtsam::Pose3& pose_j, Vector vel_j,
const gtsam::imuBias::ConstantBias& bias);
};
#include <gtsam/navigation/CombinedImuFactor.h>
class PreintegratedCombinedMeasurements {
// Testable
void print(string s) const;
bool equals(const gtsam::PreintegratedCombinedMeasurements& expected,
double tol);
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega,
double deltaT);
void resetIntegration();
Matrix preintMeasCov() const;
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;
};
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::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements);
// Standard Interface
gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i,
const gtsam::Pose3& pose_j, Vector vel_j,
const gtsam::imuBias::ConstantBias& bias_i,
const gtsam::imuBias::ConstantBias& bias_j);
};
#include <gtsam/navigation/AHRSFactor.h>
class PreintegratedAhrsMeasurements {
// Standard Constructor
PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance);
PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance);
PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol);
// get Data
gtsam::Rot3 deltaRij() const;
double deltaTij() const;
Vector biasHat() const;
// Standard Interface
void integrateMeasurement(Vector measuredOmega, double deltaT);
void resetIntegration() ;
};
virtual class AHRSFactor : gtsam::NonlinearFactor {
AHRSFactor(size_t rot_i, size_t rot_j,size_t bias,
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis);
AHRSFactor(size_t rot_i, size_t rot_j, size_t bias,
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis,
const gtsam::Pose3& body_P_sensor);
// Standard Interface
gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j,
Vector bias) const;
gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias,
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements,
Vector omegaCoriolis) const;
};
#include <gtsam/navigation/AttitudeFactor.h>
//virtual class AttitudeFactor : gtsam::NonlinearFactor {
// AttitudeFactor(const Unit3& nZ, const Unit3& bRef);
// AttitudeFactor();
//};
virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{
Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model,
const gtsam::Unit3& bRef);
Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model);
Rot3AttitudeFactor();
void print(string s) const;
bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
gtsam::Unit3 nZ() const;
gtsam::Unit3 bRef() const;
};
virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model,
const gtsam::Unit3& bRef);
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model);
Pose3AttitudeFactor();
void print(string s) const;
bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
gtsam::Unit3 nZ() const;
gtsam::Unit3 bRef() const;
};
//*************************************************************************
// Utilities
//*************************************************************************
namespace utilities {
#include <matlab.h>
gtsam::KeyList createKeyList(Vector I);
gtsam::KeyList createKeyList(string s, Vector I);
gtsam::KeyVector createKeyVector(Vector I);
gtsam::KeyVector createKeyVector(string s, Vector I);
gtsam::KeySet createKeySet(Vector I);
gtsam::KeySet createKeySet(string s, Vector I);
Matrix extractPoint2(const gtsam::Values& values);
Matrix extractPoint3(const gtsam::Values& values);
Matrix extractPose2(const gtsam::Values& values);
gtsam::Values allPose3s(gtsam::Values& values);
Matrix extractPose3(const gtsam::Values& values);
void perturbPoint2(gtsam::Values& values, double sigma, int seed);
void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, 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);
void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
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);
Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values);
gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base);
gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys);
} //\namespace utilities
}

Comment ( 0 )

Sign in for post a comment

1
https://gitee.com/zerolover/gtsam.git
git@gitee.com:zerolover/gtsam.git
zerolover
gtsam
gtsam
develop

Search