diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 85f694bd2..ddf164cb4 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -48,6 +48,8 @@ protected: public: + Marginals(const Marginals&) = default; + /** Construct a marginals class. * @param graph The factor graph defining the full joint density on all variables. * @param solution The linearization point about which to compute Gaussian marginals (usually the MLE as obtained from a NonlinearOptimizer). @@ -91,6 +93,8 @@ protected: FastMap indices_; public: + JointMarginal(const JointMarginal&) = default; + /** Access a block, corresponding to a pair of variables, of the joint * marginal. Each block is accessed by its "vertical position", * corresponding to the variable with nonlinear Key \c iVariable and diff --git a/wrap/Argument.h b/wrap/Argument.h index 669108ff1..e288be411 100644 --- a/wrap/Argument.h +++ b/wrap/Argument.h @@ -39,6 +39,12 @@ struct Argument { type(t), name(n), is_const(false), is_ref(false), is_ptr(false) { } + bool typesEqual(const Argument& other) const { + return type == other.type + && is_const == other.is_const && is_ref == other.is_ref + && is_ptr == other.is_ptr; + } + bool operator==(const Argument& other) const { return type == other.type && name == other.name && is_const == other.is_const && is_ref == other.is_ref @@ -98,6 +104,12 @@ struct ArgumentList: public std::vector { ArgumentList expandTemplate(const TemplateSubstitution& ts) const; + bool typesEqual(const ArgumentList& other) const { + for(size_t i = 0; i& classes) { [&](boost::tuple const& parentOverload) { return overload.get<0>() == parentOverload.get<0>() && - overload.get<1>() == parentOverload.get<1>(); + overload.get<1>().typesEqual(parentOverload.get<1>()); }) != parentMethodOverloads.end(); return found; }); @@ -811,24 +811,6 @@ void Class::emit_cython_pyx(FileWriter& pyxFile, const std::vector& allCl pyxInitParentObj(pyxFile, "\t\tret", "other", allClasses); pyxFile.oss << "\t\treturn ret" << "\n"; - // Generate cyCreateFromValue by default, although for some classes it can't be used - // It's only usable if its copy constructor AND its copy assignment operator exist - // Copy assignment operator is needed because Cython might assign the obj to its temp variable. - // Some class (e.g. noiseModel::Robust) have copy constructor but no copy assignment operator - if (constructor.nrOverloads() >= 1) { - // cyCreateFromValue - pyxFile.oss << "\t@staticmethod\n"; - pyxFile.oss << "\tcdef " << pythonClass() << " cyCreateFromValue(const " - << pyxCythonClass() << "& value):\n" - << "\t\tcdef " << pythonClass() - << " ret = " << pythonClass() << "()\n" - << "\t\tret." << pyxCythonObj() << " = " << pyxSharedCythonClass() - << "(new " << pyxCythonClass() << "(value))\n"; - pyxInitParentObj(pyxFile, "\t\tret", "ret." + pyxCythonObj(), allClasses); - pyxFile.oss << "\t\treturn ret" << "\n"; - pyxFile.oss << "\n"; - } - // Constructors constructor.emit_cython_pyx(pyxFile, *this); if (constructor.nrOverloads()>0) pyxFile.oss << "\n"; diff --git a/wrap/Constructor.cpp b/wrap/Constructor.cpp index e306a51f0..2dd9c48b3 100644 --- a/wrap/Constructor.cpp +++ b/wrap/Constructor.cpp @@ -131,17 +131,8 @@ bool Constructor::hasDefaultConstructor() const { /* ************************************************************************* */ void Constructor::emit_cython_pxd(FileWriter& pxdFile, Str className) const { - // HACK HACK: always add the default copy constructor by default - pxdFile.oss << "\t\t" << className << "(const " << className - << "&) except +\n"; - for (size_t i = 0; i < nrOverloads(); i++) { ArgumentList args = argumentList(i); - // ignore copy constructor, it's generated above by default - if (args.size() == 1 && args[0].is_const && args[0].is_ref && - !args[0].is_ptr && (args[0].type.cythonClass() == className || - args[0].type.cythonClass() == "This")) - continue; // generate the constructor pxdFile.oss << "\t\t" << className << "("; diff --git a/wrap/Method.cpp b/wrap/Method.cpp index 5de0ca5ef..476260b2c 100644 --- a/wrap/Method.cpp +++ b/wrap/Method.cpp @@ -28,6 +28,16 @@ using namespace std; using namespace wrap; +/* ************************************************************************* */ +static const std::array pythonKeywords{{"print", "lambda"}}; +static std::string pyRename(const std::string& name) { + if (std::find(pythonKeywords.begin(), pythonKeywords.end(), name) == + pythonKeywords.end()) + return name; + else + return "_" + name; +} + /* ************************************************************************* */ bool Method::addOverload(Str name, const ArgumentList& args, const ReturnValue& retVal, bool is_const, @@ -81,18 +91,18 @@ void Method::emit_cython_pxd(FileWriter& file, const Class& cls) const { for(size_t i = 0; i < nrOverloads(); ++i) { file.oss << "\t\t"; returnVals_[i].emit_cython_pxd(file, cls.cythonClass()); - file.oss << ((name_ == "print") ? "_print \"print\"" : name_) << "("; + file.oss << pyRename(name_) + " \"" + name_ + "\"" << "("; + // ((name_ == "print") ? "_print \"print\"" : name_) << "("; argumentList(i).emit_cython_pxd(file, cls.cythonClass()); file.oss << ")"; if (is_const_) file.oss << " const"; file.oss << "\n"; } - } /* ************************************************************************* */ void Method::emit_cython_pyx(FileWriter& file, const Class& cls) const { - string funcName = ((name_ == "print") ? "_print" : name_); + string funcName = pyRename(name_); size_t N = nrOverloads(); for(size_t i = 0; i < N; ++i) { // Function definition diff --git a/wrap/MethodBase.cpp b/wrap/MethodBase.cpp index 142c17c2c..f74ffc12d 100644 --- a/wrap/MethodBase.cpp +++ b/wrap/MethodBase.cpp @@ -151,12 +151,20 @@ void MethodBase::emit_cython_pyx_function_call(FileWriter& file, returnVals_[iOverload].emit_cython_pyx_return_type(file); file.oss << " ret = "; } + if (!returnVals_[iOverload].isPair && !returnVals_[iOverload].type1.isPtr && returnVals_[iOverload].type1.isNonBasicType()) + file.oss << returnVals_[iOverload].type1.pyxSharedCythonClass() << "(new " + << returnVals_[iOverload].type1.pyxCythonClass() << "("; + //... function call file.oss << caller << "." << funcName; if (templateArgValue_) file.oss << "[" << templateArgValue_->pyxCythonClass() << "]"; file.oss << "("; argumentList(iOverload).emit_cython_pyx_asParams(file); - file.oss << ")\n"; + file.oss << ")"; + + if (!returnVals_[iOverload].isPair && !returnVals_[iOverload].type1.isPtr && returnVals_[iOverload].type1.isNonBasicType()) + file.oss << "))"; + file.oss << "\n"; // ... casting return value if (!returnVals_[iOverload].isVoid()) { diff --git a/wrap/Qualified.h b/wrap/Qualified.h index 3dae6a802..e96ecb4c2 100644 --- a/wrap/Qualified.h +++ b/wrap/Qualified.h @@ -116,6 +116,10 @@ public: || name() == "size_t" || name() == "double"); } + bool isVoid() const { + return name() == "void"; + } + bool isString() const { return name() == "string"; } @@ -125,7 +129,7 @@ public: } bool isNonBasicType() const { - return !isString() && !isScalar() && !isEigen(); + return !isString() && !isScalar() && !isEigen() && !isVoid(); } public: diff --git a/wrap/ReturnType.cpp b/wrap/ReturnType.cpp index 31bbdaab9..62202dc44 100644 --- a/wrap/ReturnType.cpp +++ b/wrap/ReturnType.cpp @@ -77,27 +77,49 @@ void ReturnType::emit_cython_pxd(FileWriter& file, const std::string& className) file.oss << cythonType; } -/* ************************************************************************* */ -void ReturnType::emit_cython_pyx_return_type(FileWriter& file) const { +void ReturnType::emit_cython_pyx_return_type_noshared(FileWriter& file) const { string retType = pyxCythonClass(); if (isPtr) retType = "shared_ptr[" + retType + "]"; file.oss << retType; } + +/* ************************************************************************* */ +void ReturnType::emit_cython_pyx_return_type(FileWriter& file) const { + string retType = pyxCythonClass(); + if (isPtr || isNonBasicType()) retType = "shared_ptr[" + retType + "]"; + file.oss << retType; +} + +void ReturnType::emit_cython_pyx_casting_noshared(FileWriter& file, const std::string& var) const { + if (isEigen()) + file.oss << "ndarray_copy" << "(" << var << ")"; + else if (isNonBasicType()) { + if (isPtr) + file.oss << pythonClass() << ".cyCreateFromShared" << "(" << var << ")"; + else { + file.oss << pythonClass() << ".cyCreateFromShared(" + << pyxSharedCythonClass() + << "(new " << pyxCythonClass() << "(" << var << ")))"; + } + } else file.oss << var; +} + /* ************************************************************************* */ void ReturnType::emit_cython_pyx_casting(FileWriter& file, const std::string& var) const { if (isEigen()) - file.oss << "ndarray_copy"; - else if (isNonBasicType()){ - if (isPtr) - file.oss << pythonClass() << ".cyCreateFromShared"; - else { - // if the function return an object, it must be copy constructible and copy assignable - // so it's safe to use cyCreateFromValue - file.oss << pythonClass() << ".cyCreateFromValue"; - } - } - file.oss << "(" << var << ")"; + file.oss << "ndarray_copy" << "(" << var << ")"; + else if (isNonBasicType()) { + // if (isPtr) + file.oss << pythonClass() << ".cyCreateFromShared" << "(" << var << ")"; + // else { + // // if the function return an object, it must be copy constructible and copy assignable + // // so it's safe to use cyCreateFromValue + // file.oss << pythonClass() << ".cyCreateFromShared(" + // << pyxSharedCythonClass() + // << "(new " << pyxCythonClass() << "(" << var << ")))"; + // } + } else file.oss << var; } /* ************************************************************************* */ diff --git a/wrap/ReturnType.h b/wrap/ReturnType.h index bc0eebf95..8d224976f 100644 --- a/wrap/ReturnType.h +++ b/wrap/ReturnType.h @@ -50,6 +50,8 @@ struct ReturnType: public Qualified { void emit_cython_pxd(FileWriter& file, const std::string& className) const; void emit_cython_pyx_return_type(FileWriter& file) const; void emit_cython_pyx_casting(FileWriter& file, const std::string& var) const; + void emit_cython_pyx_return_type_noshared(FileWriter& file) const; + void emit_cython_pyx_casting_noshared(FileWriter& file, const std::string& var) const; private: diff --git a/wrap/ReturnValue.cpp b/wrap/ReturnValue.cpp index 4575dac97..4f8cef4e3 100644 --- a/wrap/ReturnValue.cpp +++ b/wrap/ReturnValue.cpp @@ -86,9 +86,9 @@ void ReturnValue::emit_cython_pyx_return_type(FileWriter& file) const { if (isVoid()) return; if (isPair) { file.oss << "pair ["; - type1.emit_cython_pyx_return_type(file); + type1.emit_cython_pyx_return_type_noshared(file); file.oss << ","; - type2.emit_cython_pyx_return_type(file); + type2.emit_cython_pyx_return_type_noshared(file); file.oss << "]"; } else { type1.emit_cython_pyx_return_type(file); @@ -100,9 +100,9 @@ void ReturnValue::emit_cython_pyx_casting(FileWriter& file, const std::string& v if (isVoid()) return; if (isPair) { file.oss << "("; - type1.emit_cython_pyx_casting(file, var + ".first"); + type1.emit_cython_pyx_casting_noshared(file, var + ".first"); file.oss << ","; - type2.emit_cython_pyx_casting(file, var + ".second"); + type2.emit_cython_pyx_casting_noshared(file, var + ".second"); file.oss << ")"; } else { type1.emit_cython_pyx_casting(file, var); diff --git a/wrap/tests/cythontest.h b/wrap/tests/cythontest.h index 8aa4bbd88..8bf097153 100644 --- a/wrap/tests/cythontest.h +++ b/wrap/tests/cythontest.h @@ -46,6 +46,7 @@ virtual class Value { class LieScalar { // Standard constructors LieScalar(); + LieScalar(const gtsam::LieScalar& l); LieScalar(double d); // Standard interface @@ -75,6 +76,7 @@ class LieScalar { class LieVector { // Standard constructors LieVector(); + LieVector(const gtsam::LieVector& l); LieVector(Vector v); // Standard interface @@ -108,6 +110,7 @@ class LieMatrix { // Standard constructors LieMatrix(); LieMatrix(Matrix v); + LieMatrix(const gtsam::LieMatrix& l); // Standard interface Matrix matrix() const; @@ -145,6 +148,7 @@ class Point2 { Point2(); Point2(double x, double y); Point2(Vector v); + Point2(const gtsam::Point2& l); // Testable void print(string s) const; @@ -196,6 +200,7 @@ class StereoPoint2 { // Standard Constructors StereoPoint2(); StereoPoint2(double uL, double uR, double v); + StereoPoint2(const gtsam::StereoPoint2& l); // Testable void print(string s) const; @@ -231,6 +236,7 @@ class Point3 { Point3(); Point3(double x, double y, double z); Point3(Vector v); + Point3(const gtsam::Point3& l); // Testable void print(string s) const; @@ -254,6 +260,8 @@ class Rot2 { // Standard Constructors and Named Constructors Rot2(); Rot2(double theta); + Rot2(const gtsam::Rot2& l); + static gtsam::Rot2 fromAngle(double theta); static gtsam::Rot2 fromDegrees(double theta); static gtsam::Rot2 fromCosSin(double c, double s); @@ -298,6 +306,8 @@ class Rot3 { // Standard Constructors and Named Constructors Rot3(); Rot3(Matrix R); + Rot3(const gtsam::Rot3& l); + static gtsam::Rot3 Rx(double t); static gtsam::Rot3 Ry(double t); static gtsam::Rot3 Rz(double t); @@ -451,6 +461,8 @@ class Pose3 { class Pose3Vector { Pose3Vector(); + Pose3Vector(const gtsam::Pose3Vector& l); + size_t size() const; bool empty() const; gtsam::Pose3 at(size_t n) const; @@ -462,6 +474,7 @@ class Unit3 { // Standard Constructors Unit3(); Unit3(const gtsam::Point3& pose); + Unit3(const gtsam::Unit3& pose); // Testable void print(string s) const; @@ -483,6 +496,7 @@ class EssentialMatrix { EssentialMatrix(); // Standard Constructors EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); + EssentialMatrix(const gtsam::EssentialMatrix& pose); // Testable void print(string s) const; @@ -508,6 +522,7 @@ class 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); + Cal3_S2(const gtsam::Cal3_S2& pose); // Testable void print(string s) const; @@ -542,6 +557,7 @@ class Cal3_S2 { virtual class Cal3DS2_Base { // Standard Constructors Cal3DS2_Base(); + Cal3DS2_Base(const gtsam::Cal3DS2_Base& pose); // Testable void print(string s) const; @@ -571,6 +587,7 @@ virtual class Cal3DS2 : gtsam::Cal3DS2_Base { 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); + Cal3DS2(const gtsam::Cal3DS2& pose); // Testable bool equals(const gtsam::Cal3DS2& rhs, double tol) const; @@ -592,6 +609,7 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base { 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); + Cal3Unified(const gtsam::Cal3Unified& pose); // Testable bool equals(const gtsam::Cal3Unified& rhs, double tol) const; @@ -617,6 +635,7 @@ class Cal3_S2Stereo { Cal3_S2Stereo(); Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); Cal3_S2Stereo(Vector v); + Cal3_S2Stereo(const gtsam::Cal3_S2Stereo& pose); // Testable void print(string s) const; @@ -637,6 +656,7 @@ class Cal3Bundler { // Standard Constructors Cal3Bundler(); Cal3Bundler(double fx, double k1, double k2, double u0, double v0); + Cal3Bundler(const gtsam::Cal3Bundler& pose); // Testable void print(string s) const; @@ -675,6 +695,7 @@ class CalibratedCamera { CalibratedCamera(const gtsam::Pose3& pose); CalibratedCamera(const Vector& v); static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); + CalibratedCamera(const gtsam::CalibratedCamera& pose); // Testable void print(string s) const; @@ -747,6 +768,7 @@ virtual class SimpleCamera { 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); + SimpleCamera(const gtsam::SimpleCamera& other); // Testable void print(string s) const; @@ -787,6 +809,7 @@ class StereoCamera { // Standard Constructors and Named Constructors StereoCamera(); StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); + StereoCamera(const gtsam::StereoCamera& other); // Testable void print(string s) const; @@ -874,6 +897,7 @@ virtual class SymbolicFactorGraph { SymbolicFactorGraph(); SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet); SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree); + SymbolicFactorGraph(const gtsam::SymbolicFactorGraph& other); // From FactorGraph void push_back(gtsam::SymbolicFactor* factor); @@ -959,14 +983,14 @@ class SymbolicBayesTree { //Constructors SymbolicBayesTree(); - SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); + 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 findParentClique(const gtsam::IndexVector& parents) const; size_t size(); void saveGraph(string s) const; void clear(); @@ -1098,6 +1122,7 @@ virtual class Base { virtual class Null: gtsam::noiseModel::mEstimator::Base { Null(); + Null(const gtsam::noiseModel::mEstimator::Null& other); void print(string s) const; static gtsam::noiseModel::mEstimator::Null* Create(); @@ -1107,6 +1132,7 @@ virtual class Null: gtsam::noiseModel::mEstimator::Base { virtual class Fair: gtsam::noiseModel::mEstimator::Base { Fair(double c); + Fair(const gtsam::noiseModel::mEstimator::Fair& other); void print(string s) const; static gtsam::noiseModel::mEstimator::Fair* Create(double c); @@ -1116,6 +1142,8 @@ virtual class Fair: gtsam::noiseModel::mEstimator::Base { virtual class Huber: gtsam::noiseModel::mEstimator::Base { Huber(double k); + Huber(const gtsam::noiseModel::mEstimator::Huber& other); + void print(string s) const; static gtsam::noiseModel::mEstimator::Huber* Create(double k); @@ -1125,6 +1153,8 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base { virtual class Tukey: gtsam::noiseModel::mEstimator::Base { Tukey(double k); + Tukey(const gtsam::noiseModel::mEstimator::Tukey& other); + void print(string s) const; static gtsam::noiseModel::mEstimator::Tukey* Create(double k); @@ -1136,6 +1166,8 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { virtual class Robust : gtsam::noiseModel::Base { Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); + Robust(const gtsam::noiseModel::Robust& other); + static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); void print(string s) const; @@ -1151,6 +1183,8 @@ class Sampler { Sampler(gtsam::noiseModel::Diagonal* model, int seed); Sampler(Vector sigmas, int seed); Sampler(int seed); + Sampler(const gtsam::Sampler& other); + //Standard Interface size_t dim() const; @@ -1227,6 +1261,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor { 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); + JacobianFactor(const gtsam::JacobianFactor& other); //Testable void print(string s) const; @@ -1272,6 +1307,7 @@ virtual class HessianFactor : gtsam::GaussianFactor { Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, double f); HessianFactor(const gtsam::GaussianFactorGraph& factors); + HessianFactor(const gtsam::HessianFactor& other); //Testable size_t size() const; @@ -1295,6 +1331,7 @@ class GaussianFactorGraph { GaussianFactorGraph(); GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet); GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); + GaussianFactorGraph(const gtsam::GaussianFactorGraph& other); // From FactorGraph void print(string s) const; @@ -1382,6 +1419,7 @@ virtual class GaussianConditional : gtsam::GaussianFactor { 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); + GaussianConditional(const gtsam::GaussianConditional& other); //Standard Interface void print(string s) const; @@ -1401,6 +1439,7 @@ virtual class GaussianConditional : gtsam::GaussianFactor { virtual class GaussianDensity : gtsam::GaussianConditional { //Constructors GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); + GaussianDensity(const gtsam::GaussianDensity& other); //Standard Interface void print(string s) const; @@ -1414,6 +1453,7 @@ virtual class GaussianBayesNet { //Constructors GaussianBayesNet(); GaussianBayesNet(const gtsam::GaussianConditional* conditional); + GaussianBayesNet(const gtsam::GaussianBayesNet& other); // Testable void print(string s) const; @@ -1473,6 +1513,7 @@ class Errors { //Constructors Errors(); Errors(const gtsam::VectorValues& V); + Errors(const gtsam::Errors& other); //Testable void print(string s); @@ -1483,6 +1524,7 @@ class Errors { class GaussianISAM { //Constructor GaussianISAM(); + GaussianISAM(const gtsam::GaussianISAM& other); //Standard Interface void update(const gtsam::GaussianFactorGraph& newFactors); @@ -1492,6 +1534,7 @@ class GaussianISAM { #include virtual class IterativeOptimizationParameters { + IterativeOptimizationParameters(const gtsam::IterativeOptimizationParameters& other); string getVerbosity() const; void setVerbosity(string s) ; void print() const; @@ -1505,6 +1548,8 @@ virtual class IterativeOptimizationParameters { #include virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters { ConjugateGradientParameters(); + ConjugateGradientParameters(const gtsam::ConjugateGradientParameters& other); + int getMinIterations() const ; int getMaxIterations() const ; int getReset() const; @@ -1522,18 +1567,23 @@ virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParamete #include virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { SubgraphSolverParameters(); + SubgraphSolverParameters(const gtsam::SubgraphSolverParameters& other); void print() const; }; virtual class SubgraphSolver { SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); + SubgraphSolver(const gtsam::SubgraphSolver& other); + gtsam::VectorValues optimize() const; }; #include class KalmanFilter { KalmanFilter(size_t n); + KalmanFilter(const gtsam::KalmanFilter& other); + // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); gtsam::GaussianDensity* init(Vector x0, Matrix P0); void print(string s) const; @@ -1550,436 +1600,170 @@ class KalmanFilter { Vector z, Matrix Q); }; -// //************************************************************************* -// // nonlinear -// //************************************************************************* - -// #include -// 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 -// 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 -// 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; - -// template -// void insert(size_t j, const T& t); - -// template -// void update(size_t j, const T& t); - -// template -// T at(size_t j); - -// /// version for double -// void insertDouble(size_t j, double c); -// double atDouble(size_t j) const; -// }; - -// #include -// 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 -// }; - -// #include -// 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& constraints) const; -// gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const; -// gtsam::NonlinearFactorGraph clone() const; - -// // enabling serialization functionality -// void serialize() const; -// }; - -// #include -// 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; -// }; - -// // // Actually a FastList -// // #include -// // 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 -// // 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 -// // 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 -// // 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 -// 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 -// virtual class LinearContainerFactor : gtsam::NonlinearFactor { - -// LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint); -// LinearContainerFactor(gtsam::GaussianFactor* factor); - -// gtsam::GaussianFactor* factor() const; -// // const boost::optional& 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 -// // -// //// 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 -// 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 -// virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { -// GaussNewtonParams(); -// }; - -// #include -// 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 -// virtual class DoglegParams : gtsam::NonlinearOptimizerParams { -// DoglegParams(); - -// double getDeltaInitial() const; -// string getVerbosityDL() const; - -// void setDeltaInitial(double deltaInitial) const; -// void setVerbosityDL(string verbosityDL) const; -// }; - -// #include -// virtual class NonlinearOptimizer { -// gtsam::Values optimize(); -// gtsam::Values optimizeSafely(); -// double error() const; -// int iterations() const; -// gtsam::Values values() const; -// void iterate() const; -// }; - -// #include -// 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); -// }; - -// #include -// 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; -// }; - -// #include -// 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 -// 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); +//************************************************************************* +// nonlinear +//************************************************************************* + +#include +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 +class LabeledSymbol { + 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 +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; + + template + void insert(size_t j, const T& t); + + template + void update(size_t j, const T& t); + + template + T at(size_t j); + + /// version for double + void insertDouble(size_t j, double c); + double atDouble(size_t j) const; +}; + +#include +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 + bool 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 +}; + +#include +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& constraints) const; + gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const; + gtsam::NonlinearFactorGraph clone() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +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; +}; + +// // Actually a FastList +// #include +// class KeyList { +// KeyList(); +// KeyList(const gtsam::KeyList& other); // // Note: no print function @@ -1989,573 +1773,890 @@ class KalmanFilter { // void clear(); // // structure specific methods -// void insert(const gtsam::ISAM2ThresholdMapValue& value) const; -// }; +// 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); -// 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& constrainedKeys); -// //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& 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 -// 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 -// #include -// #include - -// #include -// template -// 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 -// template -// 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 -// template -// 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 -// template -// virtual class RangeFactor : gtsam::NoiseModelFactor { -// RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); -// }; - -// typedef gtsam::RangeFactor RangeFactorPosePoint2; -// typedef gtsam::RangeFactor RangeFactorPosePoint3; -// typedef gtsam::RangeFactor RangeFactorPose2; -// typedef gtsam::RangeFactor RangeFactorPose3; -// typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; -// typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; -// typedef gtsam::RangeFactor RangeFactorCalibratedCamera; -// typedef gtsam::RangeFactor RangeFactorSimpleCamera; - - -// #include -// template -// 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 BearingFactor2D; - -// #include -// template -// 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 BearingRangeFactor2D; - - -// #include -// template -// 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 GenericProjectionFactorCal3_S2; -// typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; - - -// #include -// template -// 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 GeneralSFMFactorCal3_S2; -// typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; - -// template -// 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 -// 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 -// template -// 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 SmartProjectionPose3Factor; - - -// #include -// template -// 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 GenericStereoFactor3D; - -// #include -// template -// virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { -// PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); -// }; - -// typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; -// typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; - -// #include -// template -// virtual class PoseRotationPrior : gtsam::NoiseModelFactor { -// PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); -// }; - -// typedef gtsam::PoseRotationPrior PoseRotationPrior2D; -// typedef gtsam::PoseRotationPrior PoseRotationPrior3D; - -// #include -// virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { -// EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, const gtsam::Point2& pB, -// const gtsam::noiseModel::Base* noiseModel); -// }; - -// #include -// pair load2D(string filename, -// gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); -// pair load2D(string filename, -// gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise); -// pair load2D(string filename, -// gtsam::noiseModel::Diagonal* model, int maxID); -// pair load2D(string filename, -// gtsam::noiseModel::Diagonal* model); -// pair load2D(string filename); -// pair 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 readG2o(string filename); -// void writeG2o(const gtsam::NonlinearFactorGraph& graph, -// const gtsam::Values& estimate, string filename); - -// //************************************************************************* -// // Navigation -// //************************************************************************* -// namespace imuBias { -// #include - -// class ConstantBias { -// // Constructors -// ConstantBias(); -// ConstantBias(Vector biasAcc, Vector biasGyro); +// // Actually a FastSet +// 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::imuBias::ConstantBias& expected, double tol) const; +// bool equals(const gtsam::KeySet& other) 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; +// // common STL methods +// size_t size() const; +// bool empty() const; +// void clear(); -// // Manifold -// gtsam::imuBias::ConstantBias retract(Vector v) const; -// Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; +// // 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 -// // 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; +// void serialize() const; // }; -// }///\namespace imuBias +// // Actually a vector +// class KeyVector { +// KeyVector(); +// KeyVector(const gtsam::KeyVector& other); -// #include -// class NavState { -// // Constructors -// NavState(); -// NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v); -// NavState(const gtsam::Pose3& pose, Vector v); +// // Note: no print function -// // Testable -// void print(string s) const; -// bool equals(const gtsam::NavState& expected, double tol) const; +// // common STL methods +// size_t size() const; +// bool empty() const; +// void clear(); -// // Access -// gtsam::Rot3 attitude() const; -// gtsam::Point3 position() const; -// Vector velocity() const; -// gtsam::Pose3 pose() const; +// // 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; // }; -// #include -// virtual class PreintegratedRotationParams { -// PreintegratedRotationParams(); -// void setGyroscopeCovariance(Matrix cov); -// void setOmegaCoriolis(Vector omega); -// void setBodyPSensor(const gtsam::Pose3& pose); +// // Actually a FastMap +// class KeyGroupMap { +// KeyGroupMap(); -// Matrix getGyroscopeCovariance() const; +// // Note: no print function -// // TODO(frank): allow optional -// // boost::optional getOmegaCoriolis() const; -// // boost::optional getBodyPSensor() const; +// // 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 -// virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { -// PreintegrationParams(Vector n_gravity); -// void setAccelerometerCovariance(Matrix cov); -// void setIntegrationCovariance(Matrix cov); -// void setUse2ndOrderCoriolis(bool flag); +typedef gtsam::FastMap KeyGroupMap; -// Matrix getAccelerometerCovariance() const; -// Matrix getIntegrationCovariance() const; -// bool getUse2ndOrderCoriolis() const; -// }; +#include +class JointMarginal { + JointMarginal(const JointMarginal& j); + Matrix at(size_t iVariable, size_t jVariable) const; + Matrix fullMatrix() const; + void print(string s) const; + void print() const; +}; -// #include -// class PreintegratedImuMeasurements { -// // Constructors -// PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params); -// PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params, -// const gtsam::imuBias::ConstantBias& bias); +class Marginals { + Marginals(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& solution); + Marginals(const gtsam::Marginals& other); -// // Testable -// void print(string s) const; -// bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); + 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; +}; -// // 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 +virtual class LinearContainerFactor : gtsam::NonlinearFactor { + LinearContainerFactor(const gtsam::LinearContainerFactor& other); -// #include -// class PreintegratedCombinedMeasurements { -// // Testable -// void print(string s) const; -// bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, -// double tol); + LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint); + LinearContainerFactor(gtsam::GaussianFactor* factor); -// // 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; -// }; + gtsam::GaussianFactor* factor() const; +// const boost::optional& linearizationPoint() 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); + bool isJacobian() const; + gtsam::JacobianFactor* toJacobian() const; + gtsam::HessianFactor* toHessian() const; -// // 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); -// }; + static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, + const gtsam::Values& linearizationPoint); -// #include -// class PreintegratedAhrsMeasurements { -// // Standard Constructor -// PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); -// PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); -// PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); + static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph); -// // Testable -// void print(string s) const; -// bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol); + // enabling serialization functionality + void serializable() const; +}; // \class LinearContainerFactor -// // get Data -// gtsam::Rot3 deltaRij() const; -// double deltaTij() const; -// Vector biasHat() const; +// Summarization functionality +//#include +// +//// 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); -// // Standard Interface -// void integrateMeasurement(Vector measuredOmega, double deltaT); -// void resetIntegration() ; -// }; +//************************************************************************* +// Nonlinear optimizers +//************************************************************************* +#include +virtual class NonlinearOptimizerParams { + NonlinearOptimizerParams(); + NonlinearOptimizerParams(const gtsam::NonlinearOptimizerParams& other); + void print(string s) const; -// 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); + int getMaxIterations() const; + double getRelativeErrorTol() const; + double getAbsoluteErrorTol() const; + double getErrorTol() const; + string getVerbosity() const; -// // 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; -// }; + void setMaxIterations(int value); + void setRelativeErrorTol(double value); + void setAbsoluteErrorTol(double value); + void setErrorTol(double value); + void setVerbosity(string s); -// #include -// //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; -// }; + string getLinearSolverType() 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; -// }; + void setLinearSolverType(string solver); + void setOrdering(const gtsam::Ordering& ordering); + void setIterativeParams(gtsam::IterativeOptimizationParameters* params); -// //************************************************************************* -// // Utilities -// //************************************************************************* + bool isMultifrontal() const; + bool isSequential() const; + bool isCholmod() const; + bool isIterative() const; +}; -// namespace utilities { +bool checkConvergence(double relativeErrorTreshold, + double absoluteErrorTreshold, double errorThreshold, + double currentError, double newError); -// #include -// 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); +#include +virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { + GaussNewtonParams(); + GaussNewtonParams(const gtsam::GaussNewtonParams& other); +}; -// } //\namespace utilities +#include +virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { + LevenbergMarquardtParams(); + LevenbergMarquardtParams(const gtsam::LevenbergMarquardtParams& other); + + 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 +virtual class DoglegParams : gtsam::NonlinearOptimizerParams { + DoglegParams(); + DoglegParams(const gtsam::DoglegParams& other); + + double getDeltaInitial() const; + string getVerbosityDL() const; + + void setDeltaInitial(double deltaInitial) const; + void setVerbosityDL(string verbosityDL) const; +}; + +#include +virtual class NonlinearOptimizer { + gtsam::Values optimize(); + gtsam::Values optimizeSafely(); + double error() const; + int iterations() const; + gtsam::Values values() const; + void iterate() const; +}; + +#include +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); +}; + +#include +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; +}; + +#include +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 +class ISAM2GaussNewtonParams { + ISAM2GaussNewtonParams(); + ISAM2GaussNewtonParams(const gtsam::ISAM2GaussNewtonParams& other); + + void print(string str) const; + + /** Getters and Setters for all properties */ + double getWildfireThreshold() const; + void setWildfireThreshold(double wildfireThreshold); +}; + +class ISAM2DoglegParams { + ISAM2DoglegParams(); + ISAM2DoglegParams(const gtsam::ISAM2DoglegParams& other); + + 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(); + ISAM2Params(const gtsam::ISAM2Params& other); + + 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(); + ISAM2Clique(const gtsam::ISAM2Clique& other); + + //Standard Interface + Vector gradientContribution() const; + void print(string s); +}; + +class ISAM2Result { + ISAM2Result(); + ISAM2Result(const gtsam::ISAM2Result& other); + + 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& constrainedKeys); + //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys, bool force_relinearize); + + gtsam::Values getLinearizationPoint() const; + gtsam::Values calculateEstimate() const; + template + 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 +class NonlinearISAM { + NonlinearISAM(); + NonlinearISAM(int reorderInterval); + NonlinearISAM(const gtsam::NonlinearISAM& other); + + 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 +#include +#include + +#include +template +virtual class PriorFactor : gtsam::NoiseModelFactor { + PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); + PriorFactor(const This& other); + T prior() const; + + // enabling serialization functionality + void serialize() const; +}; + + +#include +template +virtual class BetweenFactor : gtsam::NoiseModelFactor { + BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); + BetweenFactor(const This& other); + T measured() const; + + // enabling serialization functionality + void serialize() const; +}; + + + +#include +template +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); + NonlinearEquality(const This& other); + + // enabling serialization functionality + void serialize() const; +}; + + +#include +template +virtual class RangeFactor : gtsam::NoiseModelFactor { + RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); + RangeFactor(const This& other); +}; + +typedef gtsam::RangeFactor RangeFactorPosePoint2; +typedef gtsam::RangeFactor RangeFactorPosePoint3; +typedef gtsam::RangeFactor RangeFactorPose2; +typedef gtsam::RangeFactor RangeFactorPose3; +typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; +typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; +typedef gtsam::RangeFactor RangeFactorCalibratedCamera; +typedef gtsam::RangeFactor RangeFactorSimpleCamera; + + +#include +template +virtual class BearingFactor : gtsam::NoiseModelFactor { + BearingFactor(size_t key1, size_t key2, const BEARING& measured, const gtsam::noiseModel::Base* noiseModel); + BearingFactor(const This& other); + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::BearingFactor BearingFactor2D; + +#include +template +virtual class BearingRangeFactor : gtsam::NoiseModelFactor { + BearingRangeFactor(size_t poseKey, size_t pointKey, + const BEARING& measuredBearing, const RANGE& measuredRange, + const gtsam::noiseModel::Base* noiseModel); + BearingRangeFactor(const This& other); + + // enabling serialization functionality + void serialize() const; +}; + +typedef gtsam::BearingRangeFactor BearingRangeFactor2D; + + +#include +template +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); + GenericProjectionFactor(const This& other); + + gtsam::Point2 measured() const; + CALIBRATION* calibration() const; + bool verboseCheirality() const; + bool throwCheirality() const; + + // enabling serialization functionality + void serialize() const; +}; +typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; +typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; + + +#include +template +virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { + GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey); + GeneralSFMFactor(const This& other); + gtsam::Point2 measured() const; +}; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; +typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; + +template +virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { + GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey); + GeneralSFMFactor2(const This& other); + gtsam::Point2 measured() const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +class SmartProjectionParams { + SmartProjectionParams(); + SmartProjectionParams(const gtsam::SmartProjectionParams& other); + // 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 +template +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); + SmartProjectionPoseFactor(const This& other); + + void add(const gtsam::Point2& measured_i, size_t poseKey_i); + + // enabling serialization functionality + //void serialize() const; +}; + +typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; + + +#include +template +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); + GenericStereoFactor(const This& other); + gtsam::StereoPoint2 measured() const; + gtsam::Cal3_S2Stereo* calibration() const; + + // enabling serialization functionality + void serialize() const; +}; +typedef gtsam::GenericStereoFactor GenericStereoFactor3D; + +#include +template +virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { + PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); + PoseTranslationPrior(const This& other); +}; + +typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; +typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; + +#include +template +virtual class PoseRotationPrior : gtsam::NoiseModelFactor { + PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); + PoseRotationPrior(const This& other); +}; + +typedef gtsam::PoseRotationPrior PoseRotationPrior2D; +typedef gtsam::PoseRotationPrior PoseRotationPrior3D; + +#include +virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { + EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, const gtsam::Point2& pB, + const gtsam::noiseModel::Base* noiseModel); + EssentialMatrixFactor(const gtsam::EssentialMatrixFactor& other); +}; + +#include +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model, int maxID); +pair load2D(string filename, + gtsam::noiseModel::Diagonal* model); +pair load2D(string filename); +pair 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 readG2o(string filename); +void writeG2o(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& estimate, string filename); + +//************************************************************************* +// Navigation +//************************************************************************* +namespace imuBias { +#include + +class ConstantBias { + // Constructors + ConstantBias(); + ConstantBias(Vector biasAcc, Vector biasGyro); + ConstantBias(const gtsam::imuBias::ConstantBias& other); + + // 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 +class NavState { + // Constructors + NavState(); + NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v); + NavState(const gtsam::Pose3& pose, Vector v); + NavState(const gtsam::NavState& other); + + // 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 +virtual class PreintegratedRotationParams { + PreintegratedRotationParams(); + PreintegratedRotationParams(const gtsam::PreintegratedRotationParams& other); + void setGyroscopeCovariance(Matrix cov); + void setOmegaCoriolis(Vector omega); + void setBodyPSensor(const gtsam::Pose3& pose); + + Matrix getGyroscopeCovariance() const; + + // TODO(frank): allow optional + // boost::optional getOmegaCoriolis() const; + // boost::optional getBodyPSensor() const; +}; + +#include +virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { + PreintegrationParams(Vector n_gravity); + PreintegrationParams(const gtsam::PreintegrationParams& other); + void setAccelerometerCovariance(Matrix cov); + void setIntegrationCovariance(Matrix cov); + void setUse2ndOrderCoriolis(bool flag); + + Matrix getAccelerometerCovariance() const; + Matrix getIntegrationCovariance() const; + bool getUse2ndOrderCoriolis() const; +}; + +#include +class PreintegratedImuMeasurements { + // Constructors + PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params); + PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params, + const gtsam::imuBias::ConstantBias& bias); + PreintegratedImuMeasurements(const gtsam::PreintegratedImuMeasurements& other); + + // 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); + ImuFactor(const gtsam::ImuFactor& other); + + // 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 +class PreintegratedCombinedMeasurements { + PreintegratedCombinedMeasurements(const gtsam::PreintegratedCombinedMeasurements& other); + // 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); + CombinedImuFactor(const gtsam::CombinedImuFactor& other); + + // 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 +class PreintegratedAhrsMeasurements { + // Standard Constructor + 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); + AHRSFactor(const gtsam::AHRSFactor& rhs); + + // 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 +//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(); + Rot3AttitudeFactor(const gtsam::Rot3AttitudeFactor& rhs); + + 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(); + Pose3AttitudeFactor(const gtsam::Pose3AttitudeFactor& rhs); + 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 + 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 }