Merge branch 'develop' into feature/essential-mat-with-approx-k

.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

gtsam/discrete/DiscreteBayesNet.h

@@ -55,6 +55,9 @@ namespace gtsam {
     template<class DERIVEDCONDITIONAL>
     DiscreteBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph) : Base(graph) {}
 
+    /// Destructor
+    virtual ~DiscreteBayesNet() {}
+
     /// @}
 
     /// @name Testable

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
+  void print(
+      const std::string& s = "DiscreteFactor\n",
+      const KeyFormatter& formatter = DefaultKeyFormatter) const override {
+    Base::print(s, formatter);
   }
 
   /** Test whether the factor is empty */

gtsam/discrete/DiscreteFactorGraph.h

@@ -91,6 +91,9 @@ public:
   template<class DERIVEDFACTOR>
   DiscreteFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
 
+  /// Destructor
+  virtual ~DiscreteFactorGraph() {}
+
   /// @name Testable
   /// @{
 
@@ -129,8 +132,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,

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
@@ -144,7 +142,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 +322,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;

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");
   }

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;

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));

gtsam/gtsam.i

@@ -10,8 +10,11 @@
 
 namespace gtsam {
 
-// Actually a FastList<Key>
 #include <gtsam/inference/Key.h>
+
+const KeyFormatter DefaultKeyFormatter;
+
+// Actually a FastList<Key>
 class KeyList {
   KeyList();
   KeyList(const gtsam::KeyList& other);
@@ -47,7 +50,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 +224,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 +248,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 +301,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 +345,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 +382,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
@@ -430,7 +433,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 +463,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 +493,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 +554,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 +611,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 +671,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 +747,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 +777,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 +802,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
@@ -836,7 +839,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 +925,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 +946,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 +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
@@ -1022,7 +1025,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
@@ -1097,7 +1100,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 +1163,9 @@ virtual class SymbolicFactor {
 
   // From Factor
   size_t size() const;
-  void print(string s="") 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();
 };
@@ -1173,7 +1178,9 @@ virtual class SymbolicFactorGraph {
 
   // From FactorGraph
   void push_back(gtsam::SymbolicFactor* factor);
-  void print(string s="") 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;
@@ -1223,7 +1230,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 = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
   bool equals(const gtsam::SymbolicConditional& other, double tol) const;
 
   // Standard interface
@@ -1236,7 +1244,9 @@ class SymbolicBayesNet {
   SymbolicBayesNet();
   SymbolicBayesNet(const gtsam::SymbolicBayesNet& other);
   // Testable
-  void print(string s="") const;
+  void print(string s = "SymbolicBayesNet",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
   bool equals(const gtsam::SymbolicBayesNet& other, double tol) const;
 
   // Standard interface
@@ -1257,7 +1267,8 @@ class SymbolicBayesTree {
     SymbolicBayesTree(const gtsam::SymbolicBayesTree& other);
 
     // Testable
-    void print(string s="");
+    void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                  gtsam::DefaultKeyFormatter);
     bool equals(const gtsam::SymbolicBayesTree& other, double tol) const;
 
     //Standard Interface
@@ -1273,30 +1284,26 @@ class SymbolicBayesTree {
   gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const;
 };
 
-// class SymbolicBayesTreeClique {
-//   SymbolicBayesTreeClique();
-//   SymbolicBayesTreeClique(CONDITIONAL* conditional);
-//   SymbolicBayesTreeClique(const pair<typename ConditionalType::shared_ptr, typename ConditionalType::FactorType::shared_ptr>& result) : Base(result) {}
-//
-//   bool equals(const This& other, double tol) const;
-//   void print(string s="") const;
-//   void printTree() const; // Default indent of ""
-//   void printTree(string indent) const;
-//   size_t numCachedSeparatorMarginals() const;
-//
-//   CONDITIONAL* conditional() const;
-//   bool isRoot() const;
-//   size_t treeSize() const;
-//  const std::list<derived_ptr>& children() const { return children_; }
-//  derived_ptr parent() const { return parent_.lock(); }
-//
+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<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;
 //  FactorGraph<FactorType> marginal(derived_ptr root, Eliminate function) const;
 //  FactorGraph<FactorType> joint(derived_ptr C2, derived_ptr root, Eliminate function) const;
 //
-//   void deleteCachedShortcuts();
-// };
+  void deleteCachedShortcuts();
+};
 
 #include <gtsam/inference/VariableIndex.h>
 class VariableIndex {
@@ -1313,7 +1320,9 @@ class VariableIndex {
 
   // Testable
   bool equals(const gtsam::VariableIndex& other, double tol) const;
-  void print(string s="") const;
+  void print(string s = "VariableIndex: ",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
 
   // Standard interface
   size_t size() const;
@@ -1328,7 +1337,7 @@ class VariableIndex {
 namespace noiseModel {
 #include <gtsam/linear/NoiseModel.h>
 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 +1420,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 {
@@ -1540,7 +1549,7 @@ class Sampler {
 
 #include <gtsam/linear/VectorValues.h>
 class VectorValues {
-    //Constructors
+  //Constructors
   VectorValues();
   VectorValues(const gtsam::VectorValues& other);
 
@@ -1551,7 +1560,9 @@ 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 = "VectorValues",
+             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;
@@ -1582,7 +1593,8 @@ class VectorValues {
 #include <gtsam/linear/GaussianFactor.h>
 virtual class GaussianFactor {
   gtsam::KeyVector keys() const;
-  void print(string s="") 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;
@@ -1610,7 +1622,8 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
   JacobianFactor(const gtsam::GaussianFactorGraph& graph);
 
   //Testable
-  void print(string s="") 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;
@@ -1659,7 +1672,8 @@ virtual class HessianFactor : gtsam::GaussianFactor {
 
   //Testable
   size_t size() const;
-  void print(string s="") 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;
@@ -1684,7 +1698,8 @@ class GaussianFactorGraph {
   GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree);
 
   // From FactorGraph
-  void print(string s="") 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;
@@ -1775,20 +1790,23 @@ virtual class GaussianConditional : gtsam::JacobianFactor {
         size_t name2, Matrix T);
 
   //Standard Interface
-  void print(string s="") 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 <gtsam/linear/GaussianDensity.h>
@@ -1797,7 +1815,9 @@ 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 = "GaussianDensity",
+             const gtsam::KeyFormatter& keyFormatter =
+                 gtsam::DefaultKeyFormatter) const;
   bool equals(const gtsam::GaussianDensity &cg, double tol) const;
   Vector mean() const;
   Matrix covariance() const;
@@ -1810,7 +1830,8 @@ virtual class GaussianBayesNet {
   GaussianBayesNet(const gtsam::GaussianConditional* conditional);
 
   // Testable
-  void print(string s="") 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;
 
@@ -1845,7 +1866,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 = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter);
   size_t size() const;
   bool empty() const;
   size_t numCachedSeparatorMarginals() const;
@@ -1871,7 +1893,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 +1912,6 @@ class GaussianISAM {
 virtual class IterativeOptimizationParameters {
   string getVerbosity() const;
   void setVerbosity(string s) ;
-  void print() const;
 };
 
 //virtual class IterativeSolver {
@@ -1912,7 +1933,6 @@ virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParamete
   void setReset(int value);
   void setEpsilon_rel(double value);
   void setEpsilon_abs(double value);
-  void print() const;
 };
 
 #include <gtsam/linear/Preconditioner.h>
@@ -1927,14 +1947,13 @@ virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters {
 #include <gtsam/linear/PCGSolver.h>
 virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
   PCGSolverParameters();
-  void print(string s="");
+  void print(string s = "");
   void setPreconditionerParams(gtsam::PreconditionerParameters* preconditioner);
 };
 
 #include <gtsam/linear/SubgraphSolver.h>
 virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
   SubgraphSolverParameters();
-  void print() const;
 };
 
 virtual class SubgraphSolver  {
@@ -1948,7 +1967,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 +1993,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 +2058,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 +2073,8 @@ class Ordering {
   Ordering(const gtsam::Ordering& other);
 
   // Testable
-  void print(string s="") const;
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
   bool equals(const gtsam::Ordering& ord, double tol) const;
 
   // Standard interface
@@ -2075,7 +2095,9 @@ class NonlinearFactorGraph {
   NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph);
 
   // FactorGraph
-  void print(string s="") 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;
@@ -2123,7 +2145,8 @@ virtual class NonlinearFactor {
   // Factor base class
   size_t size() const;
   gtsam::KeyVector keys() const;
-  void print(string s="") 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;
@@ -2132,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 <gtsam/nonlinear/NonlinearFactor.h>
@@ -2153,7 +2176,8 @@ class Values {
   void clear();
   size_t dim() const;
 
-  void print(string s="") 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);
@@ -2242,7 +2266,8 @@ class Marginals {
   Marginals(const gtsam::GaussianFactorGraph& gfgraph,
       const gtsam::VectorValues& solutionvec);
 
-  void print(string s="") 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;
@@ -2252,8 +2277,7 @@ 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 <gtsam/nonlinear/LinearContainerFactor.h>
@@ -2296,7 +2320,7 @@ virtual class LinearContainerFactor : gtsam::NonlinearFactor {
 #include <gtsam/nonlinear/NonlinearOptimizerParams.h>
 virtual class NonlinearOptimizerParams {
   NonlinearOptimizerParams();
-  void print(string s="") const;
+  void print(string s = "") const;
 
   int getMaxIterations() const;
   double getRelativeErrorTol() const;
@@ -2407,14 +2431,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 <gtsam/nonlinear/ISAM2.h>
 class ISAM2GaussNewtonParams {
   ISAM2GaussNewtonParams();
 
-  void print(string str) const;
+  void print(string s = "") const;
 
   /** Getters and Setters for all properties */
   double getWildfireThreshold() const;
@@ -2424,7 +2448,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 +2484,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 +2514,13 @@ 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 +2536,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;
 
@@ -2544,7 +2569,8 @@ class ISAM2 {
 class NonlinearISAM {
   NonlinearISAM();
   NonlinearISAM(int reorderInterval);
-  void print(string s="") const;
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
   void printStats() const;
   void saveGraph(string s) const;
   gtsam::Values estimate() const;
@@ -2679,7 +2705,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<gtsam::Pose2, gtsam::Point2, gtsam::Rot2, double> BearingRange2D;
@@ -2747,11 +2773,17 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
 };
 
 #include <gtsam/slam/SmartProjectionFactor.h>
+
+/// 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);
@@ -3016,7 +3048,6 @@ class ShonanAveragingParameters2 {
   bool getUseHuber() const;
   void setCertifyOptimality(bool value);
   bool getCertifyOptimality() const;
-  void print() const;
 };
 
 class ShonanAveragingParameters3 {
@@ -3037,7 +3068,6 @@ class ShonanAveragingParameters3 {
   bool getUseHuber() const;
   void setCertifyOptimality(bool value);
   bool getCertifyOptimality() const;
-  void print() const;
 };
 
 class ShonanAveraging2 {
@@ -3167,7 +3197,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
@@ -3207,7 +3237,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
@@ -3225,7 +3255,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);
@@ -3248,7 +3278,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);
@@ -3268,7 +3298,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
@@ -3311,7 +3341,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);
@@ -3330,7 +3360,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);
 
@@ -3371,7 +3401,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
@@ -3410,7 +3440,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 = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
   bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
   gtsam::Unit3 nZ() const;
   gtsam::Unit3 bRef() const;
@@ -3423,7 +3454,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 = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
   bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
   gtsam::Unit3 nZ() const;
   gtsam::Unit3 bRef() const;
@@ -3435,7 +3467,8 @@ virtual class GPSFactor : gtsam::NonlinearFactor{
             const gtsam::noiseModel::Base* model);
 
   // Testable
-  void print(string s="") const;
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
   bool equals(const gtsam::GPSFactor& expected, double tol);
 
   // Standard Interface
@@ -3447,7 +3480,8 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor {
             const gtsam::noiseModel::Base* model);
 
   // Testable
-  void print(string s="") const;
+  void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
+                                gtsam::DefaultKeyFormatter) const;
   bool equals(const gtsam::GPSFactor2& expected, double tol);
 
   // Standard Interface

gtsam/inference/BayesNet-inst.h

@@ -26,30 +26,30 @@
 
 namespace gtsam {
 
-  /* ************************************************************************* */
-  template<class CONDITIONAL>
-  void BayesNet<CONDITIONAL>::print(const std::string& s, const KeyFormatter& formatter) const
-  {
-    Base::print(s, formatter);
-  }
-
-  /* ************************************************************************* */
-  template<class CONDITIONAL>
-  void BayesNet<CONDITIONAL>::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 <class CONDITIONAL>
+void BayesNet<CONDITIONAL>::print(
+    const std::string& s, const KeyFormatter& formatter) const {
+  Base::print(s, formatter);
 }
+
+/* ************************************************************************* */
+template <class CONDITIONAL>
+void BayesNet<CONDITIONAL>::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

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;
   };
 
 }

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;
   }
 

gtsam/inference/Factor.h

@@ -102,43 +102,52 @@ typedef FastSet<FactorIndex> 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
-    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;
+   /// print only keys
+   virtual void printKeys(
+       const std::string& s = "Factor",
+       const KeyFormatter& formatter = DefaultKeyFormatter) const;
 
   protected:
     /// check equality

gtsam/inference/FactorGraph-inst.h

@@ -37,7 +37,7 @@ namespace gtsam {
 template <class FACTOR>
 void FactorGraph<FACTOR>::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;

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
   /// @{
 
@@ -285,9 +289,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;
@@ -355,7 +359,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; }

gtsam/linear/GaussianBayesNet.h

@@ -55,6 +55,9 @@ namespace gtsam {
     template<class DERIVEDCONDITIONAL>
     GaussianBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph) : Base(graph) {}
 
+    /// Destructor
+    virtual ~GaussianBayesNet() {}
+
     /// @}
 
     /// @name Testable
@@ -177,6 +180,13 @@ namespace gtsam {
      */
     VectorValues backSubstituteTranspose(const VectorValues& gx) const;
 
+    /// print graph
+    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.

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
+    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;

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);

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: ");

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;

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: ");
 }

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;

gtsam/nonlinear/NonlinearFactor.h

@@ -70,8 +70,8 @@ public:
   /// @{
 
   /** print */
-  virtual void print(const std::string& s = "",
-    const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+  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;

gtsam/nonlinear/NonlinearFactorGraph.h

@@ -98,9 +98,13 @@ namespace gtsam {
     template<class DERIVEDFACTOR>
     NonlinearFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
 
+    /// Destructor
+    virtual ~NonlinearFactorGraph() {}
+
     /** 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: ",

gtsam/sfm/BinaryMeasurement.h

@@ -52,6 +52,9 @@ private:
         measured_(measured),
         noiseModel_(model) {}
 
+  /// Destructor
+  virtual ~BinaryMeasurement() {}
+
   /// @name Standard Interface
   /// @{
 
@@ -64,8 +67,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<T>::Print(measured_, "  measured: ");

gtsam/symbolic/SymbolicBayesNet.h

@@ -55,6 +55,9 @@ namespace gtsam {
     template<class DERIVEDCONDITIONAL>
     SymbolicBayesNet(const FactorGraph<DERIVEDCONDITIONAL>& graph) : Base(graph) {}
 
+    /// Destructor
+    virtual ~SymbolicBayesNet() {}
+
     /// @}
 
     /// @name Testable
@@ -63,6 +66,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

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

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;
+    void print(
+        const std::string& str = "",
+        const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
     /** Check equality */
     bool equals(const This& c, double tol = 1e-9) const;

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

gtsam/symbolic/SymbolicFactorGraph.h

@@ -81,6 +81,9 @@ namespace gtsam {
     template<class DERIVEDFACTOR>
     SymbolicFactorGraph(const FactorGraph<DERIVEDFACTOR>& graph) : Base(graph) {}
 
+    /// Destructor
+    virtual ~SymbolicFactorGraph() {}
+
     /// @}
 
     /// @name Testable
@@ -88,6 +91,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

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 {

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;

gtsam_unstable/linear/InequalityFactorGraph.h

@@ -37,8 +37,9 @@ public:
   typedef boost::shared_ptr<InequalityFactorGraph> 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);
   }
 

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;

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;

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()) << ","

python/gtsam/gtsam.tpl

@@ -13,6 +13,8 @@
 #include <pybind11/eigen.h>
 #include <pybind11/stl_bind.h>
 #include <pybind11/pybind11.h>
+#include <pybind11/operators.h>
+#include <pybind11/functional.h>
 #include <pybind11/iostream.h>
 #include "gtsam/config.h"
 #include "gtsam/base/serialization.h"

python/gtsam_unstable/gtsam_unstable.tpl

@@ -13,6 +13,7 @@
 #include <pybind11/eigen.h>
 #include <pybind11/stl_bind.h>
 #include <pybind11/pybind11.h>
+#include <pybind11/functional.h>
 #include <pybind11/iostream.h>
 #include "gtsam/base/serialization.h"
 #include "gtsam/nonlinear/utilities.h"  // for RedirectCout.