Merge pull request #750 from borglab/feature/wrap-update

release/4.3a0
Frank Dellaert 2021-04-21 08:33:08 -04:00 committed by GitHub
commit 301f7fe1a3
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 241 additions and 122 deletions

View File

@ -47,7 +47,7 @@ class KeySet {
KeySet(const gtsam::KeyList& list); KeySet(const gtsam::KeyList& list);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::KeySet& other) const; bool equals(const gtsam::KeySet& other) const;
// common STL methods // common STL methods
@ -221,7 +221,7 @@ virtual class Value {
// No constructors because this is an abstract class // No constructors because this is an abstract class
// Testable // Testable
void print(string s) const; void print(string s="") const;
// Manifold // Manifold
size_t dim() const; size_t dim() const;
@ -245,7 +245,7 @@ class Point2 {
Point2(Vector v); Point2(Vector v);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::Point2& point, double tol) const; bool equals(const gtsam::Point2& point, double tol) const;
// Group // Group
@ -298,7 +298,7 @@ class StereoPoint2 {
StereoPoint2(double uL, double uR, double v); StereoPoint2(double uL, double uR, double v);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::StereoPoint2& point, double tol) const; bool equals(const gtsam::StereoPoint2& point, double tol) const;
// Group // Group
@ -342,7 +342,7 @@ class Point3 {
Point3(Vector v); Point3(Vector v);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::Point3& p, double tol) const; bool equals(const gtsam::Point3& p, double tol) const;
// Group // Group
@ -379,7 +379,7 @@ class Rot2 {
static gtsam::Rot2 fromCosSin(double c, double s); static gtsam::Rot2 fromCosSin(double c, double s);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::Rot2& rot, double tol) const; bool equals(const gtsam::Rot2& rot, double tol) const;
// Group // Group
@ -430,7 +430,7 @@ class SO3 {
static gtsam::SO3 ClosestTo(const Matrix M); static gtsam::SO3 ClosestTo(const Matrix M);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::SO3& other, double tol) const; bool equals(const gtsam::SO3& other, double tol) const;
// Group // Group
@ -460,7 +460,7 @@ class SO4 {
static gtsam::SO4 FromMatrix(Matrix R); static gtsam::SO4 FromMatrix(Matrix R);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::SO4& other, double tol) const; bool equals(const gtsam::SO4& other, double tol) const;
// Group // Group
@ -490,7 +490,7 @@ class SOn {
static gtsam::SOn Lift(size_t n, Matrix R); static gtsam::SOn Lift(size_t n, Matrix R);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::SOn& other, double tol) const; bool equals(const gtsam::SOn& other, double tol) const;
// Group // Group
@ -551,7 +551,7 @@ class Rot3 {
static gtsam::Rot3 ClosestTo(const Matrix M); static gtsam::Rot3 ClosestTo(const Matrix M);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::Rot3& rot, double tol) const; bool equals(const gtsam::Rot3& rot, double tol) const;
// Group // Group
@ -608,7 +608,7 @@ class Pose2 {
Pose2(Vector v); Pose2(Vector v);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::Pose2& pose, double tol) const; bool equals(const gtsam::Pose2& pose, double tol) const;
// Group // Group
@ -668,7 +668,7 @@ class Pose3 {
Pose3(Matrix mat); Pose3(Matrix mat);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::Pose3& pose, double tol) const; bool equals(const gtsam::Pose3& pose, double tol) const;
// Group // Group
@ -744,7 +744,7 @@ class Unit3 {
Unit3(const gtsam::Point3& pose); Unit3(const gtsam::Point3& pose);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::Unit3& pose, double tol) const; bool equals(const gtsam::Unit3& pose, double tol) const;
// Other functionality // Other functionality
@ -774,7 +774,7 @@ class EssentialMatrix {
EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::EssentialMatrix& pose, double tol) const; bool equals(const gtsam::EssentialMatrix& pose, double tol) const;
// Manifold // Manifold
@ -799,7 +799,7 @@ class Cal3_S2 {
Cal3_S2(double fov, int w, int h); Cal3_S2(double fov, int w, int h);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::Cal3_S2& rhs, double tol) const; bool equals(const gtsam::Cal3_S2& rhs, double tol) const;
// Manifold // Manifold
@ -836,7 +836,7 @@ virtual class Cal3DS2_Base {
Cal3DS2_Base(); Cal3DS2_Base();
// Testable // Testable
void print(string s) const; void print(string s="") const;
// Standard Interface // Standard Interface
double fx() const; double fx() const;
@ -922,7 +922,7 @@ class Cal3_S2Stereo {
Cal3_S2Stereo(Vector v); Cal3_S2Stereo(Vector v);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const; bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const;
// Standard Interface // Standard Interface
@ -943,7 +943,7 @@ class Cal3Bundler {
Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol); Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::Cal3Bundler& rhs, double tol) const; bool equals(const gtsam::Cal3Bundler& rhs, double tol) const;
// Manifold // Manifold
@ -983,7 +983,7 @@ class CalibratedCamera {
static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::CalibratedCamera& camera, double tol) const; bool equals(const gtsam::CalibratedCamera& camera, double tol) const;
// Manifold // Manifold
@ -1022,7 +1022,7 @@ class PinholeCamera {
const gtsam::Point3& upVector, const CALIBRATION& K); const gtsam::Point3& upVector, const CALIBRATION& K);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const This& camera, double tol) const; bool equals(const This& camera, double tol) const;
// Standard Interface // Standard Interface
@ -1097,7 +1097,7 @@ class StereoCamera {
StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::StereoCamera& camera, double tol) const; bool equals(const gtsam::StereoCamera& camera, double tol) const;
// Standard Interface // Standard Interface
@ -1160,7 +1160,7 @@ virtual class SymbolicFactor {
// From Factor // From Factor
size_t size() const; size_t size() const;
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::SymbolicFactor& other, double tol) const; bool equals(const gtsam::SymbolicFactor& other, double tol) const;
gtsam::KeyVector keys(); gtsam::KeyVector keys();
}; };
@ -1173,7 +1173,7 @@ virtual class SymbolicFactorGraph {
// From FactorGraph // From FactorGraph
void push_back(gtsam::SymbolicFactor* factor); void push_back(gtsam::SymbolicFactor* factor);
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const;
size_t size() const; size_t size() const;
bool exists(size_t idx) const; bool exists(size_t idx) const;
@ -1223,7 +1223,7 @@ virtual class SymbolicConditional : gtsam::SymbolicFactor {
static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals); static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::SymbolicConditional& other, double tol) const; bool equals(const gtsam::SymbolicConditional& other, double tol) const;
// Standard interface // Standard interface
@ -1236,7 +1236,7 @@ class SymbolicBayesNet {
SymbolicBayesNet(); SymbolicBayesNet();
SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); SymbolicBayesNet(const gtsam::SymbolicBayesNet& other);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; bool equals(const gtsam::SymbolicBayesNet& other, double tol) const;
// Standard interface // Standard interface
@ -1257,7 +1257,7 @@ class SymbolicBayesTree {
SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); SymbolicBayesTree(const gtsam::SymbolicBayesTree& other);
// Testable // Testable
void print(string s); void print(string s="");
bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; bool equals(const gtsam::SymbolicBayesTree& other, double tol) const;
//Standard Interface //Standard Interface
@ -1279,7 +1279,7 @@ class SymbolicBayesTree {
// SymbolicBayesTreeClique(const pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {} // SymbolicBayesTreeClique(const pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
// //
// bool equals(const This& other, double tol) const; // bool equals(const This& other, double tol) const;
// void print(string s) const; // void print(string s="") const;
// void printTree() const; // Default indent of "" // void printTree() const; // Default indent of ""
// void printTree(string indent) const; // void printTree(string indent) const;
// size_t numCachedSeparatorMarginals() const; // size_t numCachedSeparatorMarginals() const;
@ -1313,7 +1313,7 @@ class VariableIndex {
// Testable // Testable
bool equals(const gtsam::VariableIndex& other, double tol) const; bool equals(const gtsam::VariableIndex& other, double tol) const;
void print(string s) const; void print(string s="") const;
// Standard interface // Standard interface
size_t size() const; size_t size() const;
@ -1328,7 +1328,7 @@ class VariableIndex {
namespace noiseModel { namespace noiseModel {
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
virtual class Base { virtual class Base {
void print(string s) const; void print(string s="") const;
// Methods below are available for all noise models. However, can't add them // Methods below are available for all noise models. However, can't add them
// because wrap (incorrectly) thinks robust classes derive from this Base as well. // because wrap (incorrectly) thinks robust classes derive from this Base as well.
// bool isConstrained() const; // bool isConstrained() const;
@ -1411,7 +1411,7 @@ virtual class Unit : gtsam::noiseModel::Isotropic {
namespace mEstimator { namespace mEstimator {
virtual class Base { virtual class Base {
void print(string s) const; void print(string s="") const;
}; };
virtual class Null: gtsam::noiseModel::mEstimator::Base { virtual class Null: gtsam::noiseModel::mEstimator::Base {
@ -1551,7 +1551,7 @@ class VectorValues {
size_t size() const; size_t size() const;
size_t dim(size_t j) const; size_t dim(size_t j) const;
bool exists(size_t j) const; bool exists(size_t j) const;
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::VectorValues& expected, double tol) const; bool equals(const gtsam::VectorValues& expected, double tol) const;
void insert(size_t j, Vector value); void insert(size_t j, Vector value);
Vector vector() const; Vector vector() const;
@ -1582,7 +1582,7 @@ class VectorValues {
#include <gtsam/linear/GaussianFactor.h> #include <gtsam/linear/GaussianFactor.h>
virtual class GaussianFactor { virtual class GaussianFactor {
gtsam::KeyVector keys() const; gtsam::KeyVector keys() const;
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const;
double error(const gtsam::VectorValues& c) const; double error(const gtsam::VectorValues& c) const;
gtsam::GaussianFactor* clone() const; gtsam::GaussianFactor* clone() const;
@ -1610,7 +1610,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
JacobianFactor(const gtsam::GaussianFactorGraph& graph); JacobianFactor(const gtsam::GaussianFactorGraph& graph);
//Testable //Testable
void print(string s) const; void print(string s="") const;
void printKeys(string s) const; void printKeys(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const;
size_t size() const; size_t size() const;
@ -1659,7 +1659,7 @@ virtual class HessianFactor : gtsam::GaussianFactor {
//Testable //Testable
size_t size() const; size_t size() const;
void print(string s) const; void print(string s="") const;
void printKeys(string s) const; void printKeys(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const;
double error(const gtsam::VectorValues& c) const; double error(const gtsam::VectorValues& c) const;
@ -1684,7 +1684,7 @@ class GaussianFactorGraph {
GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree);
// From FactorGraph // From FactorGraph
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const;
size_t size() const; size_t size() const;
gtsam::GaussianFactor* at(size_t idx) const; gtsam::GaussianFactor* at(size_t idx) const;
@ -1775,7 +1775,7 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
size_t name2, Matrix T); size_t name2, Matrix T);
//Standard Interface //Standard Interface
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::GaussianConditional &cg, double tol) const; bool equals(const gtsam::GaussianConditional &cg, double tol) const;
//Advanced Interface //Advanced Interface
@ -1797,7 +1797,7 @@ virtual class GaussianDensity : gtsam::GaussianConditional {
GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas);
//Standard Interface //Standard Interface
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::GaussianDensity &cg, double tol) const; bool equals(const gtsam::GaussianDensity &cg, double tol) const;
Vector mean() const; Vector mean() const;
Matrix covariance() const; Matrix covariance() const;
@ -1810,7 +1810,7 @@ virtual class GaussianBayesNet {
GaussianBayesNet(const gtsam::GaussianConditional* conditional); GaussianBayesNet(const gtsam::GaussianConditional* conditional);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::GaussianBayesNet& other, double tol) const; bool equals(const gtsam::GaussianBayesNet& other, double tol) const;
size_t size() const; size_t size() const;
@ -1845,7 +1845,7 @@ virtual class GaussianBayesTree {
GaussianBayesTree(); GaussianBayesTree();
GaussianBayesTree(const gtsam::GaussianBayesTree& other); GaussianBayesTree(const gtsam::GaussianBayesTree& other);
bool equals(const gtsam::GaussianBayesTree& other, double tol) const; bool equals(const gtsam::GaussianBayesTree& other, double tol) const;
void print(string s); void print(string s="");
size_t size() const; size_t size() const;
bool empty() const; bool empty() const;
size_t numCachedSeparatorMarginals() const; size_t numCachedSeparatorMarginals() const;
@ -1871,7 +1871,7 @@ class Errors {
Errors(const gtsam::VectorValues& V); Errors(const gtsam::VectorValues& V);
//Testable //Testable
void print(string s); void print(string s="");
bool equals(const gtsam::Errors& expected, double tol) const; bool equals(const gtsam::Errors& expected, double tol) const;
}; };
@ -1927,7 +1927,7 @@ virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters {
#include <gtsam/linear/PCGSolver.h> #include <gtsam/linear/PCGSolver.h>
virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters { virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
PCGSolverParameters(); PCGSolverParameters();
void print(string s); void print(string s="");
void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner); void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner);
}; };
@ -1948,7 +1948,7 @@ class KalmanFilter {
KalmanFilter(size_t n); KalmanFilter(size_t n);
// gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0);
gtsam::GaussianDensity* init(Vector x0, Matrix P0); gtsam::GaussianDensity* init(Vector x0, Matrix P0);
void print(string s) const; void print(string s="") const;
static size_t step(gtsam::GaussianDensity* p); static size_t step(gtsam::GaussianDensity* p);
gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F,
Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ);
@ -2039,7 +2039,7 @@ class LabeledSymbol {
gtsam::LabeledSymbol newChr(unsigned char c) const; gtsam::LabeledSymbol newChr(unsigned char c) const;
gtsam::LabeledSymbol newLabel(unsigned char label) const; gtsam::LabeledSymbol newLabel(unsigned char label) const;
void print(string s) const; void print(string s="") const;
}; };
size_t mrsymbol(unsigned char c, unsigned char label, size_t j); size_t mrsymbol(unsigned char c, unsigned char label, size_t j);
@ -2054,7 +2054,7 @@ class Ordering {
Ordering(const gtsam::Ordering& other); Ordering(const gtsam::Ordering& other);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::Ordering& ord, double tol) const; bool equals(const gtsam::Ordering& ord, double tol) const;
// Standard interface // Standard interface
@ -2075,7 +2075,7 @@ class NonlinearFactorGraph {
NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph);
// FactorGraph // FactorGraph
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const;
size_t size() const; size_t size() const;
bool empty() const; bool empty() const;
@ -2123,7 +2123,7 @@ virtual class NonlinearFactor {
// Factor base class // Factor base class
size_t size() const; size_t size() const;
gtsam::KeyVector keys() const; gtsam::KeyVector keys() const;
void print(string s) const; void print(string s="") const;
void printKeys(string s) const; void printKeys(string s) const;
// NonlinearFactor // NonlinearFactor
bool equals(const gtsam::NonlinearFactor& other, double tol) const; bool equals(const gtsam::NonlinearFactor& other, double tol) const;
@ -2153,7 +2153,7 @@ class Values {
void clear(); void clear();
size_t dim() const; size_t dim() const;
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::Values& other, double tol) const; bool equals(const gtsam::Values& other, double tol) const;
void insert(const gtsam::Values& values); void insert(const gtsam::Values& values);
@ -2242,7 +2242,7 @@ class Marginals {
Marginals(const gtsam::GaussianFactorGraph& gfgraph, Marginals(const gtsam::GaussianFactorGraph& gfgraph,
const gtsam::VectorValues& solutionvec); const gtsam::VectorValues& solutionvec);
void print(string s) const; void print(string s="") const;
Matrix marginalCovariance(size_t variable) const; Matrix marginalCovariance(size_t variable) const;
Matrix marginalInformation(size_t variable) const; Matrix marginalInformation(size_t variable) const;
gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const; gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const;
@ -2252,7 +2252,7 @@ class Marginals {
class JointMarginal { class JointMarginal {
Matrix at(size_t iVariable, size_t jVariable) const; Matrix at(size_t iVariable, size_t jVariable) const;
Matrix fullMatrix() const; Matrix fullMatrix() const;
void print(string s) const; void print(string s="") const;
void print() const; void print() const;
}; };
@ -2296,7 +2296,7 @@ virtual class LinearContainerFactor : gtsam::NonlinearFactor {
#include <gtsam/nonlinear/NonlinearOptimizerParams.h> #include <gtsam/nonlinear/NonlinearOptimizerParams.h>
virtual class NonlinearOptimizerParams { virtual class NonlinearOptimizerParams {
NonlinearOptimizerParams(); NonlinearOptimizerParams();
void print(string s) const; void print(string s="") const;
int getMaxIterations() const; int getMaxIterations() const;
double getRelativeErrorTol() const; double getRelativeErrorTol() const;
@ -2490,7 +2490,7 @@ class ISAM2Clique {
//Standard Interface //Standard Interface
Vector gradientContribution() const; Vector gradientContribution() const;
void print(string s); void print(string s="");
}; };
class ISAM2Result { class ISAM2Result {
@ -2512,7 +2512,7 @@ class ISAM2 {
ISAM2(const gtsam::ISAM2& other); ISAM2(const gtsam::ISAM2& other);
bool equals(const gtsam::ISAM2& other, double tol) const; bool equals(const gtsam::ISAM2& other, double tol) const;
void print(string s) const; void print(string s="") const;
void printStats() const; void printStats() const;
void saveGraph(string s) const; void saveGraph(string s) const;
@ -2544,7 +2544,7 @@ class ISAM2 {
class NonlinearISAM { class NonlinearISAM {
NonlinearISAM(); NonlinearISAM();
NonlinearISAM(int reorderInterval); NonlinearISAM(int reorderInterval);
void print(string s) const; void print(string s="") const;
void printStats() const; void printStats() const;
void saveGraph(string s) const; void saveGraph(string s) const;
gtsam::Values estimate() const; gtsam::Values estimate() const;
@ -2679,7 +2679,7 @@ class BearingRange {
static This Measure(const POSE& pose, const POINT& point); static This Measure(const POSE& pose, const POINT& point);
static BEARING MeasureBearing(const POSE& pose, const POINT& point); static BEARING MeasureBearing(const POSE& pose, const POINT& point);
static RANGE MeasureRange(const POSE& pose, const POINT& point); static RANGE MeasureRange(const POSE& pose, const POINT& point);
void print(string s) const; void print(string s="") const;
}; };
typedef gtsam::BearingRange<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double> BearingRange2D; typedef gtsam::BearingRange<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double> BearingRange2D;
@ -3167,7 +3167,7 @@ class ConstantBias {
ConstantBias(Vector biasAcc, Vector biasGyro); ConstantBias(Vector biasAcc, Vector biasGyro);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const;
// Group // Group
@ -3207,7 +3207,7 @@ class NavState {
NavState(const gtsam::Pose3& pose, Vector v); NavState(const gtsam::Pose3& pose, Vector v);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::NavState& expected, double tol) const; bool equals(const gtsam::NavState& expected, double tol) const;
// Access // Access
@ -3225,7 +3225,7 @@ virtual class PreintegratedRotationParams {
PreintegratedRotationParams(); PreintegratedRotationParams();
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::PreintegratedRotationParams& expected, double tol); bool equals(const gtsam::PreintegratedRotationParams& expected, double tol);
void setGyroscopeCovariance(Matrix cov); void setGyroscopeCovariance(Matrix cov);
@ -3248,7 +3248,7 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
static gtsam::PreintegrationParams* MakeSharedU(); // default g = 9.81 static gtsam::PreintegrationParams* MakeSharedU(); // default g = 9.81
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::PreintegrationParams& expected, double tol); bool equals(const gtsam::PreintegrationParams& expected, double tol);
void setAccelerometerCovariance(Matrix cov); void setAccelerometerCovariance(Matrix cov);
@ -3268,7 +3268,7 @@ class PreintegratedImuMeasurements {
const gtsam::imuBias::ConstantBias& bias); const gtsam::imuBias::ConstantBias& bias);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol);
// Standard Interface // Standard Interface
@ -3311,7 +3311,7 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams {
static gtsam::PreintegrationCombinedParams* MakeSharedU(); // default g = 9.81 static gtsam::PreintegrationCombinedParams* MakeSharedU(); // default g = 9.81
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol); bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol);
void setBiasAccCovariance(Matrix cov); void setBiasAccCovariance(Matrix cov);
@ -3330,7 +3330,7 @@ class PreintegratedCombinedMeasurements {
PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params, PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params,
const gtsam::imuBias::ConstantBias& bias); const gtsam::imuBias::ConstantBias& bias);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, bool equals(const gtsam::PreintegratedCombinedMeasurements& expected,
double tol); double tol);
@ -3371,7 +3371,7 @@ class PreintegratedAhrsMeasurements {
PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol); bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol);
// get Data // get Data
@ -3410,7 +3410,7 @@ virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{
const gtsam::Unit3& bRef); const gtsam::Unit3& bRef);
Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model);
Rot3AttitudeFactor(); Rot3AttitudeFactor();
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::NonlinearFactor& expected, double tol) const; bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
gtsam::Unit3 nZ() const; gtsam::Unit3 nZ() const;
gtsam::Unit3 bRef() const; gtsam::Unit3 bRef() const;
@ -3423,7 +3423,7 @@ virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor {
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ,
const gtsam::noiseModel::Diagonal* model); const gtsam::noiseModel::Diagonal* model);
Pose3AttitudeFactor(); Pose3AttitudeFactor();
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::NonlinearFactor& expected, double tol) const; bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
gtsam::Unit3 nZ() const; gtsam::Unit3 nZ() const;
gtsam::Unit3 bRef() const; gtsam::Unit3 bRef() const;
@ -3435,7 +3435,7 @@ virtual class GPSFactor : gtsam::NonlinearFactor{
const gtsam::noiseModel::Base* model); const gtsam::noiseModel::Base* model);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::GPSFactor& expected, double tol); bool equals(const gtsam::GPSFactor& expected, double tol);
// Standard Interface // Standard Interface
@ -3447,7 +3447,7 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor {
const gtsam::noiseModel::Base* model); const gtsam::noiseModel::Base* model);
// Testable // Testable
void print(string s) const; void print(string s="") const;
bool equals(const gtsam::GPSFactor2& expected, double tol); bool equals(const gtsam::GPSFactor2& expected, double tol);
// Standard Interface // Standard Interface

View File

@ -109,3 +109,15 @@ Arguments:
include_directories. Again, normally, leave this empty. include_directories. Again, normally, leave this empty.
- `extraMexFlags`: Any _additional_ flags to pass to the compiler when building - `extraMexFlags`: Any _additional_ flags to pass to the compiler when building
the wrap code. Normally, leave this empty. the wrap code. Normally, leave this empty.
## Git subtree and Contributing
**\*WARNING\*: Running the ./update_wrap.sh script from the GTSAM repo creates 2 new commits in GTSAM. Be sure to _NOT_ push these directly to master/develop. Preferably, open up a new PR with these updates (see below).**
The [wrap library](https://github.com/borglab/wrap) is included in GTSAM as a git subtree. This means that sometimes the wrap library can have new features or changes that are not yet reflected in GTSAM. There are two options to get the most up-to-date versions of wrap:
1. Clone and install the [wrap repository](https://github.com/borglab/wrap). For external projects, make sure cmake is using the external `wrap` rather than the one pre-packaged with GTSAM.
2. Run `./update_wrap.sh` from the root of GTSAM's repository to pull in the newest version of wrap to your local GTSAM installation. See the warning above about this script automatically creating commits.
To make a PR on GTSAM with the most recent wrap updates, create a new branch/fork then pull in the most recent wrap changes using `./update_wrap.sh`. You should find that two new commits have been made: a squash and a merge from master. You can push these (to the non-develop branch) and open a PR.
For any code contributions to the wrap project, please make them on the [wrap repository](https://github.com/borglab/wrap).

View File

@ -210,17 +210,24 @@ class PybindWrapper:
return res return res
def wrap_variable(self, def wrap_variable(self,
module, namespace,
module_var, module_var,
variable, variable,
prefix='\n' + ' ' * 8): prefix='\n' + ' ' * 8):
"""Wrap a variable that's not part of a class (i.e. global) """Wrap a variable that's not part of a class (i.e. global)
""" """
return '{prefix}{module_var}.attr("{variable_name}") = {module}{variable_name};'.format( variable_value = ""
if variable.default is None:
variable_value = variable.name
else:
variable_value = variable.default
return '{prefix}{module_var}.attr("{variable_name}") = {namespace}{variable_value};'.format(
prefix=prefix, prefix=prefix,
module=module,
module_var=module_var, module_var=module_var,
variable_name=variable.name) variable_name=variable.name,
namespace=namespace,
variable_value=variable_value)
def wrap_properties(self, properties, cpp_class, prefix='\n' + ' ' * 8): def wrap_properties(self, properties, cpp_class, prefix='\n' + ' ' * 8):
"""Wrap all the properties in the `cpp_class`.""" """Wrap all the properties in the `cpp_class`."""
@ -254,6 +261,45 @@ class PybindWrapper:
op.operator)) op.operator))
return res return res
def wrap_enum(self, enum, class_name='', module=None, prefix=' ' * 4):
"""
Wrap an enum.
Args:
enum: The parsed enum to wrap.
class_name: The class under which the enum is defined.
prefix: The amount of indentation.
"""
if module is None:
module = self._gen_module_var(enum.namespaces())
cpp_class = enum.cpp_typename().to_cpp()
if class_name:
# If class_name is provided, add that as the namespace
cpp_class = class_name + "::" + cpp_class
res = '{prefix}py::enum_<{cpp_class}>({module}, "{enum.name}", py::arithmetic())'.format(
prefix=prefix, module=module, enum=enum, cpp_class=cpp_class)
for enumerator in enum.enumerators:
res += '\n{prefix} .value("{enumerator.name}", {cpp_class}::{enumerator.name})'.format(
prefix=prefix, enumerator=enumerator, cpp_class=cpp_class)
res += ";\n\n"
return res
def wrap_enums(self, enums, instantiated_class, prefix=' ' * 4):
"""Wrap multiple enums defined in a class."""
cpp_class = instantiated_class.cpp_class()
module_var = instantiated_class.name.lower()
res = ''
for enum in enums:
res += "\n" + self.wrap_enum(
enum,
class_name=cpp_class,
module=module_var,
prefix=prefix)
return res
def wrap_instantiated_class( def wrap_instantiated_class(
self, instantiated_class: instantiator.InstantiatedClass): self, instantiated_class: instantiator.InstantiatedClass):
"""Wrap the class.""" """Wrap the class."""
@ -261,24 +307,48 @@ class PybindWrapper:
cpp_class = instantiated_class.cpp_class() cpp_class = instantiated_class.cpp_class()
if cpp_class in self.ignore_classes: if cpp_class in self.ignore_classes:
return "" return ""
return ( if instantiated_class.parent_class:
class_parent = "{instantiated_class.parent_class}, ".format(
instantiated_class=instantiated_class)
else:
class_parent = ''
if instantiated_class.enums:
# If class has enums, define an instance and set module_var to the instance
instance_name = instantiated_class.name.lower()
class_declaration = (
'\n py::class_<{cpp_class}, {class_parent}'
'{shared_ptr_type}::shared_ptr<{cpp_class}>> '
'{instance_name}({module_var}, "{class_name}");'
'\n {instance_name}').format(
shared_ptr_type=('boost' if self.use_boost else 'std'),
cpp_class=cpp_class,
class_name=instantiated_class.name,
class_parent=class_parent,
instance_name=instance_name,
module_var=module_var)
module_var = instance_name
else:
class_declaration = (
'\n py::class_<{cpp_class}, {class_parent}' '\n py::class_<{cpp_class}, {class_parent}'
'{shared_ptr_type}::shared_ptr<{cpp_class}>>({module_var}, "{class_name}")' '{shared_ptr_type}::shared_ptr<{cpp_class}>>({module_var}, "{class_name}")'
).format(shared_ptr_type=('boost' if self.use_boost else 'std'),
cpp_class=cpp_class,
class_name=instantiated_class.name,
class_parent=class_parent,
module_var=module_var)
return ('{class_declaration}'
'{wrapped_ctors}' '{wrapped_ctors}'
'{wrapped_methods}' '{wrapped_methods}'
'{wrapped_static_methods}' '{wrapped_static_methods}'
'{wrapped_properties}' '{wrapped_properties}'
'{wrapped_operators};\n'.format( '{wrapped_operators};\n'.format(
shared_ptr_type=('boost' if self.use_boost else 'std'), class_declaration=class_declaration,
cpp_class=cpp_class,
class_name=instantiated_class.name,
class_parent="{instantiated_class.parent_class}, ".format(
instantiated_class=instantiated_class)
if instantiated_class.parent_class else '',
module_var=module_var,
wrapped_ctors=self.wrap_ctors(instantiated_class), wrapped_ctors=self.wrap_ctors(instantiated_class),
wrapped_methods=self.wrap_methods(instantiated_class.methods, wrapped_methods=self.wrap_methods(
cpp_class), instantiated_class.methods, cpp_class),
wrapped_static_methods=self.wrap_methods( wrapped_static_methods=self.wrap_methods(
instantiated_class.static_methods, cpp_class), instantiated_class.static_methods, cpp_class),
wrapped_properties=self.wrap_properties( wrapped_properties=self.wrap_properties(
@ -315,18 +385,6 @@ class PybindWrapper:
stl_class.properties, cpp_class), stl_class.properties, cpp_class),
)) ))
def wrap_enum(self, enum, prefix='\n' + ' ' * 8):
"""Wrap an enum."""
module_var = self._gen_module_var(enum.namespaces())
cpp_class = enum.cpp_typename().to_cpp()
res = '\n py::enum_<{cpp_class}>({module_var}, "{enum.name}", py::arithmetic())'.format(
module_var=module_var, enum=enum, cpp_class=cpp_class)
for enumerator in enum.enumerators:
res += '{prefix}.value("{enumerator.name}", {cpp_class}::{enumerator.name})'.format(
prefix=prefix, enumerator=enumerator, cpp_class=cpp_class)
res += ";\n\n"
return res
def _partial_match(self, namespaces1, namespaces2): def _partial_match(self, namespaces1, namespaces2):
for i in range(min(len(namespaces1), len(namespaces2))): for i in range(min(len(namespaces1), len(namespaces2))):
if namespaces1[i] != namespaces2[i]: if namespaces1[i] != namespaces2[i]:
@ -400,9 +458,11 @@ class PybindWrapper:
elif isinstance(element, instantiator.InstantiatedClass): elif isinstance(element, instantiator.InstantiatedClass):
wrapped += self.wrap_instantiated_class(element) wrapped += self.wrap_instantiated_class(element)
wrapped += self.wrap_enums(element.enums, element)
elif isinstance(element, parser.Variable): elif isinstance(element, parser.Variable):
module = self._add_namespaces('', namespaces) variable_namespace = self._add_namespaces('', namespaces)
wrapped += self.wrap_variable(module=module, wrapped += self.wrap_variable(namespace=variable_namespace,
module_var=module_var, module_var=module_var,
variable=element, variable=element,
prefix='\n' + ' ' * 4) prefix='\n' + ' ' * 4)

View File

@ -21,13 +21,23 @@ namespace py = pybind11;
PYBIND11_MODULE(enum_py, m_) { PYBIND11_MODULE(enum_py, m_) {
m_.doc() = "pybind11 wrapper of enum_py"; m_.doc() = "pybind11 wrapper of enum_py";
py::enum_<Color>(m_, "Color", py::arithmetic())
.value("Red", Color::Red)
.value("Green", Color::Green)
.value("Blue", Color::Blue);
py::enum_<Kind>(m_, "Kind", py::arithmetic())
.value("Dog", Kind::Dog) py::class_<Pet, std::shared_ptr<Pet>> pet(m_, "Pet");
.value("Cat", Kind::Cat); pet
.def(py::init<const string&, Kind>(), py::arg("name"), py::arg("type"))
.def_readwrite("name", &Pet::name)
.def_readwrite("type", &Pet::type);
py::enum_<Pet::Kind>(pet, "Kind", py::arithmetic())
.value("Dog", Pet::Kind::Dog)
.value("Cat", Pet::Kind::Cat);
pybind11::module m_gtsam = m_.def_submodule("gtsam", "gtsam submodule"); pybind11::module m_gtsam = m_.def_submodule("gtsam", "gtsam submodule");
py::enum_<gtsam::VerbosityLM>(m_gtsam, "VerbosityLM", py::arithmetic()) py::enum_<gtsam::VerbosityLM>(m_gtsam, "VerbosityLM", py::arithmetic())
.value("SILENT", gtsam::VerbosityLM::SILENT) .value("SILENT", gtsam::VerbosityLM::SILENT)
.value("SUMMARY", gtsam::VerbosityLM::SUMMARY) .value("SUMMARY", gtsam::VerbosityLM::SUMMARY)
@ -39,10 +49,25 @@ PYBIND11_MODULE(enum_py, m_) {
.value("TRYDELTA", gtsam::VerbosityLM::TRYDELTA); .value("TRYDELTA", gtsam::VerbosityLM::TRYDELTA);
py::class_<gtsam::Pet, std::shared_ptr<gtsam::Pet>>(m_gtsam, "Pet") py::class_<gtsam::MCU, std::shared_ptr<gtsam::MCU>> mcu(m_gtsam, "MCU");
.def(py::init<const string&, Kind>(), py::arg("name"), py::arg("type")) mcu
.def_readwrite("name", &gtsam::Pet::name) .def(py::init<>());
.def_readwrite("type", &gtsam::Pet::type);
py::enum_<gtsam::MCU::Avengers>(mcu, "Avengers", py::arithmetic())
.value("CaptainAmerica", gtsam::MCU::Avengers::CaptainAmerica)
.value("IronMan", gtsam::MCU::Avengers::IronMan)
.value("Hulk", gtsam::MCU::Avengers::Hulk)
.value("Hawkeye", gtsam::MCU::Avengers::Hawkeye)
.value("Thor", gtsam::MCU::Avengers::Thor);
py::enum_<gtsam::MCU::GotG>(mcu, "GotG", py::arithmetic())
.value("Starlord", gtsam::MCU::GotG::Starlord)
.value("Gamorra", gtsam::MCU::GotG::Gamorra)
.value("Rocket", gtsam::MCU::GotG::Rocket)
.value("Drax", gtsam::MCU::GotG::Drax)
.value("Groot", gtsam::MCU::GotG::Groot);
#include "python/specializations.h" #include "python/specializations.h"

View File

@ -1,4 +1,13 @@
enum Kind { Dog, Cat }; enum Color { Red, Green, Blue };
class Pet {
enum Kind { Dog, Cat };
Pet(const string &name, Kind type);
string name;
Kind type;
};
namespace gtsam { namespace gtsam {
enum VerbosityLM { enum VerbosityLM {
@ -12,12 +21,25 @@ enum VerbosityLM {
TRYDELTA TRYDELTA
}; };
class Pet { class MCU {
enum Kind { Dog, Cat }; MCU();
Pet(const string &name, Kind type); enum Avengers {
CaptainAmerica,
IronMan,
Hulk,
Hawkeye,
Thor
};
enum GotG {
Starlord,
Gamorra,
Rocket,
Drax,
Groot
};
string name;
Kind type;
}; };
} // namespace gtsam } // namespace gtsam