From 78210f348031f88a4384cd7c35a604ee35084392 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 8 Apr 2021 04:34:23 -0400 Subject: [PATCH 01/13] Squashed 'wrap/' changes from 5ddaff8ba..b43f7c6d7 b43f7c6d7 Merge pull request #80 from borglab/feature/multiple_interface_files 7b9d080f5 Revert "unit test for printing with keyformatter" 45e203195 Revert "funcitonal print passes unit test" 8b5d66f03 Revert "include functional and iostream" 34bfbec21 Revert "delete debug statement" dbe661c93 Revert "add includes to the example tpl file." f47e23f1a Revert "Revert "function to concatenate multiple header files together"" e667e2095 Revert "function to concatenate multiple header files together" 600c77bf4 add includes to the example tpl file. d5caca20e delete debug statement 4a554edb8 include functional and iostream e8ad2c26f funcitonal print passes unit test 077d5c4e7 unit test for printing with keyformatter d5a1e6dff function to concatenate multiple header files together git-subtree-dir: wrap git-subtree-split: b43f7c6d7d6cb50ebe585d7e38390e2bfeb51dde --- cmake/GtwrapUtils.cmake | 34 ++++++++++++++++++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/cmake/GtwrapUtils.cmake b/cmake/GtwrapUtils.cmake index 8479015dc..3c26e70ae 100644 --- a/cmake/GtwrapUtils.cmake +++ b/cmake/GtwrapUtils.cmake @@ -103,3 +103,37 @@ macro(gtwrap_get_python_version) configure_python_variables() endmacro() + +# Concatenate multiple wrapper interface headers into one. +# The concatenation will be (re)performed if and only if any interface files +# change. +# +# Arguments: +# ~~~ +# destination: The concatenated master interface header file will be placed here. +# inputs (optional): All the input interface header files +function(combine_interface_headers + destination + #inputs + ) + # check if any interface headers changed + foreach(INTERFACE_FILE ${ARGN}) + if(NOT EXISTS ${destination} OR + ${INTERFACE_FILE} IS_NEWER_THAN ${destination}) + set(UPDATE_INTERFACE TRUE) + endif() + # trigger cmake on file change + set_property(DIRECTORY + APPEND + PROPERTY CMAKE_CONFIGURE_DEPENDS ${INTERFACE_FILE}) + endforeach() + # if so, then update the overall interface file + if (UPDATE_INTERFACE) + file(WRITE ${destination} "") + # append additional interface headers to end of gtdynamics.i + foreach(INTERFACE_FILE ${ARGN}) + file(READ ${INTERFACE_FILE} interface_contents) + file(APPEND ${destination} "${interface_contents}") + endforeach() + endif() +endfunction() From 36ab16855875e097103d321c3dde856d11b93dcd Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 8 Apr 2021 05:01:14 -0400 Subject: [PATCH 02/13] update gtsam.i print function declarations --- gtsam/gtsam.i | 178 ++++++++++++++++++++++++++++---------------------- 1 file changed, 100 insertions(+), 78 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 27f2aa924..aa706a7f4 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -47,7 +47,7 @@ class KeySet { KeySet(const gtsam::KeyList& list); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::KeySet& other) const; // common STL methods @@ -221,7 +221,7 @@ virtual class Value { // No constructors because this is an abstract class // Testable - void print(string s) const; + void print(string s = "") const; // Manifold size_t dim() const; @@ -245,7 +245,7 @@ class Point2 { Point2(Vector v); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::Point2& point, double tol) const; // Group @@ -298,7 +298,7 @@ class StereoPoint2 { StereoPoint2(double uL, double uR, double v); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::StereoPoint2& point, double tol) const; // Group @@ -342,7 +342,7 @@ class Point3 { Point3(Vector v); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::Point3& p, double tol) const; // Group @@ -379,7 +379,7 @@ class Rot2 { static gtsam::Rot2 fromCosSin(double c, double s); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::Rot2& rot, double tol) const; // Group @@ -430,7 +430,7 @@ class SO3 { static gtsam::SO3 ClosestTo(const Matrix M); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::SO3& other, double tol) const; // Group @@ -460,7 +460,7 @@ class SO4 { static gtsam::SO4 FromMatrix(Matrix R); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::SO4& other, double tol) const; // Group @@ -490,7 +490,7 @@ class SOn { static gtsam::SOn Lift(size_t n, Matrix R); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::SOn& other, double tol) const; // Group @@ -551,7 +551,7 @@ class Rot3 { static gtsam::Rot3 ClosestTo(const Matrix M); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::Rot3& rot, double tol) const; // Group @@ -608,7 +608,7 @@ class Pose2 { Pose2(Vector v); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::Pose2& pose, double tol) const; // Group @@ -668,7 +668,7 @@ class Pose3 { Pose3(Matrix mat); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::Pose3& pose, double tol) const; // Group @@ -744,7 +744,7 @@ class Unit3 { Unit3(const gtsam::Point3& pose); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::Unit3& pose, double tol) const; // Other functionality @@ -774,7 +774,7 @@ class EssentialMatrix { EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::EssentialMatrix& pose, double tol) const; // Manifold @@ -799,7 +799,7 @@ class Cal3_S2 { Cal3_S2(double fov, int w, int h); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::Cal3_S2& rhs, double tol) const; // Manifold @@ -836,7 +836,7 @@ virtual class Cal3DS2_Base { Cal3DS2_Base(); // Testable - void print(string s) const; + void print(string s = "") const; // Standard Interface double fx() const; @@ -922,7 +922,7 @@ class Cal3_S2Stereo { Cal3_S2Stereo(Vector v); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const; // Standard Interface @@ -943,7 +943,7 @@ class Cal3Bundler { Cal3Bundler(double fx, double k1, double k2, double u0, double v0, double tol); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::Cal3Bundler& rhs, double tol) const; // Manifold @@ -983,7 +983,7 @@ class CalibratedCamera { static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::CalibratedCamera& camera, double tol) const; // Manifold @@ -1022,7 +1022,7 @@ class PinholeCamera { const gtsam::Point3& upVector, const CALIBRATION& K); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const This& camera, double tol) const; // Standard Interface @@ -1097,7 +1097,7 @@ class StereoCamera { StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::StereoCamera& camera, double tol) const; // Standard Interface @@ -1160,7 +1160,8 @@ virtual class SymbolicFactor { // From Factor size_t size() const; - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::SymbolicFactor& other, double tol) const; gtsam::KeyVector keys(); }; @@ -1173,7 +1174,8 @@ virtual class SymbolicFactorGraph { // From FactorGraph void push_back(gtsam::SymbolicFactor* factor); - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; size_t size() const; bool exists(size_t idx) const; @@ -1223,7 +1225,8 @@ virtual class SymbolicConditional : gtsam::SymbolicFactor { static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals); // Testable - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::SymbolicConditional& other, double tol) const; // Standard interface @@ -1236,7 +1239,8 @@ class SymbolicBayesNet { SymbolicBayesNet(); SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); // Testable - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; // Standard interface @@ -1257,7 +1261,8 @@ class SymbolicBayesTree { SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); // Testable - void print(string s); + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; //Standard Interface @@ -1279,7 +1284,8 @@ class SymbolicBayesTree { // SymbolicBayesTreeClique(const pair& result) : Base(result) {} // // bool equals(const This& other, double tol) const; -// void print(string s) const; +// void print(string s = "", +// gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; // void printTree() const; // Default indent of "" // void printTree(string indent) const; // size_t numCachedSeparatorMarginals() const; @@ -1313,7 +1319,8 @@ class VariableIndex { // Testable bool equals(const gtsam::VariableIndex& other, double tol) const; - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; // Standard interface size_t size() const; @@ -1328,7 +1335,7 @@ class VariableIndex { namespace noiseModel { #include 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 // because wrap (incorrectly) thinks robust classes derive from this Base as well. // bool isConstrained() const; @@ -1411,7 +1418,7 @@ virtual class Unit : gtsam::noiseModel::Isotropic { namespace mEstimator { virtual class Base { - void print(string s) const; + void print(string s = "") const; }; virtual class Null: gtsam::noiseModel::mEstimator::Base { @@ -1551,7 +1558,8 @@ class VectorValues { size_t size() const; size_t dim(size_t j) const; bool exists(size_t j) const; - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::VectorValues& expected, double tol) const; void insert(size_t j, Vector value); Vector vector() const; @@ -1582,7 +1590,8 @@ class VectorValues { #include virtual class GaussianFactor { gtsam::KeyVector keys() const; - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const; double error(const gtsam::VectorValues& c) const; gtsam::GaussianFactor* clone() const; @@ -1610,7 +1619,8 @@ virtual class JacobianFactor : gtsam::GaussianFactor { JacobianFactor(const gtsam::GaussianFactorGraph& graph); //Testable - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; void printKeys(string s) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const; size_t size() const; @@ -1659,7 +1669,8 @@ virtual class HessianFactor : gtsam::GaussianFactor { //Testable size_t size() const; - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; void printKeys(string s) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const; double error(const gtsam::VectorValues& c) const; @@ -1684,7 +1695,8 @@ class GaussianFactorGraph { GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); // From FactorGraph - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; size_t size() const; gtsam::GaussianFactor* at(size_t idx) const; @@ -1775,7 +1787,8 @@ virtual class GaussianConditional : gtsam::JacobianFactor { size_t name2, Matrix T); //Standard Interface - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GaussianConditional &cg, double tol) const; //Advanced Interface @@ -1797,7 +1810,8 @@ virtual class GaussianDensity : gtsam::GaussianConditional { GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); //Standard Interface - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GaussianDensity &cg, double tol) const; Vector mean() const; Matrix covariance() const; @@ -1810,7 +1824,8 @@ virtual class GaussianBayesNet { GaussianBayesNet(const gtsam::GaussianConditional* conditional); // Testable - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GaussianBayesNet& other, double tol) const; size_t size() const; @@ -1845,7 +1860,8 @@ virtual class GaussianBayesTree { GaussianBayesTree(); GaussianBayesTree(const gtsam::GaussianBayesTree& other); bool equals(const gtsam::GaussianBayesTree& other, double tol) const; - void print(string s); + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); size_t size() const; bool empty() const; size_t numCachedSeparatorMarginals() const; @@ -1871,7 +1887,7 @@ class Errors { Errors(const gtsam::VectorValues& V); //Testable - void print(string s); + void print(string s = "Errors"); bool equals(const gtsam::Errors& expected, double tol) const; }; @@ -1890,7 +1906,6 @@ class GaussianISAM { virtual class IterativeOptimizationParameters { string getVerbosity() const; void setVerbosity(string s) ; - void print() const; }; //virtual class IterativeSolver { @@ -1912,7 +1927,6 @@ virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParamete void setReset(int value); void setEpsilon_rel(double value); void setEpsilon_abs(double value); - void print() const; }; #include @@ -1927,14 +1941,13 @@ virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters { #include virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters { PCGSolverParameters(); - void print(string s); + void print(string s = ""); void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner); }; #include virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { SubgraphSolverParameters(); - void print() const; }; virtual class SubgraphSolver { @@ -1948,7 +1961,7 @@ class KalmanFilter { KalmanFilter(size_t n); // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); gtsam::GaussianDensity* init(Vector x0, Matrix P0); - void print(string s) const; + void print(string s = "") const; static size_t step(gtsam::GaussianDensity* p); gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); @@ -1974,7 +1987,7 @@ class Symbol { Symbol(size_t key); size_t key() const; - void print(const string& s) const; + void print(const string& s = "") const; bool equals(const gtsam::Symbol& expected, double tol) const; char chr() const; @@ -2039,7 +2052,7 @@ class LabeledSymbol { gtsam::LabeledSymbol newChr(unsigned char c) 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); @@ -2054,7 +2067,8 @@ class Ordering { Ordering(const gtsam::Ordering& other); // Testable - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::Ordering& ord, double tol) const; // Standard interface @@ -2075,7 +2089,8 @@ class NonlinearFactorGraph { NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); // FactorGraph - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; size_t size() const; bool empty() const; @@ -2123,7 +2138,8 @@ virtual class NonlinearFactor { // Factor base class size_t size() const; gtsam::KeyVector keys() const; - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; void printKeys(string s) const; // NonlinearFactor bool equals(const gtsam::NonlinearFactor& other, double tol) const; @@ -2153,7 +2169,8 @@ class Values { void clear(); size_t dim() const; - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::Values& other, double tol) const; void insert(const gtsam::Values& values); @@ -2242,7 +2259,8 @@ class Marginals { Marginals(const gtsam::GaussianFactorGraph& gfgraph, const gtsam::VectorValues& solutionvec); - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; Matrix marginalCovariance(size_t variable) const; Matrix marginalInformation(size_t variable) const; gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const; @@ -2252,8 +2270,8 @@ class Marginals { class JointMarginal { Matrix at(size_t iVariable, size_t jVariable) const; Matrix fullMatrix() const; - void print(string s) const; - void print() const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; }; #include @@ -2296,7 +2314,7 @@ virtual class LinearContainerFactor : gtsam::NonlinearFactor { #include virtual class NonlinearOptimizerParams { NonlinearOptimizerParams(); - void print(string s) const; + void print(string s = "") const; int getMaxIterations() const; double getRelativeErrorTol() const; @@ -2407,14 +2425,14 @@ 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; + void print(string s = "") const; }; #include class ISAM2GaussNewtonParams { ISAM2GaussNewtonParams(); - void print(string str) const; + void print(string s = "") const; /** Getters and Setters for all properties */ double getWildfireThreshold() const; @@ -2424,7 +2442,7 @@ class ISAM2GaussNewtonParams { class ISAM2DoglegParams { ISAM2DoglegParams(); - void print(string str) const; + void print(string s = "") const; /** Getters and Setters for all properties */ double getWildfireThreshold() const; @@ -2460,7 +2478,7 @@ class ISAM2ThresholdMap { class ISAM2Params { ISAM2Params(); - void print(string str) const; + void print(string s = "") const; /** Getters and Setters for all properties */ void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& gauss_newton__params); @@ -2490,13 +2508,14 @@ class ISAM2Clique { //Standard Interface Vector gradientContribution() const; - void print(string s); + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); }; class ISAM2Result { ISAM2Result(); - void print(string str) const; + void print(string s = "") const; /** Getters and Setters for all properties */ size_t getVariablesRelinearized() const; @@ -2512,7 +2531,7 @@ class ISAM2 { ISAM2(const gtsam::ISAM2& other); bool equals(const gtsam::ISAM2& other, double tol) const; - void print(string s) const; + void print(string s = "") const; void printStats() const; void saveGraph(string s) const; @@ -2544,7 +2563,8 @@ class ISAM2 { class NonlinearISAM { NonlinearISAM(); NonlinearISAM(int reorderInterval); - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; void printStats() const; void saveGraph(string s) const; gtsam::Values estimate() const; @@ -2682,7 +2702,7 @@ class BearingRange { static This Measure(const POSE& pose, const POINT& point); static BEARING MeasureBearing(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 BearingRange2D; @@ -3019,7 +3039,6 @@ class ShonanAveragingParameters2 { bool getUseHuber() const; void setCertifyOptimality(bool value); bool getCertifyOptimality() const; - void print() const; }; class ShonanAveragingParameters3 { @@ -3040,7 +3059,6 @@ class ShonanAveragingParameters3 { bool getUseHuber() const; void setCertifyOptimality(bool value); bool getCertifyOptimality() const; - void print() const; }; class ShonanAveraging2 { @@ -3170,7 +3188,7 @@ class ConstantBias { ConstantBias(Vector biasAcc, Vector biasGyro); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; // Group @@ -3210,7 +3228,7 @@ class NavState { NavState(const gtsam::Pose3& pose, Vector v); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::NavState& expected, double tol) const; // Access @@ -3228,7 +3246,7 @@ virtual class PreintegratedRotationParams { PreintegratedRotationParams(); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::PreintegratedRotationParams& expected, double tol); void setGyroscopeCovariance(Matrix cov); @@ -3251,7 +3269,7 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { static gtsam::PreintegrationParams* MakeSharedU(); // default g = 9.81 // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::PreintegrationParams& expected, double tol); void setAccelerometerCovariance(Matrix cov); @@ -3271,7 +3289,7 @@ class PreintegratedImuMeasurements { const gtsam::imuBias::ConstantBias& bias); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); // Standard Interface @@ -3314,7 +3332,7 @@ virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { static gtsam::PreintegrationCombinedParams* MakeSharedU(); // default g = 9.81 // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::PreintegrationCombinedParams& expected, double tol); void setBiasAccCovariance(Matrix cov); @@ -3333,7 +3351,7 @@ class PreintegratedCombinedMeasurements { PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params, const gtsam::imuBias::ConstantBias& bias); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, double tol); @@ -3374,7 +3392,7 @@ class PreintegratedAhrsMeasurements { PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); // Testable - void print(string s) const; + void print(string s = "") const; bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol); // get Data @@ -3413,7 +3431,8 @@ virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ const gtsam::Unit3& bRef); Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); Rot3AttitudeFactor(); - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::NonlinearFactor& expected, double tol) const; gtsam::Unit3 nZ() const; gtsam::Unit3 bRef() const; @@ -3426,7 +3445,8 @@ virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); Pose3AttitudeFactor(); - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::NonlinearFactor& expected, double tol) const; gtsam::Unit3 nZ() const; gtsam::Unit3 bRef() const; @@ -3438,7 +3458,8 @@ virtual class GPSFactor : gtsam::NonlinearFactor{ const gtsam::noiseModel::Base* model); // Testable - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GPSFactor& expected, double tol); // Standard Interface @@ -3450,7 +3471,8 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor { const gtsam::noiseModel::Base* model); // Testable - void print(string s) const; + void print(string s = "", + gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GPSFactor2& expected, double tol); // Standard Interface From 70658852d4560b18f4594135dd6b5a626f0594bb Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 8 Apr 2021 05:22:34 -0400 Subject: [PATCH 03/13] update default args to match with c++ --- gtsam/gtsam.i | 50 +++++++++++++++++++++++++------------------------- 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index aa706a7f4..a2067118f 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -379,7 +379,7 @@ class Rot2 { static gtsam::Rot2 fromCosSin(double c, double s); // Testable - void print(string s = "") const; + void print(string s = "theta") const; bool equals(const gtsam::Rot2& rot, double tol) const; // Group @@ -799,7 +799,7 @@ class Cal3_S2 { Cal3_S2(double fov, int w, int h); // Testable - void print(string s = "") const; + void print(string s = "Cal3_S2") const; bool equals(const gtsam::Cal3_S2& rhs, double tol) const; // Manifold @@ -983,7 +983,7 @@ class CalibratedCamera { static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); // Testable - void print(string s = "") const; + void print(string s = "PinholeBase") const; bool equals(const gtsam::CalibratedCamera& camera, double tol) const; // Manifold @@ -1022,7 +1022,7 @@ class PinholeCamera { const gtsam::Point3& upVector, const CALIBRATION& K); // Testable - void print(string s = "") const; + void print(string s = "PinholeCamera") const; bool equals(const This& camera, double tol) const; // Standard Interface @@ -1160,7 +1160,7 @@ virtual class SymbolicFactor { // From Factor size_t size() const; - void print(string s = "", + void print(string s = "Factor", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::SymbolicFactor& other, double tol) const; gtsam::KeyVector keys(); @@ -1174,7 +1174,7 @@ virtual class SymbolicFactorGraph { // From FactorGraph void push_back(gtsam::SymbolicFactor* factor); - void print(string s = "", + void print(string s = "FactorGraph", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; size_t size() const; @@ -1225,7 +1225,7 @@ virtual class SymbolicConditional : gtsam::SymbolicFactor { static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals); // Testable - void print(string s = "", + void print(string s = "Factor", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::SymbolicConditional& other, double tol) const; @@ -1239,7 +1239,7 @@ class SymbolicBayesNet { SymbolicBayesNet(); SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); // Testable - void print(string s = "", + void print(string s = "BayesNet", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; @@ -1319,7 +1319,7 @@ class VariableIndex { // Testable bool equals(const gtsam::VariableIndex& other, double tol) const; - void print(string s = "", + void print(string s = "VariableIndex: ", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; // Standard interface @@ -1558,7 +1558,7 @@ class VectorValues { size_t size() const; size_t dim(size_t j) const; bool exists(size_t j) const; - void print(string s = "", + void print(string s = "VectorValues", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::VectorValues& expected, double tol) const; void insert(size_t j, Vector value); @@ -1590,7 +1590,7 @@ class VectorValues { #include virtual class GaussianFactor { gtsam::KeyVector keys() const; - void print(string s = "", + void print(string s = "Factor", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const; double error(const gtsam::VectorValues& c) const; @@ -1619,7 +1619,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor { JacobianFactor(const gtsam::GaussianFactorGraph& graph); //Testable - void print(string s = "", + void print(string s = "Factor", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; void printKeys(string s) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const; @@ -1669,7 +1669,7 @@ virtual class HessianFactor : gtsam::GaussianFactor { //Testable size_t size() const; - void print(string s = "", + void print(string s = "Factor", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; void printKeys(string s) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const; @@ -1695,7 +1695,7 @@ class GaussianFactorGraph { GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); // From FactorGraph - void print(string s = "", + void print(string s = "FactorGraph", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; size_t size() const; @@ -1787,7 +1787,7 @@ virtual class GaussianConditional : gtsam::JacobianFactor { size_t name2, Matrix T); //Standard Interface - void print(string s = "", + void print(string s = "GaussianConditional", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GaussianConditional &cg, double tol) const; @@ -1810,7 +1810,7 @@ virtual class GaussianDensity : gtsam::GaussianConditional { GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); //Standard Interface - void print(string s = "", + void print(string s = "GaussianDensity", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GaussianDensity &cg, double tol) const; Vector mean() const; @@ -1824,7 +1824,7 @@ virtual class GaussianBayesNet { GaussianBayesNet(const gtsam::GaussianConditional* conditional); // Testable - void print(string s = "", + void print(string s = "BayesNet", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GaussianBayesNet& other, double tol) const; size_t size() const; @@ -2089,7 +2089,7 @@ class NonlinearFactorGraph { NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); // FactorGraph - void print(string s = "", + void print(string s = "NonlinearFactorGraph: ", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; size_t size() const; @@ -2138,9 +2138,9 @@ virtual class NonlinearFactor { // Factor base class size_t size() const; gtsam::KeyVector keys() const; - void print(string s = "", + void print(string s = "Factor", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; - void printKeys(string s) const; + void printKeys(string s = "Factor") const; // NonlinearFactor bool equals(const gtsam::NonlinearFactor& other, double tol) const; double error(const gtsam::Values& c) const; @@ -2259,7 +2259,7 @@ class Marginals { Marginals(const gtsam::GaussianFactorGraph& gfgraph, const gtsam::VectorValues& solutionvec); - void print(string s = "", + void print(string s = "Marginals: ", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; Matrix marginalCovariance(size_t variable) const; Matrix marginalInformation(size_t variable) const; @@ -3351,7 +3351,7 @@ class PreintegratedCombinedMeasurements { PreintegratedCombinedMeasurements(const gtsam::PreintegrationCombinedParams* params, const gtsam::imuBias::ConstantBias& bias); // Testable - void print(string s = "") const; + void print(string s = "Preintegrated Measurements:") const; bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, double tol); @@ -3392,7 +3392,7 @@ class PreintegratedAhrsMeasurements { PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); // Testable - void print(string s = "") const; + void print(string s = "Preintegrated Measurements: ") const; bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol); // get Data @@ -3445,7 +3445,7 @@ virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); Pose3AttitudeFactor(); - void print(string s = "", + void print(string s = "Factor", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::NonlinearFactor& expected, double tol) const; gtsam::Unit3 nZ() const; @@ -3471,7 +3471,7 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor { const gtsam::noiseModel::Base* model); // Testable - void print(string s = "", + void print(string s = "Factor", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GPSFactor2& expected, double tol); From 8c6efb59179ec6805e7c20bf336ff4c851f58218 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Thu, 8 Apr 2021 05:23:02 -0400 Subject: [PATCH 04/13] include pybind11/function --- python/gtsam/gtsam.tpl | 2 ++ python/gtsam_unstable/gtsam_unstable.tpl | 2 ++ 2 files changed, 4 insertions(+) diff --git a/python/gtsam/gtsam.tpl b/python/gtsam/gtsam.tpl index 60b3d1fd0..0e0881ce9 100644 --- a/python/gtsam/gtsam.tpl +++ b/python/gtsam/gtsam.tpl @@ -13,6 +13,8 @@ #include #include #include +#include +#include #include "gtsam/config.h" #include "gtsam/base/serialization.h" #include "gtsam/nonlinear/utilities.h" // for RedirectCout. diff --git a/python/gtsam_unstable/gtsam_unstable.tpl b/python/gtsam_unstable/gtsam_unstable.tpl index 111e46d5e..c1033ba43 100644 --- a/python/gtsam_unstable/gtsam_unstable.tpl +++ b/python/gtsam_unstable/gtsam_unstable.tpl @@ -13,6 +13,8 @@ #include #include #include +#include +#include #include "gtsam/base/serialization.h" #include "gtsam/nonlinear/utilities.h" // for RedirectCout. From 2b43e7f8f8095cd310bb19c51079fe7159aa5f34 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Tue, 27 Apr 2021 11:24:46 +0200 Subject: [PATCH 05/13] Avoid potential wrong memory access If the user uses an invalid index, the [] operator won't check it and the program will access invalid memory. Using at() would throw instead. --- gtsam/inference/FactorGraph.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 2bc9578b2..9d2308d9b 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -355,7 +355,7 @@ class FactorGraph { /** delete factor without re-arranging indexes by inserting a nullptr pointer */ - void remove(size_t i) { factors_[i].reset(); } + void remove(size_t i) { factors_.at(i).reset(); } /** replace a factor by index */ void replace(size_t index, sharedFactor factor) { at(index) = factor; } From 45ce9ebc7de7deb7c1c76b41e74668211698c5e4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 28 Apr 2021 14:03:03 -0400 Subject: [PATCH 06/13] print default arguments update --- gtsam/gtsam.i | 139 ++++++++++++++++++++++++++------------------------ 1 file changed, 73 insertions(+), 66 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 7dc56eeff..33e9a58ca 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -12,6 +12,9 @@ namespace gtsam { // Actually a FastList #include + +const KeyFormatter DefaultKeyFormatter; + class KeyList { KeyList(); KeyList(const gtsam::KeyList& other); @@ -983,7 +986,7 @@ class CalibratedCamera { static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); // Testable - void print(string s = "PinholeBase") const; + void print(string s = "") const; bool equals(const gtsam::CalibratedCamera& camera, double tol) const; // Manifold @@ -1160,8 +1163,8 @@ virtual class SymbolicFactor { // From Factor size_t size() const; - void print(string s = "Factor", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::SymbolicFactor& other, double tol) const; gtsam::KeyVector keys(); }; @@ -1174,8 +1177,8 @@ virtual class SymbolicFactorGraph { // From FactorGraph void push_back(gtsam::SymbolicFactor* factor); - void print(string s = "FactorGraph", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; size_t size() const; bool exists(size_t idx) const; @@ -1225,8 +1228,8 @@ virtual class SymbolicConditional : gtsam::SymbolicFactor { static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals); // Testable - void print(string s = "Factor", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::SymbolicConditional& other, double tol) const; // Standard interface @@ -1239,8 +1242,8 @@ class SymbolicBayesNet { SymbolicBayesNet(); SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); // Testable - void print(string s = "BayesNet", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; // Standard interface @@ -1261,8 +1264,8 @@ class SymbolicBayesTree { SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); // Testable - void print(string s = "", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter); bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; //Standard Interface @@ -1285,7 +1288,7 @@ class SymbolicBayesTree { // // bool equals(const This& other, double tol) const; // void print(string s = "", -// gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; +// const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; // void printTree() const; // Default indent of "" // void printTree(string indent) const; // size_t numCachedSeparatorMarginals() const; @@ -1320,7 +1323,8 @@ class VariableIndex { // Testable bool equals(const gtsam::VariableIndex& other, double tol) const; void print(string s = "VariableIndex: ", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; // Standard interface size_t size() const; @@ -1559,7 +1563,8 @@ class VectorValues { size_t dim(size_t j) const; bool exists(size_t j) const; void print(string s = "VectorValues", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::VectorValues& expected, double tol) const; void insert(size_t j, Vector value); Vector vector() const; @@ -1590,8 +1595,8 @@ class VectorValues { #include virtual class GaussianFactor { gtsam::KeyVector keys() const; - void print(string s = "Factor", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const; double error(const gtsam::VectorValues& c) const; gtsam::GaussianFactor* clone() const; @@ -1619,8 +1624,8 @@ virtual class JacobianFactor : gtsam::GaussianFactor { JacobianFactor(const gtsam::GaussianFactorGraph& graph); //Testable - void print(string s = "Factor", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; void printKeys(string s) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const; size_t size() const; @@ -1669,8 +1674,8 @@ virtual class HessianFactor : gtsam::GaussianFactor { //Testable size_t size() const; - void print(string s = "Factor", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; void printKeys(string s) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const; double error(const gtsam::VectorValues& c) const; @@ -1695,8 +1700,8 @@ class GaussianFactorGraph { GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); // From FactorGraph - void print(string s = "FactorGraph", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; size_t size() const; gtsam::GaussianFactor* at(size_t idx) const; @@ -1787,21 +1792,23 @@ virtual class GaussianConditional : gtsam::JacobianFactor { size_t name2, Matrix T); //Standard Interface - void print(string s = "GaussianConditional", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GaussianConditional &cg, double tol) const; + void print(string s = "GaussianConditional", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::GaussianConditional& cg, double tol) const; - //Advanced Interface - gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; - gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, const gtsam::VectorValues& rhs) const; - void solveTransposeInPlace(gtsam::VectorValues& gy) const; - void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; - Matrix R() const; - Matrix S() const; - Vector d() const; + // Advanced Interface + gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; + gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, + const gtsam::VectorValues& rhs) const; + void solveTransposeInPlace(gtsam::VectorValues& gy) const; + void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; + Matrix R() const; + Matrix S() const; + Vector d() const; - // enabling serialization functionality - void serialize() const; + // enabling serialization functionality + void serialize() const; }; #include @@ -1811,7 +1818,8 @@ virtual class GaussianDensity : gtsam::GaussianConditional { //Standard Interface void print(string s = "GaussianDensity", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GaussianDensity &cg, double tol) const; Vector mean() const; Matrix covariance() const; @@ -1824,8 +1832,8 @@ virtual class GaussianBayesNet { GaussianBayesNet(const gtsam::GaussianConditional* conditional); // Testable - void print(string s = "BayesNet", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GaussianBayesNet& other, double tol) const; size_t size() const; @@ -1860,8 +1868,8 @@ virtual class GaussianBayesTree { GaussianBayesTree(); GaussianBayesTree(const gtsam::GaussianBayesTree& other); bool equals(const gtsam::GaussianBayesTree& other, double tol) const; - void print(string s = "", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter); size_t size() const; bool empty() const; size_t numCachedSeparatorMarginals() const; @@ -2067,8 +2075,8 @@ class Ordering { Ordering(const gtsam::Ordering& other); // Testable - void print(string s = "", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::Ordering& ord, double tol) const; // Standard interface @@ -2089,8 +2097,8 @@ class NonlinearFactorGraph { NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); // FactorGraph - void print(string s = "NonlinearFactorGraph: ", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; size_t size() const; bool empty() const; @@ -2138,9 +2146,9 @@ virtual class NonlinearFactor { // Factor base class size_t size() const; gtsam::KeyVector keys() const; - void print(string s = "Factor", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; - void printKeys(string s = "Factor") const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + void printKeys(string s) const; // NonlinearFactor bool equals(const gtsam::NonlinearFactor& other, double tol) const; double error(const gtsam::Values& c) const; @@ -2169,8 +2177,8 @@ class Values { void clear(); size_t dim() const; - void print(string s = "", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::Values& other, double tol) const; void insert(const gtsam::Values& values); @@ -2259,8 +2267,8 @@ class Marginals { Marginals(const gtsam::GaussianFactorGraph& gfgraph, const gtsam::VectorValues& solutionvec); - void print(string s = "Marginals: ", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + void print(string s = "Marginals: ", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; Matrix marginalCovariance(size_t variable) const; Matrix marginalInformation(size_t variable) const; gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const; @@ -2270,8 +2278,7 @@ class Marginals { class JointMarginal { Matrix at(size_t iVariable, size_t jVariable) const; Matrix fullMatrix() const; - void print(string s = "", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + void print(string s = "", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; }; #include @@ -2508,8 +2515,7 @@ class ISAM2Clique { //Standard Interface Vector gradientContribution() const; - void print(string s = "", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); + void print(string s = "", gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter); }; class ISAM2Result { @@ -2531,7 +2537,8 @@ class ISAM2 { ISAM2(const gtsam::ISAM2& other); bool equals(const gtsam::ISAM2& other, double tol) const; - void print(string s = "") const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; void printStats() const; void saveGraph(string s) const; @@ -2563,8 +2570,8 @@ class ISAM2 { class NonlinearISAM { NonlinearISAM(); NonlinearISAM(int reorderInterval); - void print(string s = "", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; void printStats() const; void saveGraph(string s) const; gtsam::Values estimate() const; @@ -3428,8 +3435,8 @@ virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ const gtsam::Unit3& bRef); Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); Rot3AttitudeFactor(); - void print(string s = "", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::NonlinearFactor& expected, double tol) const; gtsam::Unit3 nZ() const; gtsam::Unit3 bRef() const; @@ -3442,8 +3449,8 @@ virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); Pose3AttitudeFactor(); - void print(string s = "Factor", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::NonlinearFactor& expected, double tol) const; gtsam::Unit3 nZ() const; gtsam::Unit3 bRef() const; @@ -3455,8 +3462,8 @@ virtual class GPSFactor : gtsam::NonlinearFactor{ const gtsam::noiseModel::Base* model); // Testable - void print(string s = "", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GPSFactor& expected, double tol); // Standard Interface @@ -3468,8 +3475,8 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor { const gtsam::noiseModel::Base* model); // Testable - void print(string s = "Factor", - gtsam::KeyFormatter keyFormatter = gtsam::DefaultKeyFormatter) const; + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::GPSFactor2& expected, double tol); // Standard Interface From 4d6eef2c2ff2088cb54e53241ce061e0fa978459 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 29 Apr 2021 19:43:27 -0400 Subject: [PATCH 07/13] override print methods and update wrapper --- gtsam/discrete/DiscreteFactor.h | 11 ++-- gtsam/discrete/DiscreteFactorGraph.h | 5 +- gtsam/geometry/CalibratedCamera.h | 7 ++- gtsam/geometry/PinholeCamera.h | 2 +- gtsam/geometry/PinholePose.h | 2 +- gtsam/gtsam.i | 22 ++++---- gtsam/inference/BayesNet-inst.h | 52 +++++++++---------- gtsam/inference/BayesNet.h | 16 +++--- gtsam/inference/Factor.cpp | 4 +- gtsam/inference/Factor.h | 8 ++- gtsam/inference/FactorGraph-inst.h | 2 +- gtsam/inference/FactorGraph.h | 6 +-- gtsam/linear/GaussianBayesNet.h | 7 +++ gtsam/linear/GaussianFactor.h | 7 ++- gtsam/navigation/AttitudeFactor.cpp | 3 +- gtsam/navigation/AttitudeFactor.h | 8 +-- gtsam/navigation/GPSFactor.cpp | 3 +- gtsam/navigation/GPSFactor.h | 8 +-- gtsam/nonlinear/NonlinearFactor.h | 5 +- gtsam/nonlinear/NonlinearFactorGraph.h | 5 +- gtsam/sfm/BinaryMeasurement.h | 4 +- gtsam/symbolic/SymbolicBayesNet.h | 7 +++ gtsam/symbolic/SymbolicConditional.cpp | 25 +++++---- gtsam/symbolic/SymbolicConditional.h | 4 +- gtsam/symbolic/SymbolicFactor.h | 14 +++++ gtsam/symbolic/SymbolicFactorGraph.h | 7 +++ gtsam_unstable/linear/InequalityFactorGraph.h | 5 +- 27 files changed, 155 insertions(+), 94 deletions(-) diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index e24dfdf2a..f8b3fc0bb 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -74,13 +74,14 @@ public: /// @name Testable /// @{ - // equals + /// equals virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const = 0; - // print - virtual void print(const std::string& s = "DiscreteFactor\n", - const KeyFormatter& formatter = DefaultKeyFormatter) const { - Factor::print(s, formatter); + /// print + virtual void print( + const std::string& s = "DiscreteFactor\n", + const KeyFormatter& formatter = DefaultKeyFormatter) const override { + Base::print(s, formatter); } /** Test whether the factor is empty */ diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index 4c2607f1f..8df602af5 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -129,8 +129,9 @@ public: double operator()(const DiscreteFactor::Values & values) const; /// print - void print(const std::string& s = "DiscreteFactorGraph", - const KeyFormatter& formatter =DefaultKeyFormatter) const; + void print( + const std::string& s = "DiscreteFactorGraph", + const KeyFormatter& formatter = DefaultKeyFormatter) const override; /** Solve the factor graph by performing variable elimination in COLAMD order using * the dense elimination function specified in \c function, diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index eff747eb5..97ebe8c7e 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -144,7 +144,7 @@ public: bool equals(const PinholeBase &camera, double tol = 1e-9) const; /// print - void print(const std::string& s = "PinholeBase") const; + virtual void print(const std::string& s = "PinholeBase") const; /// @} /// @name Standard Interface @@ -324,6 +324,11 @@ public: /// Return canonical coordinate Vector localCoordinates(const CalibratedCamera& T2) const; + /// print + void print(const std::string& s = "CalibratedCamera") const override { + PinholeBase::print(s); + } + /// @deprecated inline size_t dim() const { return dimension; diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index ecff766e2..8ac67a5c3 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -148,7 +148,7 @@ public: } /// print - void print(const std::string& s = "PinholeCamera") const { + void print(const std::string& s = "PinholeCamera") const override { Base::print(s); K_.print(s + ".calibration"); } diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index 1b5a06609..949caaa27 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -340,7 +340,7 @@ public: } /// print - void print(const std::string& s = "PinholePose") const { + void print(const std::string& s = "PinholePose") const override { Base::print(s); if (!K_) std::cout << "s No calibration given" << std::endl; diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 33e9a58ca..c5bf6511c 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -986,7 +986,7 @@ class CalibratedCamera { static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); // Testable - void print(string s = "") const; + void print(string s = "CalibratedCamera") const; bool equals(const gtsam::CalibratedCamera& camera, double tol) const; // Manifold @@ -1163,8 +1163,9 @@ virtual class SymbolicFactor { // From Factor size_t size() const; - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; + void print(string s = "SymbolicFactor", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::SymbolicFactor& other, double tol) const; gtsam::KeyVector keys(); }; @@ -1177,8 +1178,9 @@ virtual class SymbolicFactorGraph { // From FactorGraph void push_back(gtsam::SymbolicFactor* factor); - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; + void print(string s = "SymbolicFactorGraph", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; size_t size() const; bool exists(size_t idx) const; @@ -1242,8 +1244,9 @@ class SymbolicBayesNet { SymbolicBayesNet(); SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); // Testable - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; + void print(string s = "SymbolicBayesNet", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; // Standard interface @@ -2097,8 +2100,9 @@ class NonlinearFactorGraph { NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); // FactorGraph - void print(string s = "", const gtsam::KeyFormatter& keyFormatter = - gtsam::DefaultKeyFormatter) const; + void print(string s = "NonlinearFactorGraph: ", + const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; size_t size() const; bool empty() const; diff --git a/gtsam/inference/BayesNet-inst.h b/gtsam/inference/BayesNet-inst.h index a3bd87887..a73762258 100644 --- a/gtsam/inference/BayesNet-inst.h +++ b/gtsam/inference/BayesNet-inst.h @@ -26,30 +26,30 @@ namespace gtsam { - /* ************************************************************************* */ - template - void BayesNet::print(const std::string& s, const KeyFormatter& formatter) const - { - Base::print(s, formatter); - } - - /* ************************************************************************* */ - template - void BayesNet::saveGraph(const std::string &s, const KeyFormatter& keyFormatter) const - { - std::ofstream of(s.c_str()); - of << "digraph G{\n"; - - for (auto conditional: boost::adaptors::reverse(*this)) { - typename CONDITIONAL::Frontals frontals = conditional->frontals(); - Key me = frontals.front(); - typename CONDITIONAL::Parents parents = conditional->parents(); - for(Key p: parents) - of << keyFormatter(p) << "->" << keyFormatter(me) << std::endl; - } - - of << "}"; - of.close(); - } - +/* ************************************************************************* */ +template +void BayesNet::print( + const std::string& s, const KeyFormatter& formatter) const { + Base::print(s, formatter); } + +/* ************************************************************************* */ +template +void BayesNet::saveGraph(const std::string& s, + const KeyFormatter& keyFormatter) const { + std::ofstream of(s.c_str()); + of << "digraph G{\n"; + + for (auto conditional : boost::adaptors::reverse(*this)) { + typename CONDITIONAL::Frontals frontals = conditional->frontals(); + Key me = frontals.front(); + typename CONDITIONAL::Parents parents = conditional->parents(); + for (Key p : parents) + of << keyFormatter(p) << "->" << keyFormatter(me) << std::endl; + } + + of << "}"; + of.close(); +} + +} // namespace gtsam diff --git a/gtsam/inference/BayesNet.h b/gtsam/inference/BayesNet.h index 0597ece98..938278d5a 100644 --- a/gtsam/inference/BayesNet.h +++ b/gtsam/inference/BayesNet.h @@ -57,16 +57,18 @@ namespace gtsam { /// @name Testable /// @{ - /** print out graph */ - void print(const std::string& s = "BayesNet", - const KeyFormatter& formatter = DefaultKeyFormatter) const; + /** print out graph */ + void print( + const std::string& s = "BayesNet", + const KeyFormatter& formatter = DefaultKeyFormatter) const override; - /// @} + /// @} - /// @name Standard Interface - /// @{ + /// @name Standard Interface + /// @{ - void saveGraph(const std::string &s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + void saveGraph(const std::string& s, + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; }; } diff --git a/gtsam/inference/Factor.cpp b/gtsam/inference/Factor.cpp index 58448edbb..6fe96c777 100644 --- a/gtsam/inference/Factor.cpp +++ b/gtsam/inference/Factor.cpp @@ -33,8 +33,8 @@ namespace gtsam { /* ************************************************************************* */ void Factor::printKeys(const std::string& s, const KeyFormatter& formatter) const { - std::cout << s << " "; - for(Key key: keys_) std::cout << " " << formatter(key); + std::cout << (s.empty() ? "" : s + " "); + for (Key key : keys_) std::cout << " " << formatter(key); std::cout << std::endl; } diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 1aaaff0e4..57f95b0ea 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -135,10 +135,14 @@ typedef FastSet FactorIndexSet; /// @{ /// print - void print(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const; + virtual void print( + const std::string& s = "Factor", + const KeyFormatter& formatter = DefaultKeyFormatter) const; /// print only keys - void printKeys(const std::string& s = "Factor", const KeyFormatter& formatter = DefaultKeyFormatter) const; + virtual void printKeys( + const std::string& s = "Factor", + const KeyFormatter& formatter = DefaultKeyFormatter) const; protected: /// check equality diff --git a/gtsam/inference/FactorGraph-inst.h b/gtsam/inference/FactorGraph-inst.h index df68019e1..e1c18274a 100644 --- a/gtsam/inference/FactorGraph-inst.h +++ b/gtsam/inference/FactorGraph-inst.h @@ -37,7 +37,7 @@ namespace gtsam { template void FactorGraph::print(const std::string& s, const KeyFormatter& formatter) const { - std::cout << s << std::endl; + std::cout << (s.empty() ? "" : s + " ") << std::endl; std::cout << "size: " << size() << std::endl; for (size_t i = 0; i < factors_.size(); i++) { std::stringstream ss; diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 9d2308d9b..90b9d7ef2 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -285,9 +285,9 @@ class FactorGraph { /// @name Testable /// @{ - /** print out graph */ - void print(const std::string& s = "FactorGraph", - const KeyFormatter& formatter = DefaultKeyFormatter) const; + /// print out graph + virtual void print(const std::string& s = "FactorGraph", + const KeyFormatter& formatter = DefaultKeyFormatter) const; /** Check equality */ bool equals(const This& fg, double tol = 1e-9) const; diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 06782c3cf..a45168e0b 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -177,6 +177,13 @@ namespace gtsam { */ VectorValues backSubstituteTranspose(const VectorValues& gx) const; + /// print graph + virtual void print( + const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const override { + Base::print(s, formatter); + } + /** * @brief Save the GaussianBayesNet as an image. Requires `dot` to be * installed. diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 9b4c5f940..72ad69693 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -54,8 +54,11 @@ namespace gtsam { virtual ~GaussianFactor() {} // Implementing Testable interface - virtual void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const = 0; + + /// print + virtual void print( + const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const override = 0; /** Equals for testable */ virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const = 0; diff --git a/gtsam/navigation/AttitudeFactor.cpp b/gtsam/navigation/AttitudeFactor.cpp index 7f335152e..8c8eb5772 100644 --- a/gtsam/navigation/AttitudeFactor.cpp +++ b/gtsam/navigation/AttitudeFactor.cpp @@ -42,7 +42,8 @@ Vector AttitudeFactor::attitudeError(const Rot3& nRb, //*************************************************************************** void Rot3AttitudeFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << s << "Rot3AttitudeFactor on " << keyFormatter(this->key()) << "\n"; + cout << (s.empty() ? "" : s + " ") << "Rot3AttitudeFactor on " + << keyFormatter(this->key()) << "\n"; nZ_.print(" measured direction in nav frame: "); bRef_.print(" reference direction in body frame: "); this->noiseModel_->print(" noise model: "); diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 23fbbca89..3016b31af 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -114,8 +114,8 @@ public: } /** print */ - void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const override; + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; /** equals */ bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; @@ -188,8 +188,8 @@ public: } /** print */ - void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const override; + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; /** equals */ bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp index f2448c488..1d6b78e13 100644 --- a/gtsam/navigation/GPSFactor.cpp +++ b/gtsam/navigation/GPSFactor.cpp @@ -24,7 +24,8 @@ namespace gtsam { //*************************************************************************** void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { - cout << s << "GPSFactor on " << keyFormatter(key()) << "\n"; + cout << (s.empty() ? "" : s + " ") << "GPSFactor on " << keyFormatter(key()) + << "\n"; cout << " GPS measurement: " << nT_ << "\n"; noiseModel_->print(" noise model: "); } diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index f6469346e..8fcf0f099 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -71,8 +71,8 @@ public: } /// print - void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const override; + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; /// equals bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; @@ -143,8 +143,8 @@ public: } /// print - void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const override; + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; /// equals bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 8c257f7ca..adb6310e8 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -70,8 +70,9 @@ public: /// @{ /** print */ - virtual void print(const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /** Check if two factors are equal */ virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const; diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 989f493d3..9bca4a29d 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -99,8 +99,9 @@ namespace gtsam { NonlinearFactorGraph(const FactorGraph& graph) : Base(graph) {} /** print */ - void print(const std::string& str = "NonlinearFactorGraph: ", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + void print( + const std::string& str = "NonlinearFactorGraph: ", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /** print errors along with factors*/ void printErrors(const Values& values, const std::string& str = "NonlinearFactorGraph: ", diff --git a/gtsam/sfm/BinaryMeasurement.h b/gtsam/sfm/BinaryMeasurement.h index 99e553f7a..7e102fee7 100644 --- a/gtsam/sfm/BinaryMeasurement.h +++ b/gtsam/sfm/BinaryMeasurement.h @@ -64,8 +64,8 @@ private: /// @name Testable /// @{ - void print(const std::string &s, - const KeyFormatter &keyFormatter = DefaultKeyFormatter) const { + void print(const std::string &s, const KeyFormatter &keyFormatter = + DefaultKeyFormatter) const override { std::cout << s << "BinaryMeasurement(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << ")\n"; traits::Print(measured_, " measured: "); diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index ca87b2bbc..45df56abd 100644 --- a/gtsam/symbolic/SymbolicBayesNet.h +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -63,6 +63,13 @@ namespace gtsam { /** Check equality */ GTSAM_EXPORT bool equals(const This& bn, double tol = 1e-9) const; + /// print + GTSAM_EXPORT void print( + const std::string& s = "SymbolicBayesNet", + const KeyFormatter& formatter = DefaultKeyFormatter) const override { + Base::print(s, formatter); + } + /// @} /// @name Standard Interface diff --git a/gtsam/symbolic/SymbolicConditional.cpp b/gtsam/symbolic/SymbolicConditional.cpp index d733c0937..f0d9944b2 100644 --- a/gtsam/symbolic/SymbolicConditional.cpp +++ b/gtsam/symbolic/SymbolicConditional.cpp @@ -20,18 +20,17 @@ namespace gtsam { - using namespace std; - - /* ************************************************************************* */ - void SymbolicConditional::print(const std::string& str, const KeyFormatter& keyFormatter) const - { - BaseConditional::print(str, keyFormatter); - } - - /* ************************************************************************* */ - bool SymbolicConditional::equals(const This& c, double tol) const - { - return BaseFactor::equals(c) && BaseConditional::equals(c); - } +using namespace std; +/* ************************************************************************* */ +void SymbolicConditional::print(const std::string& str, + const KeyFormatter& keyFormatter) const { + BaseConditional::print(str, keyFormatter); } + +/* ************************************************************************* */ +bool SymbolicConditional::equals(const This& c, double tol) const { + return BaseFactor::equals(c) && BaseConditional::equals(c); +} + +} // namespace gtsam diff --git a/gtsam/symbolic/SymbolicConditional.h b/gtsam/symbolic/SymbolicConditional.h index ead72a989..4088cbfb6 100644 --- a/gtsam/symbolic/SymbolicConditional.h +++ b/gtsam/symbolic/SymbolicConditional.h @@ -105,7 +105,9 @@ namespace gtsam { /// @name Testable /** Print with optional formatter */ - virtual void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print( + const std::string& str = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /** Check equality */ bool equals(const This& c, double tol = 1e-9) const; diff --git a/gtsam/symbolic/SymbolicFactor.h b/gtsam/symbolic/SymbolicFactor.h index 66657aa7d..2a488a4da 100644 --- a/gtsam/symbolic/SymbolicFactor.h +++ b/gtsam/symbolic/SymbolicFactor.h @@ -92,6 +92,20 @@ namespace gtsam { bool equals(const This& other, double tol = 1e-9) const; + /// print + void print( + const std::string& s = "SymbolicFactor", + const KeyFormatter& formatter = DefaultKeyFormatter) const override { + Base::print(s, formatter); + } + + /// print only keys + void printKeys( + const std::string& s = "SymbolicFactor", + const KeyFormatter& formatter = DefaultKeyFormatter) const override { + Base::printKeys(s, formatter); + } + /// @} /// @name Advanced Constructors diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index b6f0de2ae..7f4c84631 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -88,6 +88,13 @@ namespace gtsam { bool equals(const This& fg, double tol = 1e-9) const; + /// print + void print( + const std::string& s = "SymbolicFactorGraph", + const KeyFormatter& formatter = DefaultKeyFormatter) const override { + Base::print(s, formatter); + } + /// @} /// @name Standard Interface diff --git a/gtsam_unstable/linear/InequalityFactorGraph.h b/gtsam_unstable/linear/InequalityFactorGraph.h index d042b0436..7016a7e97 100644 --- a/gtsam_unstable/linear/InequalityFactorGraph.h +++ b/gtsam_unstable/linear/InequalityFactorGraph.h @@ -37,8 +37,9 @@ public: typedef boost::shared_ptr shared_ptr; /** print */ - void print(const std::string& str, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { + void print( + const std::string& str = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(str, keyFormatter); } From 6c3aca8cac62989eee3c1d23db77e25ddc1f4b87 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 30 Apr 2021 12:58:52 -0400 Subject: [PATCH 08/13] remove virtual from overridden methods, add virtual destructors to appease compiler --- gtsam/discrete/DiscreteBayesNet.h | 3 +++ gtsam/discrete/DiscreteFactor.h | 2 +- gtsam/discrete/DiscreteFactorGraph.h | 3 +++ gtsam/linear/GaussianBayesNet.h | 5 ++++- gtsam/linear/GaussianFactor.h | 2 +- gtsam/linear/Preconditioner.h | 4 ++-- gtsam/nonlinear/NonlinearFactor.h | 5 ++--- gtsam/nonlinear/NonlinearFactorGraph.h | 3 +++ gtsam/sfm/BinaryMeasurement.h | 3 +++ gtsam/symbolic/SymbolicBayesNet.h | 3 +++ gtsam/symbolic/SymbolicConditional.h | 2 +- gtsam/symbolic/SymbolicFactorGraph.h | 3 +++ gtsam_unstable/discrete/Scheduler.cpp | 5 ++--- gtsam_unstable/discrete/Scheduler.h | 18 +++++++++++------- .../ConcurrentFilteringAndSmoothing.h | 8 ++++++-- gtsam_unstable/nonlinear/FixedLagSmoother.h | 4 +++- .../EquivInertialNavFactor_GlobalVel_NoBias.h | 4 +++- 17 files changed, 54 insertions(+), 23 deletions(-) diff --git a/gtsam/discrete/DiscreteBayesNet.h b/gtsam/discrete/DiscreteBayesNet.h index 237caf745..d5ba30584 100644 --- a/gtsam/discrete/DiscreteBayesNet.h +++ b/gtsam/discrete/DiscreteBayesNet.h @@ -55,6 +55,9 @@ namespace gtsam { template DiscreteBayesNet(const FactorGraph& graph) : Base(graph) {} + /// Destructor + virtual ~DiscreteBayesNet() {} + /// @} /// @name Testable diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index f8b3fc0bb..6b0919507 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -78,7 +78,7 @@ public: virtual bool equals(const DiscreteFactor& lf, double tol = 1e-9) const = 0; /// print - virtual void print( + void print( const std::string& s = "DiscreteFactor\n", const KeyFormatter& formatter = DefaultKeyFormatter) const override { Base::print(s, formatter); diff --git a/gtsam/discrete/DiscreteFactorGraph.h b/gtsam/discrete/DiscreteFactorGraph.h index 8df602af5..f39adc9a8 100644 --- a/gtsam/discrete/DiscreteFactorGraph.h +++ b/gtsam/discrete/DiscreteFactorGraph.h @@ -91,6 +91,9 @@ public: template DiscreteFactorGraph(const FactorGraph& graph) : Base(graph) {} + /// Destructor + virtual ~DiscreteFactorGraph() {} + /// @name Testable /// @{ diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index a45168e0b..e55a89bcd 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -55,6 +55,9 @@ namespace gtsam { template GaussianBayesNet(const FactorGraph& graph) : Base(graph) {} + /// Destructor + virtual ~GaussianBayesNet() {} + /// @} /// @name Testable @@ -178,7 +181,7 @@ namespace gtsam { VectorValues backSubstituteTranspose(const VectorValues& gx) const; /// print graph - virtual void print( + void print( const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const override { Base::print(s, formatter); diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index 72ad69693..334722868 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -56,7 +56,7 @@ namespace gtsam { // Implementing Testable interface /// print - virtual void print( + void print( const std::string& s = "", const KeyFormatter& formatter = DefaultKeyFormatter) const override = 0; diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h index 3a406c0a5..1a5a08090 100644 --- a/gtsam/linear/Preconditioner.h +++ b/gtsam/linear/Preconditioner.h @@ -44,9 +44,9 @@ struct GTSAM_EXPORT PreconditionerParameters { inline Kernel kernel() const { return kernel_; } inline Verbosity verbosity() const { return verbosity_; } - void print() const ; + void print() const; - virtual void print(std::ostream &os) const ; + virtual void print(std::ostream &os) const; static Kernel kernelTranslator(const std::string &s); static Verbosity verbosityTranslator(const std::string &s); diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index adb6310e8..21c05dc2c 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -70,9 +70,8 @@ public: /// @{ /** print */ - virtual void print( - const std::string& s = "", - const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; /** Check if two factors are equal */ virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const; diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 9bca4a29d..4d321f8ab 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -98,6 +98,9 @@ namespace gtsam { template NonlinearFactorGraph(const FactorGraph& graph) : Base(graph) {} + /// Destructor + virtual ~NonlinearFactorGraph() {} + /** print */ void print( const std::string& str = "NonlinearFactorGraph: ", diff --git a/gtsam/sfm/BinaryMeasurement.h b/gtsam/sfm/BinaryMeasurement.h index 7e102fee7..bdb45d622 100644 --- a/gtsam/sfm/BinaryMeasurement.h +++ b/gtsam/sfm/BinaryMeasurement.h @@ -52,6 +52,9 @@ private: measured_(measured), noiseModel_(model) {} + /// Destructor + virtual ~BinaryMeasurement() {} + /// @name Standard Interface /// @{ diff --git a/gtsam/symbolic/SymbolicBayesNet.h b/gtsam/symbolic/SymbolicBayesNet.h index 45df56abd..464af060b 100644 --- a/gtsam/symbolic/SymbolicBayesNet.h +++ b/gtsam/symbolic/SymbolicBayesNet.h @@ -55,6 +55,9 @@ namespace gtsam { template SymbolicBayesNet(const FactorGraph& graph) : Base(graph) {} + /// Destructor + virtual ~SymbolicBayesNet() {} + /// @} /// @name Testable diff --git a/gtsam/symbolic/SymbolicConditional.h b/gtsam/symbolic/SymbolicConditional.h index 4088cbfb6..3abec92b8 100644 --- a/gtsam/symbolic/SymbolicConditional.h +++ b/gtsam/symbolic/SymbolicConditional.h @@ -105,7 +105,7 @@ namespace gtsam { /// @name Testable /** Print with optional formatter */ - virtual void print( + void print( const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; diff --git a/gtsam/symbolic/SymbolicFactorGraph.h b/gtsam/symbolic/SymbolicFactorGraph.h index 7f4c84631..36379fd83 100644 --- a/gtsam/symbolic/SymbolicFactorGraph.h +++ b/gtsam/symbolic/SymbolicFactorGraph.h @@ -81,6 +81,9 @@ namespace gtsam { template SymbolicFactorGraph(const FactorGraph& graph) : Base(graph) {} + /// Destructor + virtual ~SymbolicFactorGraph() {} + /// @} /// @name Testable diff --git a/gtsam_unstable/discrete/Scheduler.cpp b/gtsam_unstable/discrete/Scheduler.cpp index a81048291..3273778c4 100644 --- a/gtsam_unstable/discrete/Scheduler.cpp +++ b/gtsam_unstable/discrete/Scheduler.cpp @@ -178,8 +178,7 @@ namespace gtsam { } // buildGraph /** print */ - void Scheduler::print(const string& s) const { - + void Scheduler::print(const string& s, const KeyFormatter& formatter) const { cout << s << " Faculty:" << endl; for(const string& name: facultyName_) cout << name << '\n'; @@ -210,7 +209,7 @@ namespace gtsam { CSP::print(s + " Factor graph"); cout << endl; - } // print + } // print /** Print readable form of assignment */ void Scheduler::printAssignment(sharedValues assignment) const { diff --git a/gtsam_unstable/discrete/Scheduler.h b/gtsam_unstable/discrete/Scheduler.h index 15ba60f46..6faf9956f 100644 --- a/gtsam_unstable/discrete/Scheduler.h +++ b/gtsam_unstable/discrete/Scheduler.h @@ -66,15 +66,17 @@ namespace gtsam { /** * Constructor - * WE need to know the number of students in advance for ordering keys. + * We need to know the number of students in advance for ordering keys. * then add faculty, slots, areas, availability, students, in that order */ - Scheduler(size_t maxNrStudents):maxNrStudents_(maxNrStudents) { - } + Scheduler(size_t maxNrStudents) : maxNrStudents_(maxNrStudents) {} - void addFaculty(const std::string& facultyName) { - facultyIndex_[facultyName] = nrFaculty(); - facultyName_.push_back(facultyName); + /// Destructor + virtual ~Scheduler() {} + + void addFaculty(const std::string& facultyName) { + facultyIndex_[facultyName] = nrFaculty(); + facultyName_.push_back(facultyName); } size_t nrFaculty() const { @@ -140,7 +142,9 @@ namespace gtsam { void buildGraph(size_t mutexBound = 7); /** print */ - void print(const std::string& s = "Scheduler") const; + void print( + const std::string& s = "Scheduler", + const KeyFormatter& formatter = DefaultKeyFormatter) const override; /** Print readable form of assignment */ void printAssignment(sharedValues assignment) const; diff --git a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h index 316db921a..c87b99275 100644 --- a/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h +++ b/gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h @@ -47,7 +47,9 @@ public: virtual ~ConcurrentFilter() {}; /** Implement a standard 'print' function */ - virtual void print(const std::string& s = "Concurrent Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const = 0; + virtual void print( + const std::string& s = "Concurrent Filter:\n", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const = 0; /** Check if two Concurrent Smoothers are equal */ virtual bool equals(const ConcurrentFilter& rhs, double tol = 1e-9) const = 0; @@ -107,7 +109,9 @@ public: virtual ~ConcurrentSmoother() {}; /** Implement a standard 'print' function */ - virtual void print(const std::string& s = "Concurrent Smoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const = 0; + virtual void print( + const std::string& s = "Concurrent Smoother:\n", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const = 0; /** Check if two Concurrent Smoothers are equal */ virtual bool equals(const ConcurrentSmoother& rhs, double tol = 1e-9) const = 0; diff --git a/gtsam_unstable/nonlinear/FixedLagSmoother.h b/gtsam_unstable/nonlinear/FixedLagSmoother.h index 362cfae96..17fcf3908 100644 --- a/gtsam_unstable/nonlinear/FixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/FixedLagSmoother.h @@ -69,7 +69,9 @@ public: virtual ~FixedLagSmoother() { } /** Print the factor for debugging and testing (implementing Testable) */ - virtual void print(const std::string& s = "FixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + virtual void print( + const std::string& s = "FixedLagSmoother:\n", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** Check if two IncrementalFixedLagSmoother Objects are equal */ virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const; diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h index 9f5d800db..40dc81c9a 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h @@ -132,7 +132,9 @@ public: /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s = "EquivInertialNavFactor_GlobalVel_NoBias", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + virtual void print( + const std::string& s = "EquivInertialNavFactor_GlobalVel_NoBias", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2()) << "," From 0a501a561549d98181b1fc6d1d8dfb34042b2ba2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 3 May 2021 17:01:18 -0400 Subject: [PATCH 09/13] fix warnings from tests --- gtsam/geometry/CalibratedCamera.h | 18 ++++----- gtsam/inference/Factor.h | 61 +++++++++++++++++-------------- gtsam/inference/FactorGraph.h | 4 ++ 3 files changed, 45 insertions(+), 38 deletions(-) diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 97ebe8c7e..1d4a379a1 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -119,22 +119,20 @@ public: /// @name Standard Constructors /// @{ - /** default constructor */ - PinholeBase() { - } + /// Default constructor + PinholeBase() {} - /** constructor with pose */ - explicit PinholeBase(const Pose3& pose) : - pose_(pose) { - } + /// Constructor with pose + explicit PinholeBase(const Pose3& pose) : pose_(pose) {} /// @} /// @name Advanced Constructors /// @{ - explicit PinholeBase(const Vector &v) : - pose_(Pose3::Expmap(v)) { - } + explicit PinholeBase(const Vector& v) : pose_(Pose3::Expmap(v)) {} + + /// Default destructor + virtual ~PinholeBase() = default; /// @} /// @name Testable diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 57f95b0ea..6ea81030a 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -102,47 +102,52 @@ typedef FastSet FactorIndexSet; /// @} public: - /// @name Standard Interface - /// @{ + /// Default destructor + // public since it is required for boost serialization and static methods. + // virtual since it is public. + // http://isocpp.github.io/CppCoreGuidelines/CppCoreGuidelines#Rc-dtor-virtual + virtual ~Factor() = default; - /// First key - Key front() const { return keys_.front(); } + /// @name Standard Interface + /// @{ - /// Last key - Key back() const { return keys_.back(); } + /// First key + Key front() const { return keys_.front(); } - /// find - const_iterator find(Key key) const { return std::find(begin(), end(), key); } + /// Last key + Key back() const { return keys_.back(); } - /// Access the factor's involved variable keys - const KeyVector& keys() const { return keys_; } + /// find + const_iterator find(Key key) const { return std::find(begin(), end(), key); } - /** Iterator at beginning of involved variable keys */ - const_iterator begin() const { return keys_.begin(); } + /// Access the factor's involved variable keys + const KeyVector& keys() const { return keys_; } - /** Iterator at end of involved variable keys */ - const_iterator end() const { return keys_.end(); } + /** Iterator at beginning of involved variable keys */ + const_iterator begin() const { return keys_.begin(); } - /** + /** Iterator at end of involved variable keys */ + const_iterator end() const { return keys_.end(); } + + /** * @return the number of variables involved in this factor */ - size_t size() const { return keys_.size(); } + size_t size() const { return keys_.size(); } - /// @} + /// @} + /// @name Testable + /// @{ - /// @name Testable - /// @{ + /// print + virtual void print( + const std::string& s = "Factor", + const KeyFormatter& formatter = DefaultKeyFormatter) const; - /// print - virtual void print( - const std::string& s = "Factor", - const KeyFormatter& formatter = DefaultKeyFormatter) const; - - /// print only keys - virtual void printKeys( - const std::string& s = "Factor", - const KeyFormatter& formatter = DefaultKeyFormatter) const; + /// print only keys + virtual void printKeys( + const std::string& s = "Factor", + const KeyFormatter& formatter = DefaultKeyFormatter) const; protected: /// check equality diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 90b9d7ef2..a9ca7f84d 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -148,6 +148,10 @@ class FactorGraph { /// @} public: + /// Default destructor + // Public and virtual so boost serialization can call it. + virtual ~FactorGraph() = default; + /// @name Adding Single Factors /// @{ From cba8f626422b46336bfa9436cf3821dd313e265e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 3 May 2021 17:01:26 -0400 Subject: [PATCH 10/13] fix unused warning --- gtsam/geometry/tests/testRot3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 889f68580..34f90c8cc 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -825,7 +825,7 @@ TEST(Rot3, RQ_derivative) { const auto R = Rot3::RzRyRx(xyz).matrix(); const auto num = numericalDerivative11(RQ_proxy, R); Matrix39 calc; - RQ(R, calc).second; + RQ(R, calc); const auto err = vec_err.second; CHECK(assert_equal(num, calc, err)); From c398a629432f0c23a62477925a64633b482273c0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 4 May 2021 12:16:38 -0400 Subject: [PATCH 11/13] fix some interface todos --- gtsam/gtsam.i | 53 ++++++++++++++++++++++++++------------------------- 1 file changed, 27 insertions(+), 26 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index c5bf6511c..d053c8422 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -10,11 +10,11 @@ namespace gtsam { -// Actually a FastList #include const KeyFormatter DefaultKeyFormatter; +// Actually a FastList class KeyList { KeyList(); KeyList(const gtsam::KeyList& other); @@ -1284,31 +1284,26 @@ class SymbolicBayesTree { gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; }; -// class SymbolicBayesTreeClique { -// SymbolicBayesTreeClique(); -// SymbolicBayesTreeClique(CONDITIONAL* conditional); -// SymbolicBayesTreeClique(const pair& result) : Base(result) {} -// -// bool equals(const This& other, double tol) const; -// void print(string s = "", -// const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; -// void printTree() const; // Default indent of "" -// void printTree(string indent) const; -// size_t numCachedSeparatorMarginals() const; -// -// CONDITIONAL* conditional() const; -// bool isRoot() const; -// size_t treeSize() const; -// const std::list& children() const { return children_; } -// derived_ptr parent() const { return parent_.lock(); } -// +class SymbolicBayesTreeClique { + SymbolicBayesTreeClique(); + // SymbolicBayesTreeClique(gtsam::sharedConditional* conditional); + + bool equals(const gtsam::SymbolicBayesTreeClique& other, double tol) const; + void print(string s = "", + const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; + size_t numCachedSeparatorMarginals() const; + // gtsam::sharedConditional* conditional() const; + bool isRoot() const; + size_t treeSize() const; + gtsam::SymbolicBayesTreeClique* parent() const; + // // TODO: need wrapped versions graphs, BayesNet // BayesNet shortcut(derived_ptr root, Eliminate function) const; // FactorGraph marginal(derived_ptr root, Eliminate function) const; // FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; // -// void deleteCachedShortcuts(); -// }; + void deleteCachedShortcuts(); +}; #include class VariableIndex { @@ -1554,7 +1549,7 @@ class Sampler { #include class VectorValues { - //Constructors + //Constructors VectorValues(); VectorValues(const gtsam::VectorValues& other); @@ -2160,7 +2155,7 @@ virtual class NonlinearFactor { 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; //TODO: Conversion from KeyVector to std::vector does not happen + gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; }; #include @@ -2778,11 +2773,17 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { }; #include + +/// Linearization mode: what factor to linearize to +enum LinearizationMode { HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD }; + +/// How to manage degeneracy +enum DegeneracyMode { IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY }; + class SmartProjectionParams { SmartProjectionParams(); - // TODO(frank): make these work: - // void setLinearizationMode(LinearizationMode linMode); - // void setDegeneracyMode(DegeneracyMode degMode); + void setLinearizationMode(gtsam::LinearizationMode linMode); + void setDegeneracyMode(gtsam::DegeneracyMode degMode); void setRankTolerance(double rankTol); void setEnableEPI(bool enableEPI); void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); From 26a8b602a597779a6b624404ae1cfdd305e77623 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 4 May 2021 12:17:36 -0400 Subject: [PATCH 12/13] add pybind11/operators.h to interface template --- python/gtsam/gtsam.tpl | 1 + 1 file changed, 1 insertion(+) diff --git a/python/gtsam/gtsam.tpl b/python/gtsam/gtsam.tpl index 0e0881ce9..b800f7c35 100644 --- a/python/gtsam/gtsam.tpl +++ b/python/gtsam/gtsam.tpl @@ -13,6 +13,7 @@ #include #include #include +#include #include #include #include "gtsam/config.h" From a83721380fb17a11dd0389b17931033d66fa9b24 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 4 May 2021 12:28:38 -0400 Subject: [PATCH 13/13] update boost download link --- .github/scripts/boost.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/scripts/boost.sh b/.github/scripts/boost.sh index 8b36a8f21..647a84628 100644 --- a/.github/scripts/boost.sh +++ b/.github/scripts/boost.sh @@ -2,7 +2,7 @@ BOOST_FOLDER=boost_${BOOST_VERSION//./_} # Download Boost -wget https://dl.bintray.com/boostorg/release/${BOOST_VERSION}/source/${BOOST_FOLDER}.tar.gz +wget https://boostorg.jfrog.io/artifactory/main/release/${BOOST_VERSION}/source/${BOOST_FOLDER}.tar.gz # Unzip tar -zxf ${BOOST_FOLDER}.tar.gz