Merge branch 'develop' into dellaert/issue420

.travis.yml

@@ -1,6 +1,5 @@
 language: cpp
 cache: ccache
-sudo: required
 dist: xenial
 
 addons:
@@ -33,7 +32,7 @@ stages:
 
 env:
   global:
-    - MAKEFLAGS="-j2"
+    - MAKEFLAGS="-j3"
     - CCACHE_SLOPPINESS=pch_defines,time_macros
 
 # Compile stage without building examples/tests to populate the caches.

CppUnitLite/Test.h

@@ -64,7 +64,7 @@ protected:
   class testGroup##testName##Test : public Test \
   { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, true) {} \
             virtual ~testGroup##testName##Test () {};\
-            void run (TestResult& result_);} \
+            void run (TestResult& result_) override;} \
     testGroup##testName##Instance; \
   void testGroup##testName##Test::run (TestResult& result_)
 
@@ -82,7 +82,7 @@ protected:
   class testGroup##testName##Test : public Test \
   { public: testGroup##testName##Test () : Test (#testName "Test", __FILE__, __LINE__, false) {} \
             virtual ~testGroup##testName##Test () {};\
-            void run (TestResult& result_);} \
+            void run (TestResult& result_) override;} \
     testGroup##testName##Instance; \
   void testGroup##testName##Test::run (TestResult& result_)
 

cmake/GtsamBuildTypes.cmake

@@ -104,8 +104,24 @@ if(MSVC)
   set(GTSAM_COMPILE_OPTIONS_PRIVATE_TIMING          /MD /O2  CACHE STRING "(User editable) Private compiler flags for Timing configuration.")
 else()
   # Common to all configurations, next for each configuration:
-  # "-fPIC" is to ensure proper code generation for shared libraries
+
-  set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON          -Wall -fPIC CACHE STRING "(User editable) Private compiler flags for all configurations.")
+  if (
+      ((CMAKE_CXX_COMPILER_ID MATCHES "Clang") AND (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 12.0.0)) OR
+      (CMAKE_CXX_COMPILER_ID MATCHES "GNU")
+     )
+    set(flag_override_ -Wsuggest-override) # -Werror=suggest-override: Add again someday
+  endif()
+
+  set(GTSAM_COMPILE_OPTIONS_PRIVATE_COMMON
+    -Wall                                          # Enable common warnings
+    -fPIC                                          # ensure proper code generation for shared libraries
+    $<$<CXX_COMPILER_ID:GNU>:-Wreturn-local-addr -Werror=return-local-addr>            # Error: return local address
+    $<$<CXX_COMPILER_ID:Clang>:-Wreturn-stack-address   -Werror=return-stack-address>  # Error: return local address
+    -Wreturn-type  -Werror=return-type             # Error on missing return()
+    -Wformat -Werror=format-security               # Error on wrong printf() arguments
+    $<$<COMPILE_LANGUAGE:CXX>:${flag_override_}>   # Enforce the use of the override keyword
+    #
+    CACHE STRING "(User editable) Private compiler flags for all configurations.")
   set(GTSAM_COMPILE_OPTIONS_PRIVATE_DEBUG           -g -fno-inline  CACHE STRING "(User editable) Private compiler flags for Debug configuration.")
   set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELWITHDEBINFO  -g -O3  CACHE STRING "(User editable) Private compiler flags for RelWithDebInfo configuration.")
   set(GTSAM_COMPILE_OPTIONS_PRIVATE_RELEASE         -O3  CACHE STRING "(User editable) Private compiler flags for Release configuration.")

cython/gtsam/utils/plot.py

@@ -135,7 +135,8 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
         axes.add_patch(e1)
 
 
-def plot_pose2(fignum, pose, axis_length=0.1, covariance=None):
+def plot_pose2(fignum, pose, axis_length=0.1, covariance=None,
+               axis_labels=('X axis', 'Y axis', 'Z axis')):
     """
     Plot a 2D pose on given figure with given `axis_length`.
 
@@ -144,6 +145,7 @@ def plot_pose2(fignum, pose, axis_length=0.1, covariance=None):
         pose (gtsam.Pose2): The pose to be plotted.
         axis_length (float): The length of the camera axes.
         covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
+        axis_labels (iterable[string]): List of axis labels to set.
     """
     # get figure object
     fig = plt.figure(fignum)
@@ -151,6 +153,12 @@ def plot_pose2(fignum, pose, axis_length=0.1, covariance=None):
     plot_pose2_on_axes(axes, pose, axis_length=axis_length,
                        covariance=covariance)
 
+    axes.set_xlabel(axis_labels[0])
+    axes.set_ylabel(axis_labels[1])
+    axes.set_zlabel(axis_labels[2])
+
+    return fig
+
 
 def plot_point3_on_axes(axes, point, linespec, P=None):
     """
@@ -167,7 +175,8 @@ def plot_point3_on_axes(axes, point, linespec, P=None):
         plot_covariance_ellipse_3d(axes, point.vector(), P)
 
 
-def plot_point3(fignum, point, linespec, P=None):
+def plot_point3(fignum, point, linespec, P=None,
+                axis_labels=('X axis', 'Y axis', 'Z axis')):
     """
     Plot a 3D point on given figure with given `linespec`.
 
@@ -176,13 +185,25 @@ def plot_point3(fignum, point, linespec, P=None):
         point (gtsam.Point3): The point to be plotted.
         linespec (string): String representing formatting options for Matplotlib.
         P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
+        axis_labels (iterable[string]): List of axis labels to set.
+
+    Returns:
+        fig: The matplotlib figure.
+
     """
     fig = plt.figure(fignum)
     axes = fig.gca(projection='3d')
     plot_point3_on_axes(axes, point, linespec, P)
 
+    axes.set_xlabel(axis_labels[0])
+    axes.set_ylabel(axis_labels[1])
+    axes.set_zlabel(axis_labels[2])
 
-def plot_3d_points(fignum, values, linespec="g*", marginals=None):
+    return fig
+
+
+def plot_3d_points(fignum, values, linespec="g*", marginals=None,
+                   title="3D Points", axis_labels=('X axis', 'Y axis', 'Z axis')):
     """
     Plots the Point3s in `values`, with optional covariances.
     Finds all the Point3 objects in the given Values object and plots them.
@@ -193,7 +214,9 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None):
         fignum (int): Integer representing the figure number to use for plotting.
         values (gtsam.Values): Values dictionary consisting of points to be plotted.
         linespec (string): String representing formatting options for Matplotlib.
-        covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
+        marginals (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
+        title (string): The title of the plot.
+        axis_labels (iterable[string]): List of axis labels to set.
     """
 
     keys = values.keys()
@@ -208,12 +231,16 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None):
             else:
                 covariance = None
 
-            plot_point3(fignum, point, linespec, covariance)
+            fig = plot_point3(fignum, point, linespec, covariance,
+                              axis_labels=axis_labels)
 
         except RuntimeError:
             continue
             # I guess it's not a Point3
 
+    fig.suptitle(title)
+    fig.canvas.set_window_title(title.lower())
+
 
 def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
     """
@@ -251,7 +278,8 @@ def plot_pose3_on_axes(axes, pose, axis_length=0.1, P=None, scale=1):
         plot_covariance_ellipse_3d(axes, origin, gPp)
 
 
-def plot_pose3(fignum, pose, axis_length=0.1, P=None):
+def plot_pose3(fignum, pose, axis_length=0.1, P=None,
+               axis_labels=('X axis', 'Y axis', 'Z axis')):
     """
     Plot a 3D pose on given figure with given `axis_length`.
 
@@ -260,6 +288,10 @@ def plot_pose3(fignum, pose, axis_length=0.1, P=None):
         pose (gtsam.Pose3): 3D pose to be plotted.
         linespec (string): String representing formatting options for Matplotlib.
         P (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
+        axis_labels (iterable[string]): List of axis labels to set.
+
+    Returns:
+        fig: The matplotlib figure.
     """
     # get figure object
     fig = plt.figure(fignum)
@@ -267,8 +299,15 @@ def plot_pose3(fignum, pose, axis_length=0.1, P=None):
     plot_pose3_on_axes(axes, pose, P=P,
                        axis_length=axis_length)
 
+    axes.set_xlabel(axis_labels[0])
+    axes.set_ylabel(axis_labels[1])
+    axes.set_zlabel(axis_labels[2])
 
-def plot_trajectory(fignum, values, scale=1, marginals=None):
+    return fig
+
+
+def plot_trajectory(fignum, values, scale=1, marginals=None,
+                    title="Plot Trajectory", axis_labels=('X axis', 'Y axis', 'Z axis')):
     """
     Plot a complete 3D trajectory using poses in `values`.
 
@@ -278,6 +317,8 @@ def plot_trajectory(fignum, values, scale=1, marginals=None):
         scale (float): Value to scale the poses by.
         marginals (gtsam.Marginals): Marginalized probability values of the estimation.
             Used to plot uncertainty bounds.
+        title (string): The title of the plot.
+        axis_labels (iterable[string]): List of axis labels to set.
     """
     pose3Values = gtsam.utilities_allPose3s(values)
     keys = gtsam.KeyVector(pose3Values.keys())
@@ -303,8 +344,8 @@ def plot_trajectory(fignum, values, scale=1, marginals=None):
             else:
                 covariance = None
 
-            plot_pose3(fignum, lastPose,  P=covariance,
+            fig = plot_pose3(fignum, lastPose,  P=covariance,
-                       axis_length=scale)
+                             axis_length=scale, axis_labels=axis_labels)
 
         lastIndex = i
 
@@ -318,8 +359,11 @@ def plot_trajectory(fignum, values, scale=1, marginals=None):
             else:
                 covariance = None
 
-            plot_pose3(fignum, lastPose, P=covariance,
+            fig = plot_pose3(fignum, lastPose, P=covariance,
-                       axis_length=scale)
+                             axis_length=scale, axis_labels=axis_labels)
 
         except:
             pass
+
+    fig.suptitle(title)
+    fig.canvas.set_window_title(title.lower())

examples/CameraResectioning.cpp

@@ -46,8 +46,8 @@ public:
   }
 
   /// evaluate the error
-  virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
+  Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
-      boost::none) const {
+      boost::none) const override {
     PinholeCamera<Cal3_S2> camera(pose, *K_);
     return camera.project(P_, H, boost::none, boost::none) - p_;
   }

examples/Data/example_with_vertices.g2o

@@ -0,0 +1,16 @@
+VERTEX_SE3:QUAT 8646911284551352320 40 -1.15443e-13 10 0.557345 0.557345 -0.435162 -0.435162
+VERTEX_SE3:QUAT 8646911284551352321 28.2843 28.2843 10 0.301633 0.728207 -0.568567 -0.235508
+VERTEX_SE3:QUAT 8646911284551352322 -1.6986e-08 40 10 -3.89609e-10 0.788205 -0.615412 -2.07622e-10
+VERTEX_SE3:QUAT 8646911284551352323 -28.2843 28.2843 10 -0.301633 0.728207 -0.568567 0.235508
+VERTEX_SE3:QUAT 8646911284551352324 -40 -2.32554e-10 10 -0.557345 0.557345 -0.435162 0.435162
+VERTEX_SE3:QUAT 8646911284551352325 -28.2843 -28.2843 10 -0.728207 0.301633 -0.235508 0.568567
+VERTEX_SE3:QUAT 8646911284551352326 -2.53531e-09 -40 10 -0.788205 -1.25891e-11 -3.82742e-13 0.615412
+VERTEX_SE3:QUAT 8646911284551352327 28.2843 -28.2843 10 -0.728207 -0.301633 0.235508 0.568567
+VERTEX_TRACKXYZ 7782220156096217088 10 10 10
+VERTEX_TRACKXYZ 7782220156096217089 -10 10 10
+VERTEX_TRACKXYZ 7782220156096217090 -10 -10 10
+VERTEX_TRACKXYZ 7782220156096217091 10 -10 10
+VERTEX_TRACKXYZ 7782220156096217092 10 10 -10
+VERTEX_TRACKXYZ 7782220156096217093 -10 10 -10
+VERTEX_TRACKXYZ 7782220156096217094 -10 -10 -10
+VERTEX_TRACKXYZ 7782220156096217095 10 -10 -10

examples/LocalizationExample.cpp

@@ -85,7 +85,7 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> {
   // function, returning a vector of errors when evaluated at the provided variable value. It
   // must also calculate the Jacobians for this measurement function, if requested.
   Vector evaluateError(const Pose2& q,
-                       boost::optional<Matrix&> H = boost::none) const {
+                       boost::optional<Matrix&> H = boost::none) const override {
     // The measurement function for a GPS-like measurement is simple:
     // error_x = pose.x - measurement.x
     // error_y = pose.y - measurement.y
@@ -99,7 +99,7 @@ class UnaryFactor: public NoiseModelFactor1<Pose2> {
   // The second is a 'clone' function that allows the factor to be copied. Under most
   // circumstances, the following code that employs the default copy constructor should
   // work fine.
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new UnaryFactor(*this))); }
 

examples/SolverComparer.cpp

@@ -50,11 +50,11 @@
 #include <boost/program_options.hpp>
 #include <boost/range/algorithm/set_algorithm.hpp>
 #include <boost/range/adaptor/reversed.hpp>
-#include <boost/random.hpp>
 #include <boost/serialization/export.hpp>
 
 #include <fstream>
 #include <iostream>
+#include <random>
 
 #ifdef GTSAM_USE_TBB
 #include <tbb/task_arena.h> // tbb::task_arena
@@ -554,8 +554,8 @@ void runCompare()
 void runPerturb()
 {
   // Set up random number generator
-  boost::mt19937 rng;
+  std::mt19937 rng;
-  boost::normal_distribution<double> normal(0.0, perturbationNoise);
+  std::normal_distribution<double> normal(0.0, perturbationNoise);
 
   // Perturb values
   VectorValues noise;

gtsam.h

@@ -2996,6 +2996,7 @@ class PreintegratedImuMeasurements {
   void resetIntegrationAndSetBias(const gtsam::imuBias::ConstantBias& biasHat);
 
   Matrix preintMeasCov() const;
+  Vector preintegrated() const;
   double deltaTij() const;
   gtsam::Rot3 deltaRij() const;
   Vector deltaPij() const;

gtsam/CMakeLists.txt

@@ -14,7 +14,6 @@ set (gtsam_subdirs
     sam
     sfm
     slam
-    smart
 	  navigation
 )
 

gtsam/base/GenericValue.h

@@ -70,7 +70,7 @@ public:
   }
 
   /// equals implementing generic Value interface
-  virtual bool equals_(const Value& p, double tol = 1e-9) const {
+  bool equals_(const Value& p, double tol = 1e-9) const override {
     // Cast the base class Value pointer to a templated generic class pointer
     const GenericValue& genericValue2 = static_cast<const GenericValue&>(p);
     // Return the result of using the equals traits for the derived class
@@ -83,7 +83,7 @@ public:
   }
 
   /// Virtual print function, uses traits
-  virtual void print(const std::string& str) const {
+  void print(const std::string& str) const override {
     std::cout << "(" << demangle(typeid(T).name()) << ") ";
     traits<T>::Print(value_, str);
   }
@@ -91,7 +91,7 @@ public:
     /**
      * Create a duplicate object returned as a pointer to the generic Value interface.
      */
-    virtual Value* clone_() const {
+    Value* clone_() const override {
       GenericValue* ptr = new GenericValue(*this); // calls copy constructor to fill in
       return ptr;
     }
@@ -99,19 +99,19 @@ public:
     /**
      * Destroy and deallocate this object, only if it was originally allocated using clone_().
      */
-    virtual void deallocate_() const {
+    void deallocate_() const override {
       delete this;
     }
 
     /**
      * Clone this value (normal clone on the heap, delete with 'delete' operator)
      */
-    virtual boost::shared_ptr<Value> clone() const {
+    boost::shared_ptr<Value> clone() const override {
 		return boost::allocate_shared<GenericValue>(Eigen::aligned_allocator<GenericValue>(), *this);
     }
 
     /// Generic Value interface version of retract
-    virtual Value* retract_(const Vector& delta) const {
+    Value* retract_(const Vector& delta) const override {
       // Call retract on the derived class using the retract trait function
       const T retractResult = traits<T>::Retract(GenericValue<T>::value(), delta);
 
@@ -122,7 +122,7 @@ public:
     }
 
     /// Generic Value interface version of localCoordinates
-    virtual Vector localCoordinates_(const Value& value2) const {
+    Vector localCoordinates_(const Value& value2) const override {
       // Cast the base class Value pointer to a templated generic class pointer
       const GenericValue<T>& genericValue2 =
           static_cast<const GenericValue<T>&>(value2);
@@ -142,12 +142,12 @@ public:
     }
 
     /// Return run-time dimensionality
-    virtual size_t dim() const {
+    size_t dim() const override {
       return traits<T>::GetDimension(value_);
     }
 
     /// Assignment operator
-    virtual Value& operator=(const Value& rhs) {
+    Value& operator=(const Value& rhs) override {
       // Cast the base class Value pointer to a derived class pointer
       const GenericValue& derivedRhs = static_cast<const GenericValue&>(rhs);
 
@@ -198,4 +198,12 @@ const ValueType& Value::cast() const {
   return dynamic_cast<const GenericValue<ValueType>&>(*this).value();
 }
 
+/** Functional constructor of GenericValue<T> so T can be automatically deduced
+  */
+template<class T>
+GenericValue<T> genericValue(const T& v) {
+  return GenericValue<T>(v);
+}
+
+
 } /* namespace gtsam */

gtsam/base/Matrix.cpp

@@ -136,20 +136,24 @@ Vector operator^(const Matrix& A, const Vector & v) {
   return A.transpose() * v;
 }
 
+const Eigen::IOFormat& matlabFormat() {
+  static const Eigen::IOFormat matlab(
+    Eigen::StreamPrecision, // precision
+    Eigen::DontAlignCols, // flags set such that rowSpacers are not added
+    ", ", // coeffSeparator
+    ";\n", // rowSeparator
+    "\t",  // rowPrefix
+    "", // rowSuffix
+    "[\n", // matPrefix
+    "\n]" // matSuffix
+  );
+  return matlab;
+}
+
 /* ************************************************************************* */
 //3 argument call
 void print(const Matrix& A, const string &s, ostream& stream) {
-  static const Eigen::IOFormat matlab(
+  cout << s << A.format(matlabFormat()) << endl;
-      Eigen::StreamPrecision, // precision
-      0, // flags
-      ", ", // coeffSeparator
-      ";\n", // rowSeparator
-      "\t",  // rowPrefix
-      "", // rowSuffix
-      "[\n", // matPrefix
-      "\n]" // matSuffix
-      );
-  cout << s << A.format(matlab) << endl;
 }
 
 /* ************************************************************************* */

gtsam/base/Matrix.h

@@ -73,6 +73,10 @@ GTSAM_MAKE_MATRIX_DEFS(9);
 typedef Eigen::Block<Matrix> SubMatrix;
 typedef Eigen::Block<const Matrix> ConstSubMatrix;
 
+// Matrix formatting arguments when printing.
+// Akin to Matlab style.
+const Eigen::IOFormat& matlabFormat();
+
 /**
  * equals with a tolerance
  */

gtsam/base/ThreadsafeException.h

@@ -71,12 +71,12 @@ protected:
           String(description.begin(), description.end())) {
   }
 
-  /// Default destructor doesn't have the throw()
+  /// Default destructor doesn't have the noexcept
-  virtual ~ThreadsafeException() throw () {
+  virtual ~ThreadsafeException() noexcept {
   }
 
 public:
-  virtual const char* what() const throw () {
+  const char* what() const noexcept override {
     return description_ ? description_->c_str() : "";
   }
 };
@@ -113,8 +113,8 @@ public:
 class CholeskyFailed : public gtsam::ThreadsafeException<CholeskyFailed>
 {
 public:
-  CholeskyFailed() throw() {}
+  CholeskyFailed() noexcept {}
-  virtual ~CholeskyFailed() throw() {}
+  virtual ~CholeskyFailed() noexcept {}
 };
 
 } // namespace gtsam

gtsam/base/timing.cpp

@@ -89,7 +89,7 @@ void TimingOutline::print(const std::string& outline) const {
     childOrder[child.second->myOrder_] = child.second;
   }
   // Print children
-  for(const ChildOrder::value_type order_child: childOrder) {
+  for(const ChildOrder::value_type& order_child: childOrder) {
     std::string childOutline(outline);
     childOutline += "|   ";
     order_child.second->print(childOutline);

gtsam/base/treeTraversal/parallelTraversalTasks.h

@@ -57,7 +57,7 @@ namespace gtsam {
               makeNewTasks(makeNewTasks),
               isPostOrderPhase(false) {}
 
-        tbb::task* execute()
+        tbb::task* execute() override
         {
           if(isPostOrderPhase)
           {
@@ -144,7 +144,7 @@ namespace gtsam {
           roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost),
           problemSizeThreshold(problemSizeThreshold) {}
 
-        tbb::task* execute()
+        tbb::task* execute() override
         {
           typedef PreOrderTask<NODE, DATA, VISITOR_PRE, VISITOR_POST> PreOrderTask;
           // Create data and tasks for our children

gtsam/discrete/DecisionTree-inl.h

@@ -66,42 +66,42 @@ namespace gtsam {
     }
 
     /// Leaf-Leaf equality
-    bool sameLeaf(const Leaf& q) const {
+    bool sameLeaf(const Leaf& q) const override {
       return constant_ == q.constant_;
     }
 
     /// polymorphic equality: is q is a leaf, could be
-    bool sameLeaf(const Node& q) const {
+    bool sameLeaf(const Node& q) const override {
       return (q.isLeaf() && q.sameLeaf(*this));
     }
 
     /** equality up to tolerance */
-    bool equals(const Node& q, double tol) const {
+    bool equals(const Node& q, double tol) const override {
       const Leaf* other = dynamic_cast<const Leaf*> (&q);
       if (!other) return false;
       return std::abs(double(this->constant_ - other->constant_)) < tol;
     }
 
     /** print */
-    void print(const std::string& s) const {
+    void print(const std::string& s) const override {
       bool showZero = true;
       if (showZero || constant_) std::cout << s << " Leaf " << constant_ << std::endl;
     }
 
     /** to graphviz file */
-    void dot(std::ostream& os, bool showZero) const {
+    void dot(std::ostream& os, bool showZero) const override {
       if (showZero || constant_) os << "\"" << this->id() << "\" [label=\""
           << boost::format("%4.2g") % constant_
           << "\", shape=box, rank=sink, height=0.35, fixedsize=true]\n"; // width=0.55,
     }
 
     /** evaluate */
-    const Y& operator()(const Assignment<L>& x) const {
+    const Y& operator()(const Assignment<L>& x) const override {
       return constant_;
     }
 
     /** apply unary operator */
-    NodePtr apply(const Unary& op) const {
+    NodePtr apply(const Unary& op) const override {
       NodePtr f(new Leaf(op(constant_)));
       return f;
     }
@@ -111,27 +111,27 @@ namespace gtsam {
     // Simply calls apply on argument to call correct virtual method:
     // fL.apply_f_op_g(gL) -> gL.apply_g_op_fL(fL) (below)
     // fL.apply_f_op_g(gC) -> gC.apply_g_op_fL(fL) (Choice)
-    NodePtr apply_f_op_g(const Node& g, const Binary& op) const {
+    NodePtr apply_f_op_g(const Node& g, const Binary& op) const override {
       return g.apply_g_op_fL(*this, op);
     }
 
     // Applying binary operator to two leaves results in a leaf
-    NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const {
+    NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const override {
       NodePtr h(new Leaf(op(fL.constant_, constant_))); // fL op gL
       return h;
     }
 
     // If second argument is a Choice node, call it's apply with leaf as second
-    NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const {
+    NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const override {
       return fC.apply_fC_op_gL(*this, op); // operand order back to normal
     }
 
     /** choose a branch, create new memory ! */
-    NodePtr choose(const L& label, size_t index) const {
+    NodePtr choose(const L& label, size_t index) const override {
       return NodePtr(new Leaf(constant()));
     }
 
-    bool isLeaf() const { return true; }
+    bool isLeaf() const override { return true; }
 
   }; // Leaf
 
@@ -175,7 +175,7 @@ namespace gtsam {
         return f;
     }
 
-    bool isLeaf() const { return false; }
+    bool isLeaf() const override { return false; }
 
     /** Constructor, given choice label and mandatory expected branch count */
     Choice(const L& label, size_t count) :
@@ -236,7 +236,7 @@ namespace gtsam {
     }
 
     /** print (as a tree) */
-    void print(const std::string& s) const {
+    void print(const std::string& s) const override {
       std::cout << s << " Choice(";
       //        std::cout << this << ",";
       std::cout << label_ << ") " << std::endl;
@@ -245,7 +245,7 @@ namespace gtsam {
     }
 
     /** output to graphviz (as a a graph) */
-    void dot(std::ostream& os, bool showZero) const {
+    void dot(std::ostream& os, bool showZero) const override {
       os << "\"" << this->id() << "\" [shape=circle, label=\"" << label_
           << "\"]\n";
       for (size_t i = 0; i < branches_.size(); i++) {
@@ -266,17 +266,17 @@ namespace gtsam {
     }
 
     /// Choice-Leaf equality: always false
-    bool sameLeaf(const Leaf& q) const {
+    bool sameLeaf(const Leaf& q) const override {
       return false;
     }
 
     /// polymorphic equality: if q is a leaf, could be...
-    bool sameLeaf(const Node& q) const {
+    bool sameLeaf(const Node& q) const override {
       return (q.isLeaf() && q.sameLeaf(*this));
     }
 
     /** equality up to tolerance */
-    bool equals(const Node& q, double tol) const {
+    bool equals(const Node& q, double tol) const override {
       const Choice* other = dynamic_cast<const Choice*> (&q);
       if (!other) return false;
       if (this->label_ != other->label_) return false;
@@ -288,7 +288,7 @@ namespace gtsam {
     }
 
     /** evaluate */
-    const Y& operator()(const Assignment<L>& x) const {
+    const Y& operator()(const Assignment<L>& x) const override {
 #ifndef NDEBUG
       typename Assignment<L>::const_iterator it = x.find(label_);
       if (it == x.end()) {
@@ -314,7 +314,7 @@ namespace gtsam {
     }
 
     /** apply unary operator */
-    NodePtr apply(const Unary& op) const {
+    NodePtr apply(const Unary& op) const override {
       boost::shared_ptr<Choice> r(new Choice(label_, *this, op));
       return Unique(r);
     }
@@ -324,12 +324,12 @@ namespace gtsam {
     // Simply calls apply on argument to call correct virtual method:
     // fC.apply_f_op_g(gL) -> gL.apply_g_op_fC(fC) -> (Leaf)
     // fC.apply_f_op_g(gC) -> gC.apply_g_op_fC(fC) -> (below)
-    NodePtr apply_f_op_g(const Node& g, const Binary& op) const {
+    NodePtr apply_f_op_g(const Node& g, const Binary& op) const override {
       return g.apply_g_op_fC(*this, op);
     }
 
     // If second argument of binary op is Leaf node, recurse on branches
-    NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const {
+    NodePtr apply_g_op_fL(const Leaf& fL, const Binary& op) const override {
       boost::shared_ptr<Choice> h(new Choice(label(), nrChoices()));
       for(NodePtr branch: branches_)
               h->push_back(fL.apply_f_op_g(*branch, op));
@@ -337,7 +337,7 @@ namespace gtsam {
     }
 
     // If second argument of binary op is Choice, call constructor
-    NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const {
+    NodePtr apply_g_op_fC(const Choice& fC, const Binary& op) const override {
       boost::shared_ptr<Choice> h(new Choice(fC, *this, op));
       return Unique(h);
     }
@@ -352,7 +352,7 @@ namespace gtsam {
     }
 
     /** choose a branch, recursively */
-    NodePtr choose(const L& label, size_t index) const {
+    NodePtr choose(const L& label, size_t index) const override {
       if (label_ == label)
         return branches_[index]; // choose branch
 

gtsam/discrete/DecisionTreeFactor.cpp

@@ -69,7 +69,7 @@ namespace gtsam {
     for(Key j: f.keys()) cs[j] = f.cardinality(j);
     // Convert map into keys
     DiscreteKeys keys;
-    for(const DiscreteKey& key: cs)
+    for(const std::pair<const Key,size_t>& key: cs)
       keys.push_back(key);
     // apply operand
     ADT result = ADT::apply(f, op);

gtsam/discrete/DecisionTreeFactor.h

@@ -69,23 +69,23 @@ namespace gtsam {
     /// @{
 
     /// equality
-    bool equals(const DiscreteFactor& other, double tol = 1e-9) const;
+    bool equals(const DiscreteFactor& other, double tol = 1e-9) const override;
 
     // print
-    virtual void print(const std::string& s = "DecisionTreeFactor:\n",
+    void print(const std::string& s = "DecisionTreeFactor:\n",
-        const KeyFormatter& formatter = DefaultKeyFormatter) const;
+        const KeyFormatter& formatter = DefaultKeyFormatter) const override;
 
     /// @}
     /// @name Standard Interface
     /// @{
 
     /// Value is just look up in AlgebraicDecisonTree
-    virtual double operator()(const Values& values) const {
+    double operator()(const Values& values) const override {
       return Potentials::operator()(values);
     }
 
     /// multiply two factors
-    DecisionTreeFactor operator*(const DecisionTreeFactor& f) const {
+    DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override {
       return apply(f, ADT::Ring::mul);
     }
 
@@ -95,7 +95,7 @@ namespace gtsam {
     }
 
     /// Convert into a decisiontree
-    virtual DecisionTreeFactor toDecisionTreeFactor() const {
+    DecisionTreeFactor toDecisionTreeFactor() const override {
       return *this;
     }
 

gtsam/discrete/DiscreteBayesTree.h

@@ -45,6 +45,7 @@ class GTSAM_EXPORT DiscreteBayesTreeClique
   typedef boost::shared_ptr<This> shared_ptr;
   typedef boost::weak_ptr<This> weak_ptr;
   DiscreteBayesTreeClique() {}
+  virtual ~DiscreteBayesTreeClique() {}
   DiscreteBayesTreeClique(
       const boost::shared_ptr<DiscreteConditional>& conditional)
       : Base(conditional) {}

gtsam/discrete/DiscreteConditional.h

@@ -85,10 +85,10 @@ public:
 
   /// GTSAM-style print
   void print(const std::string& s = "Discrete Conditional: ",
-      const KeyFormatter& formatter = DefaultKeyFormatter) const;
+      const KeyFormatter& formatter = DefaultKeyFormatter) const override;
 
   /// GTSAM-style equals
-  bool equals(const DiscreteFactor& other, double tol = 1e-9) const;
+  bool equals(const DiscreteFactor& other, double tol = 1e-9) const override;
 
   /// @}
   /// @name Standard Interface
@@ -102,7 +102,7 @@ public:
   }
 
   /// Evaluate, just look up in AlgebraicDecisonTree
-  virtual double operator()(const Values& values) const {
+  double operator()(const Values& values) const override {
     return Potentials::operator()(values);
   }
 

gtsam/discrete/Potentials.cpp

@@ -56,7 +56,7 @@ bool Potentials::equals(const Potentials& other, double tol) const {
 /* ************************************************************************* */
 void Potentials::print(const string& s, const KeyFormatter& formatter) const {
   cout << s << "\n  Cardinalities: {";
-  for (const DiscreteKey& key : cardinalities_)
+  for (const std::pair<const Key,size_t>& key : cardinalities_)
     cout << formatter(key.first) << ":" << key.second << ", ";
   cout << "}" << endl;
   ADT::print(" ");

gtsam/geometry/Cal3DS2.h

@@ -60,7 +60,7 @@ public:
   /// @{
 
   /// print with optional string
-  virtual void print(const std::string& s = "") const ;
+  void print(const std::string& s = "") const override;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3DS2& K, double tol = 10e-9) const;
@@ -86,7 +86,7 @@ public:
   /// @{
 
   /// @return a deep copy of this object
-  virtual boost::shared_ptr<Base> clone() const {
+  boost::shared_ptr<Base> clone() const override {
     return boost::shared_ptr<Base>(new Cal3DS2(*this));
   }
 

gtsam/geometry/Cal3DS2_Base.h

@@ -69,7 +69,7 @@ public:
   /// @{
 
   /// print with optional string
-  virtual void print(const std::string& s = "") const ;
+  virtual void print(const std::string& s = "") const;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;

gtsam/geometry/Cal3Unified.h

@@ -75,7 +75,7 @@ public:
   /// @{
 
   /// print with optional string
-  virtual void print(const std::string& s = "") const ;
+  void print(const std::string& s = "") const override;
 
   /// assert equality up to a tolerance
   bool equals(const Cal3Unified& K, double tol = 10e-9) const;

gtsam/geometry/PinholeCamera.h

@@ -175,7 +175,7 @@ public:
   }
 
   /// return calibration
-  const Calibration& calibration() const {
+  const Calibration& calibration() const override {
     return K_;
   }
 

gtsam/geometry/PinholePose.h

@@ -361,7 +361,7 @@ public:
   }
 
   /// return calibration
-  virtual const CALIBRATION& calibration() const {
+  const CALIBRATION& calibration() const override {
     return *K_;
   }
 

gtsam/geometry/PinholeSet.h

@@ -45,7 +45,7 @@ public:
   /// @{
 
   /// print
-  virtual void print(const std::string& s = "") const {
+  void print(const std::string& s = "") const override {
     Base::print(s);
   }
 

gtsam/geometry/Pose3.cpp

@@ -106,9 +106,7 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
 
 /* ************************************************************************* */
 void Pose3::print(const string& s) const {
-  cout << s;
+  cout << (s.empty() ? s : s + " ") << *this << endl;
-  R_.print("R:\n");
-  cout << t_ << ";" << endl;
 }
 
 /* ************************************************************************* */
@@ -430,9 +428,9 @@ boost::optional<Pose3> align(const vector<Point3Pair>& baPointPairs) {
 
 /* ************************************************************************* */
 std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
-  os << pose.rotation() << "\n";
+  // Both Rot3 and Point3 have ostream definitions so we use them.
-  const Point3& t = pose.translation();
+  os << "R: " << pose.rotation() << "\n";
-  os << '[' << t.x() << ", " << t.y() << ", " << t.z() << "]\';\n";
+  os << "t: " << pose.translation();
   return os;
 }
 

gtsam/geometry/Rot3.cpp

@@ -32,7 +32,8 @@ namespace gtsam {
 
 /* ************************************************************************* */
 void Rot3::print(const std::string& s) const {
-  gtsam::print((Matrix)matrix(), s);
+  cout << (s.empty() ? "R: " : s + " ");
+  gtsam::print(static_cast<Matrix>(matrix()));
 }
 
 /* ************************************************************************* */
@@ -222,10 +223,7 @@ pair<Matrix3, Vector3> RQ(const Matrix3& A) {
 
 /* ************************************************************************* */
 ostream &operator<<(ostream &os, const Rot3& R) {
-  os << "\n";
+  os << R.matrix().format(matlabFormat());
-  os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n";
-  os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n";
-  os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n";
   return os;
 }
 

gtsam/geometry/Rot3.h

@@ -254,7 +254,7 @@ namespace gtsam {
     /// @{
 
     /** print */
-    void print(const std::string& s="R") const;
+    void print(const std::string& s="") const;
 
     /** equals with an tolerance */
     bool equals(const Rot3& p, double tol = 1e-9) const;

gtsam/geometry/SO4.cpp

@@ -35,7 +35,7 @@ namespace gtsam {
 
 // TODO(frank): is this better than SOn::Random?
 // /* *************************************************************************
-// */ static Vector3 randomOmega(boost::mt19937 &rng) {
+// */ static Vector3 randomOmega(std::mt19937 &rng) {
 //   static std::uniform_real_distribution<double> randomAngle(-M_PI, M_PI);
 //   return Unit3::Random(rng).unitVector() * randomAngle(rng);
 // }
@@ -43,7 +43,7 @@ namespace gtsam {
 // /* *************************************************************************
 // */
 // // Create random SO(4) element using direct product of lie algebras.
-// SO4 SO4::Random(boost::mt19937 &rng) {
+// SO4 SO4::Random(std::mt19937 &rng) {
 //   Vector6 delta;
 //   delta << randomOmega(rng), randomOmega(rng);
 //   return SO4::Expmap(delta);

gtsam/geometry/SO4.h

@@ -34,7 +34,7 @@ namespace gtsam {
 using SO4 = SO<4>;
 
 // /// Random SO(4) element (no big claims about uniformity)
-// static SO4 Random(boost::mt19937 &rng);
+// static SO4 Random(std::mt19937 &rng);
 
 // Below are all declarations of SO<4> specializations.
 // They are *defined* in SO4.cpp.

gtsam/geometry/tests/testPose3.cpp

@@ -862,7 +862,8 @@ TEST( Pose3, stream)
   Pose3 T;
   std::ostringstream os;
   os << T;
-  EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n\n[0, 0, 0]';\n");
+  string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: [0, 0, 0]'";
+  EXPECT(os.str() == expected);
 }
 
 //******************************************************************************
@@ -1032,19 +1033,22 @@ TEST(Pose3, print) {
   std::stringstream expected;
   Point3 translation(1, 2, 3);
 
+  // Add expected rotation
+  expected << "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n";
+
 #ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
   expected << "1\n"
               "2\n"
               "3;\n";
 #else
-  expected << '[' << translation.x() << ", " << translation.y() << ", " << translation.z() << "]\';";
+  expected << "t: [" << translation.x() << ", " << translation.y() << ", " << translation.z() << "]'\n";
 #endif
 
   // reset cout to the original stream
   std::cout.rdbuf(oldbuf);
 
   // Get substring corresponding to translation part
-  std::string actual = redirectStream.str().substr(38, 11);
+  std::string actual = redirectStream.str();
 
   CHECK_EQUAL(expected.str(), actual);
 }

gtsam/geometry/tests/testRot3.cpp

@@ -608,7 +608,8 @@ TEST( Rot3, stream)
   Rot3 R;
   std::ostringstream os;
   os << R;
-  EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n");
+  string expected = "[\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]";
+  EXPECT(os.str() == expected);
 }
 
 /* ************************************************************************* */

gtsam/inference/BayesTree.h

@@ -280,7 +280,7 @@ namespace gtsam {
       this->keys_.assign(clique->conditional()->beginParents(), clique->conditional()->endParents());
     }
 
-    void print(const std::string& s="", const KeyFormatter& formatter = DefaultKeyFormatter) const {
+    void print(const std::string& s="", const KeyFormatter& formatter = DefaultKeyFormatter) const override {
       clique->print(s + "stored clique", formatter);
     }
   };

gtsam/inference/BayesTreeCliqueBase.h

@@ -100,7 +100,7 @@ namespace gtsam {
     bool equals(const DERIVED& other, double tol = 1e-9) const;
 
     /** print this node */
-    void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+    virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
 
     /// @}
     /// @name Standard Interface

gtsam/inference/inferenceExceptions.h

@@ -28,9 +28,9 @@ namespace gtsam {
    *  with an ordering that does not include all of the variables. */
   class InconsistentEliminationRequested : public std::exception {
   public:
-    InconsistentEliminationRequested() throw() {}
+    InconsistentEliminationRequested() noexcept {}
-    virtual ~InconsistentEliminationRequested() throw() {}
+    virtual ~InconsistentEliminationRequested() noexcept {}
-    virtual const char* what() const throw() {
+    const char* what() const noexcept override {
       return
         "An inference algorithm was called with inconsistent arguments.  The\n"
         "factor graph, ordering, or variable index were inconsistent with each\n"

gtsam/linear/BinaryJacobianFactor.h

@@ -49,7 +49,7 @@ struct BinaryJacobianFactor: JacobianFactor {
 
   // Fixed-size matrix update
   void updateHessian(const KeyVector& infoKeys,
-      SymmetricBlockMatrix* info) const {
+      SymmetricBlockMatrix* info) const override {
     gttic(updateHessian_BinaryJacobianFactor);
     // Whiten the factor if it has a noise model
     const SharedDiagonal& model = get_model();

gtsam/linear/ConjugateGradientSolver.h

@@ -80,7 +80,7 @@ public:
 
 
   void print() const { Base::print(); }
-  virtual void print(std::ostream &os) const;
+  void print(std::ostream &os) const override;
 
   static std::string blasTranslator(const BLASKernel k) ;
   static BLASKernel blasTranslator(const std::string &s) ;

gtsam/linear/GaussianBayesTree.h

@@ -41,6 +41,7 @@ namespace gtsam {
     typedef boost::shared_ptr<This> shared_ptr;
     typedef boost::weak_ptr<This> weak_ptr;
     GaussianBayesTreeClique() {}
+    virtual ~GaussianBayesTreeClique() {}
     GaussianBayesTreeClique(const boost::shared_ptr<GaussianConditional>& conditional) : Base(conditional) {}
   };
 

gtsam/linear/GaussianConditional.h

@@ -88,10 +88,10 @@ namespace gtsam {
 
     /** print */
     void print(const std::string& = "GaussianConditional",
-      const KeyFormatter& formatter = DefaultKeyFormatter) const;
+      const KeyFormatter& formatter = DefaultKeyFormatter) const override;
 
     /** equals function */
-    bool equals(const GaussianFactor&cg, double tol = 1e-9) const;
+    bool equals(const GaussianFactor&cg, double tol = 1e-9) const override;
 
     /** Return a view of the upper-triangular R block of the conditional */
     constABlock R() const { return Ab_.range(0, nrFrontals()); }

gtsam/linear/GaussianDensity.h

@@ -57,7 +57,7 @@ namespace gtsam {
 
     /// print
     void print(const std::string& = "GaussianDensity",
-      const KeyFormatter& formatter = DefaultKeyFormatter) const;
+      const KeyFormatter& formatter = DefaultKeyFormatter) const override;
 
     /// Mean \f$ \mu = R^{-1} d \f$
     Vector mean() const;

gtsam/linear/GaussianISAM.h

@@ -20,6 +20,7 @@
 
 #include <gtsam/linear/GaussianBayesTree.h>
 #include <gtsam/inference/ISAM.h>
+#include <gtsam/base/Testable.h>
 
 namespace gtsam {
 
@@ -43,4 +44,8 @@ namespace gtsam {
 
   };
 
+  /// traits
+  template <>
+  struct traits<GaussianISAM> : public Testable<GaussianISAM> {};
+
 }

gtsam/linear/JacobianFactor.cpp

@@ -421,21 +421,11 @@ JacobianFactor::JacobianFactor(const GaussianFactorGraph& graph,
 /* ************************************************************************* */
 void JacobianFactor::print(const string& s,
     const KeyFormatter& formatter) const {
-  static const Eigen::IOFormat matlab(
-      Eigen::StreamPrecision, // precision
-      0, // flags
-      " ", // coeffSeparator
-      ";\n", // rowSeparator
-      "\t",  // rowPrefix
-      "", // rowSuffix
-      "[\n", // matPrefix
-      "\n  ]" // matSuffix
-      );
   if (!s.empty())
     cout << s << "\n";
   for (const_iterator key = begin(); key != end(); ++key) {
     cout << boost::format("  A[%1%] = ") % formatter(*key);
-    cout << getA(key).format(matlab) << endl;
+    cout << getA(key).format(matlabFormat()) << endl;
   }
   cout << formatMatrixIndented("  b = ", getb(), true) << "\n";
   if (model_)

gtsam/linear/LossFunctions.h

@@ -130,10 +130,10 @@ class GTSAM_EXPORT Null : public Base {
 
   Null(const ReweightScheme reweight = Block) : Base(reweight) {}
   ~Null() {}
-  double weight(double /*error*/) const { return 1.0; }
+  double weight(double /*error*/) const override { return 1.0; }
-  double loss(double distance) const { return 0.5 * distance * distance; }
+  double loss(double distance) const override { return 0.5 * distance * distance; }
-  void print(const std::string &s) const;
+  void print(const std::string &s) const override;
-  bool equals(const Base & /*expected*/, double /*tol*/) const { return true; }
+  bool equals(const Base & /*expected*/, double /*tol*/) const override { return true; }
   static shared_ptr Create();
 
  private:

gtsam/linear/PCGSolver.h

@@ -41,7 +41,7 @@ public:
   PCGSolverParameters() {
   }
 
-  virtual void print(std::ostream &os) const;
+  void print(std::ostream &os) const override;
 
   /* interface to preconditioner parameters */
   inline const PreconditionerParameters& preconditioner() const {
@@ -77,9 +77,9 @@ public:
 
   using IterativeSolver::optimize;
 
-  virtual VectorValues optimize(const GaussianFactorGraph &gfg,
+  VectorValues optimize(const GaussianFactorGraph &gfg,
       const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
-      const VectorValues &initial);
+      const VectorValues &initial) override;
 
 };
 

gtsam/linear/Preconditioner.cpp

@@ -145,7 +145,7 @@ void BlockJacobiPreconditioner::build(
 
   /* getting the block diagonals over the factors */
   std::map<Key, Matrix> hessianMap =gfg.hessianBlockDiagonal();
-  for ( const Matrix hessian: hessianMap | boost::adaptors::map_values)
+  for (const Matrix& hessian: hessianMap | boost::adaptors::map_values)
     blocks.push_back(hessian);
 
   /* if necessary, allocating the memory for cacheing the factorization results */

gtsam/linear/Preconditioner.h

@@ -111,13 +111,13 @@ public:
   virtual ~DummyPreconditioner() {}
 
   /* Computation Interfaces for raw vector */
-  virtual void solve(const Vector& y, Vector &x) const { x = y; }
+  void solve(const Vector& y, Vector &x) const override { x = y; }
-  virtual void transposeSolve(const Vector& y, Vector& x) const { x = y; }
+  void transposeSolve(const Vector& y, Vector& x) const  override { x = y; }
-  virtual void build(
+  void build(
     const GaussianFactorGraph &gfg,
     const KeyInfo &info,
     const std::map<Key,Vector> &lambda
-    )  {}
+    ) override {}
 };
 
 /*******************************************************************************************/
@@ -135,13 +135,13 @@ public:
   virtual ~BlockJacobiPreconditioner() ;
 
   /* Computation Interfaces for raw vector */
-  virtual void solve(const Vector& y, Vector &x) const;
+  void solve(const Vector& y, Vector &x) const override;
-  virtual void transposeSolve(const Vector& y, Vector& x) const ;
+  void transposeSolve(const Vector& y, Vector& x) const override;
-  virtual void build(
+  void build(
     const GaussianFactorGraph &gfg,
     const KeyInfo &info,
     const std::map<Key,Vector> &lambda
-    ) ;
+    ) override;
 
 protected:
 

gtsam/linear/RegularHessianFactor.h

@@ -109,8 +109,8 @@ private:
 public:
 
   /** y += alpha * A'*A*x */
-  virtual void multiplyHessianAdd(double alpha, const VectorValues& x,
+  void multiplyHessianAdd(double alpha, const VectorValues& x,
-      VectorValues& y) const {
+      VectorValues& y) const override {
     HessianFactor::multiplyHessianAdd(alpha, x, y);
   }
 
@@ -182,7 +182,7 @@ public:
   }
 
   /** Return the diagonal of the Hessian for this factor (raw memory version) */
-  virtual void hessianDiagonal(double* d) const {
+  void hessianDiagonal(double* d) const override {
 
     // Loop over all variables in the factor
     for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
@@ -193,7 +193,7 @@ public:
   }
 
   /// Add gradient at zero to d TODO: is it really the goal to add ??
-  virtual void gradientAtZero(double* d) const {
+  void gradientAtZero(double* d) const override {
 
     // Loop over all variables in the factor
     for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {

gtsam/linear/RegularJacobianFactor.h

@@ -70,8 +70,8 @@ public:
   using JacobianFactor::multiplyHessianAdd;
 
   /** y += alpha * A'*A*x */
-  virtual void multiplyHessianAdd(double alpha, const VectorValues& x,
+  void multiplyHessianAdd(double alpha, const VectorValues& x,
-      VectorValues& y) const {
+      VectorValues& y) const override {
     JacobianFactor::multiplyHessianAdd(alpha, x, y);
   }
 
@@ -106,7 +106,7 @@ public:
   using GaussianFactor::hessianDiagonal;
 
   /// Raw memory access version of hessianDiagonal
-  void hessianDiagonal(double* d) const {
+  void hessianDiagonal(double* d) const override {
     // Loop over all variables in the factor
     for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
       // Get the diagonal block, and insert its diagonal
@@ -125,12 +125,12 @@ public:
   }
 
   /// Expose base class gradientAtZero
-  virtual VectorValues gradientAtZero() const {
+  VectorValues gradientAtZero() const override {
     return JacobianFactor::gradientAtZero();
   }
 
   /// Raw memory access version of gradientAtZero
-  void gradientAtZero(double* d) const {
+  void gradientAtZero(double* d) const override {
 
     // Get vector b not weighted
     Vector b = getb();

gtsam/linear/SubgraphSolver.h

@@ -38,7 +38,7 @@ struct GTSAM_EXPORT SubgraphSolverParameters
   explicit SubgraphSolverParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
     : builderParams(p) {}
   void print() const { Base::print(); }
-  virtual void print(std::ostream &os) const {
+  void print(std::ostream &os) const override {
     Base::print(os);
   }
 };

gtsam/linear/VectorValues.cpp

@@ -45,7 +45,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   VectorValues::VectorValues(const Vector& x, const Dims& dims) {
-    typedef pair<Key, size_t> Pair;
+    using Pair = pair<const Key, size_t>;
     size_t j = 0;
     for (const Pair& v : dims) {
       Key key;

gtsam/linear/linearExceptions.cpp

@@ -24,7 +24,7 @@
 namespace gtsam {
 
   /* ************************************************************************* */
-  const char* IndeterminantLinearSystemException::what() const throw()
+  const char* IndeterminantLinearSystemException::what() const noexcept
   {
     if(!description_) {
       description_ = String(
@@ -43,7 +43,7 @@ more information.");
   }
 
   /* ************************************************************************* */
-  const char* InvalidNoiseModel::what() const throw() {
+  const char* InvalidNoiseModel::what() const noexcept {
     if(description_.empty())
       description_ = (boost::format(
       "A JacobianFactor was attempted to be constructed or modified to use a\n"
@@ -54,7 +54,7 @@ more information.");
   }
 
   /* ************************************************************************* */
-  const char* InvalidMatrixBlock::what() const throw() {
+  const char* InvalidMatrixBlock::what() const noexcept {
     if(description_.empty())
       description_ = (boost::format(
       "A JacobianFactor was attempted to be constructed with a matrix block of\n"

gtsam/linear/linearExceptions.h

@@ -94,10 +94,10 @@ namespace gtsam {
   class GTSAM_EXPORT IndeterminantLinearSystemException : public ThreadsafeException<IndeterminantLinearSystemException> {
     Key j_;
   public:
-    IndeterminantLinearSystemException(Key j) throw() : j_(j) {}
+    IndeterminantLinearSystemException(Key j) noexcept : j_(j) {}
-    virtual ~IndeterminantLinearSystemException() throw() {}
+    virtual ~IndeterminantLinearSystemException() noexcept {}
     Key nearbyVariable() const { return j_; }
-    virtual const char* what() const throw();
+    const char* what() const noexcept override;
   };
 
   /* ************************************************************************* */
@@ -110,9 +110,9 @@ namespace gtsam {
 
     InvalidNoiseModel(DenseIndex factorDims, DenseIndex noiseModelDims) :
       factorDims(factorDims), noiseModelDims(noiseModelDims) {}
-    virtual ~InvalidNoiseModel() throw() {}
+    virtual ~InvalidNoiseModel() noexcept {}
 
-    virtual const char* what() const throw();
+    const char* what() const noexcept override;
 
   private:
     mutable std::string description_;
@@ -128,9 +128,9 @@ namespace gtsam {
 
     InvalidMatrixBlock(DenseIndex factorRows, DenseIndex blockRows) :
       factorRows(factorRows), blockRows(blockRows) {}
-    virtual ~InvalidMatrixBlock() throw() {}
+    virtual ~InvalidMatrixBlock() noexcept {}
 
-    virtual const char* what() const throw();
+    const char* what() const noexcept override;
 
   private:
     mutable std::string description_;

gtsam/linear/tests/testGaussianBayesNet.cpp

@@ -137,7 +137,7 @@ TEST( GaussianBayesNet, optimize3 )
 }
 
 /* ************************************************************************* */
-TEST(GaussianBayesNet, ordering) 
+TEST(GaussianBayesNet, ordering)
 {
   Ordering expected;
   expected += _x_, _y_;
@@ -155,7 +155,7 @@ TEST( GaussianBayesNet, MatrixStress )
   bn.emplace_shared<GC>(_z_, Vector2(5, 6), 6 * I_2x2);
 
   const VectorValues expected = bn.optimize();
-  for (const auto keys :
+  for (const auto& keys :
        {KeyVector({_x_, _y_, _z_}), KeyVector({_x_, _z_, _y_}),
         KeyVector({_y_, _x_, _z_}), KeyVector({_y_, _z_, _x_}),
         KeyVector({_z_, _x_, _y_}), KeyVector({_z_, _y_, _x_})}) {
@@ -183,7 +183,7 @@ TEST( GaussianBayesNet, backSubstituteTranspose )
 
   VectorValues actual = smallBayesNet.backSubstituteTranspose(x);
   EXPECT(assert_equal(expected, actual));
-  
+
   const auto ordering = noisyBayesNet.ordering();
   const Matrix R = smallBayesNet.matrix(ordering).first;
   const Vector expected_vector = R.transpose().inverse() * x.vector(ordering);
@@ -206,7 +206,7 @@ TEST( GaussianBayesNet, backSubstituteTransposeNoisy )
 
   VectorValues actual = noisyBayesNet.backSubstituteTranspose(x);
   EXPECT(assert_equal(expected, actual));
-  
+
   const auto ordering = noisyBayesNet.ordering();
   const Matrix R = noisyBayesNet.matrix(ordering).first;
   const Vector expected_vector = R.transpose().inverse() * x.vector(ordering);

gtsam/navigation/AHRSFactor.h

@@ -158,14 +158,14 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const;
+  gtsam::NonlinearFactor::shared_ptr clone() const override;
 
   /// print
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
+  void print(const std::string& s, const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const;
+      DefaultKeyFormatter) const override;
 
   /// equals
-  virtual bool equals(const NonlinearFactor&, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor&, double tol = 1e-9) const override;
 
   /// Access the preintegrated measurements.
   const PreintegratedAhrsMeasurements& preintegratedMeasurements() const {
@@ -178,7 +178,7 @@ public:
   Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j,
       const Vector3& bias, boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
-          boost::none) const;
+          boost::none) const override;
 
   /// predicted states from IMU
   /// TODO(frank): relationship with PIM predict ??

gtsam/navigation/AttitudeFactor.h

@@ -108,21 +108,21 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   /** print */
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
+  void print(const std::string& s, const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const;
+      DefaultKeyFormatter) const override;
 
   /** equals */
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
   /** vector of errors */
-  virtual Vector evaluateError(const Rot3& nRb, //
+  Vector evaluateError(const Rot3& nRb, //
-      boost::optional<Matrix&> H = boost::none) const {
+      boost::optional<Matrix&> H = boost::none) const override {
     return attitudeError(nRb, H);
   }
 
@@ -182,21 +182,21 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   /** print */
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
+  void print(const std::string& s, const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const;
+      DefaultKeyFormatter) const override;
 
   /** equals */
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
   /** vector of errors */
-  virtual Vector evaluateError(const Pose3& nTb, //
+  Vector evaluateError(const Pose3& nTb, //
-      boost::optional<Matrix&> H = boost::none) const {
+      boost::optional<Matrix&> H = boost::none) const override {
     Vector e = attitudeError(nTb.rotation(), H);
     if (H) {
       Matrix H23 = *H;

gtsam/navigation/CombinedImuFactor.h

@@ -87,8 +87,8 @@ struct GTSAM_EXPORT PreintegrationCombinedParams : PreintegrationParams {
     return boost::shared_ptr<PreintegrationCombinedParams>(new PreintegrationCombinedParams(Vector3(0, 0, -g)));
   }
 
-  void print(const std::string& s="") const;
+  void print(const std::string& s="") const override;
-  bool equals(const PreintegratedRotationParams& other, double tol) const;
+  bool equals(const PreintegratedRotationParams& other, double tol) const override;
 
   void setBiasAccCovariance(const Matrix3& cov) { biasAccCovariance=cov; }
   void setBiasOmegaCovariance(const Matrix3& cov) { biasOmegaCovariance=cov; }
@@ -294,7 +294,7 @@ public:
   virtual ~CombinedImuFactor() {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const;
+  gtsam::NonlinearFactor::shared_ptr clone() const override;
 
   /** implement functions needed for Testable */
 
@@ -303,11 +303,11 @@ public:
   GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
                                                const CombinedImuFactor&);
   /// print
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
+  void print(const std::string& s, const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const;
+      DefaultKeyFormatter) const override;
 
   /// equals
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
   /// @}
 
   /** Access the preintegrated measurements. */
@@ -325,7 +325,7 @@ public:
       boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
           boost::none, boost::optional<Matrix&> H3 = boost::none,
       boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
-          boost::none, boost::optional<Matrix&> H6 = boost::none) const;
+          boost::none, boost::optional<Matrix&> H6 = boost::none) const override;
 
  private:
   /** Serialization function */

gtsam/navigation/GPSFactor.h

@@ -65,21 +65,21 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   /// print
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
+  void print(const std::string& s, const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const;
+      DefaultKeyFormatter) const override;
 
   /// equals
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
   /// vector of errors
   Vector evaluateError(const Pose3& p,
-      boost::optional<Matrix&> H = boost::none) const;
+      boost::optional<Matrix&> H = boost::none) const override;
 
   inline const Point3 & measurementIn() const {
     return nT_;
@@ -137,21 +137,21 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   /// print
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
+  void print(const std::string& s, const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const;
+      DefaultKeyFormatter) const override;
 
   /// equals
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
   /// vector of errors
   Vector evaluateError(const NavState& p,
-      boost::optional<Matrix&> H = boost::none) const;
+      boost::optional<Matrix&> H = boost::none) const override;
 
   inline const Point3 & measurementIn() const {
     return nT_;

gtsam/navigation/ImuFactor.h

@@ -200,14 +200,14 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const;
+  gtsam::NonlinearFactor::shared_ptr clone() const override;
 
   /// @name Testable
   /// @{
   GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor&);
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
+  void print(const std::string& s, const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const;
+      DefaultKeyFormatter) const override;
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
   /// @}
 
   /** Access the preintegrated measurements. */
@@ -224,7 +224,7 @@ public:
       const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1 =
           boost::none, boost::optional<Matrix&> H2 = boost::none,
       boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
-          boost::none, boost::optional<Matrix&> H5 = boost::none) const;
+          boost::none, boost::optional<Matrix&> H5 = boost::none) const override;
 
 #ifdef GTSAM_TANGENT_PREINTEGRATION
   /// Merge two pre-integrated measurement classes
@@ -278,14 +278,14 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const;
+  gtsam::NonlinearFactor::shared_ptr clone() const override;
 
   /// @name Testable
   /// @{
   GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const ImuFactor2&);
-  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
+  void print(const std::string& s, const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const;
+      DefaultKeyFormatter) const override;
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
   /// @}
 
   /** Access the preintegrated measurements. */
@@ -301,7 +301,7 @@ public:
                        const imuBias::ConstantBias& bias_i,  //
                        boost::optional<Matrix&> H1 = boost::none,
                        boost::optional<Matrix&> H2 = boost::none,
-                       boost::optional<Matrix&> H3 = boost::none) const;
+                       boost::optional<Matrix&> H3 = boost::none) const override;
 
 private:
 

gtsam/navigation/MagFactor.h

@@ -53,7 +53,7 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual NonlinearFactor::shared_ptr clone() const {
+  NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<NonlinearFactor>(
         NonlinearFactor::shared_ptr(new MagFactor(*this)));
   }
@@ -73,7 +73,7 @@ public:
    * @brief vector of errors
    */
   Vector evaluateError(const Rot2& nRb,
-      boost::optional<Matrix&> H = boost::none) const {
+      boost::optional<Matrix&> H = boost::none) const override {
     // measured bM = nRb<52> * nM + b
     Point3 hx = unrotate(nRb, nM_, H) + bias_;
     return (hx - measured_);
@@ -102,7 +102,7 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual NonlinearFactor::shared_ptr clone() const {
+  NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<NonlinearFactor>(
         NonlinearFactor::shared_ptr(new MagFactor1(*this)));
   }
@@ -111,7 +111,7 @@ public:
    * @brief vector of errors
    */
   Vector evaluateError(const Rot3& nRb,
-      boost::optional<Matrix&> H = boost::none) const {
+      boost::optional<Matrix&> H = boost::none) const override {
     // measured bM = nRb<52> * nM + b
     Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_;
     return (hx - measured_);
@@ -138,7 +138,7 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual NonlinearFactor::shared_ptr clone() const {
+  NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<NonlinearFactor>(
         NonlinearFactor::shared_ptr(new MagFactor2(*this)));
   }
@@ -150,7 +150,7 @@ public:
    */
   Vector evaluateError(const Point3& nM, const Point3& bias,
       boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
-          boost::none) const {
+          boost::none) const override {
     // measured bM = nRb<52> * nM + b, where b is unknown bias
     Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias;
     if (H2)
@@ -179,7 +179,7 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual NonlinearFactor::shared_ptr clone() const {
+  NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<NonlinearFactor>(
         NonlinearFactor::shared_ptr(new MagFactor3(*this)));
   }
@@ -192,7 +192,7 @@ public:
   Vector evaluateError(const double& scale, const Unit3& direction,
       const Point3& bias, boost::optional<Matrix&> H1 = boost::none,
       boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
-          boost::none) const {
+          boost::none) const override {
     // measured bM = nRb<52> * nM + b, where b is unknown bias
     Unit3 rotated = bRn_.rotate(direction, boost::none, H2);
     Point3 hx = scale * rotated.point3() + bias;

gtsam/navigation/NavState.cpp

@@ -88,15 +88,15 @@ Matrix7 NavState::matrix() const {
 
 //------------------------------------------------------------------------------
 ostream& operator<<(ostream& os, const NavState& state) {
-  os << "R:" << state.attitude();
+  os << "R: " << state.attitude() << "\n";
-  os << "p:" << state.position() << endl;
+  os << "p: " << state.position() << "\n";
-  os << "v:" << Point3(state.velocity()) << endl;
+  os << "v: " << Point3(state.velocity());
   return os;
 }
 
 //------------------------------------------------------------------------------
 void NavState::print(const string& s) const {
-  cout << s << *this << endl;
+  cout << (s.empty() ? s : s + " ") << *this << endl;
 }
 
 //------------------------------------------------------------------------------

gtsam/navigation/PreintegrationParams.h

@@ -56,8 +56,8 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
     return boost::shared_ptr<PreintegrationParams>(new PreintegrationParams(Vector3(0, 0, -g)));
   }
 
-  void print(const std::string& s="") const;
+  void print(const std::string& s="") const override;
-  bool equals(const PreintegratedRotationParams& other, double tol) const;
+  bool equals(const PreintegratedRotationParams& other, double tol) const override;
 
   void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; }
   void setIntegrationCovariance(const Matrix3& cov)   { integrationCovariance = cov; }

gtsam/navigation/tests/testNavState.cpp

@@ -206,6 +206,18 @@ TEST(NavState, CorrectPIM) {
   EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2));
 }
 
+/* ************************************************************************* */
+TEST(NavState, Stream)
+{
+  NavState state;
+
+  std::ostringstream os;
+  os << state;
+  string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\np: [0, 0, 0]'\nv: [0, 0, 0]'";
+  EXPECT(os.str() == expected);
+}
+
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/nonlinear/ExpressionFactor.h

@@ -19,16 +19,26 @@
 
 #pragma once
 
+#include <array>
+#include <gtsam/config.h>
+#include <gtsam/base/Testable.h>
 #include <gtsam/nonlinear/Expression.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/base/Testable.h>
 #include <numeric>
 
 namespace gtsam {
 
 /**
-
+ * Factor that supports arbitrary expressions via AD.
- * Factor that supports arbitrary expressions via AD
+ *
+ * Arbitrary instances of this template can be directly inserted into a factor
+ * graph for optimization. However, to enable the correct (de)serialization of
+ * such instances, the user should declare derived classes from this template,
+ * implementing expresion(), serialize(), clone(), print(), and defining the
+ * corresponding `struct traits<NewFactor> : public Testable<NewFactor> {}`.
+ *
+ * \tparam T Type for measurements.
+ *
  */
 template<typename T>
 class ExpressionFactor: public NoiseModelFactor {
@@ -68,13 +78,13 @@ protected:
 
   /// print relies on Testable traits being defined for T
   void print(const std::string& s = "",
-             const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+             const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     NoiseModelFactor::print(s, keyFormatter);
     traits<T>::Print(measured_, "ExpressionFactor with measurement: ");
   }
 
   /// equals relies on Testable traits being defined for T
-  bool equals(const NonlinearFactor& f, double tol) const {
+  bool equals(const NonlinearFactor& f, double tol) const override {
     const ExpressionFactor* p = dynamic_cast<const ExpressionFactor*>(&f);
     return p && NoiseModelFactor::equals(f, tol) &&
            traits<T>::Equals(measured_, p->measured_, tol) &&
@@ -86,8 +96,8 @@ protected:
    * We override this method to provide
    * both the function evaluation and its derivative(s) in H.
    */
-  virtual Vector unwhitenedError(const Values& x,
+  Vector unwhitenedError(const Values& x,
-                                 boost::optional<std::vector<Matrix>&> H = boost::none) const {
+    boost::optional<std::vector<Matrix>&> H = boost::none) const override {
     if (H) {
       const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H);
       // NOTE(hayk): Doing the reverse, AKA Local(measured_, value) is not correct here
@@ -99,7 +109,7 @@ protected:
     }
   }
 
-  virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
+  boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
     // Only linearize if the factor is active
     if (!active(x))
       return boost::shared_ptr<JacobianFactor>();
@@ -138,7 +148,7 @@ protected:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
@@ -217,26 +227,98 @@ private:
 template <typename T>
 struct traits<ExpressionFactor<T> > : public Testable<ExpressionFactor<T> > {};
 
+/**
+ * N-ary variadic template for ExpressionFactor meant as a base class for N-ary
+ * factors. Enforces an 'expression' method with N keys.
+ * Derived class (an N-factor!) needs to call 'initialize'.
+ *
+ * Does not provide backward compatible 'evaluateError'.
+ *
+ * \tparam T Type for measurements. The rest of template arguments are types
+ *         for the N key-indexed Values.
+ *
+ */
+template <typename T, typename... Args>
+class ExpressionFactorN : public ExpressionFactor<T> {
+public:
+  static const std::size_t NARY_EXPRESSION_SIZE = sizeof...(Args);
+  using ArrayNKeys = std::array<Key, NARY_EXPRESSION_SIZE>;
+
+  /// Destructor
+  virtual ~ExpressionFactorN() = default;
+
+  // Don't provide backward compatible evaluateVector(), due to its problematic
+  // variable length of optional Jacobian arguments. Vector evaluateError(const
+  // Args... args,...);
+
+  /// Recreate expression from given keys_ and measured_, used in load
+  /// Needed to deserialize a derived factor
+  virtual Expression<T> expression(const ArrayNKeys &keys) const {
+    throw std::runtime_error(
+        "ExpressionFactorN::expression not provided: cannot deserialize.");
+  }
+
+protected:
+  /// Default constructor, for serialization
+  ExpressionFactorN() = default;
+
+  /// Constructor takes care of keys, but still need to call initialize
+  ExpressionFactorN(const ArrayNKeys &keys, const SharedNoiseModel &noiseModel,
+                    const T &measurement)
+      : ExpressionFactor<T>(noiseModel, measurement) {
+    for (const auto &key : keys)
+      Factor::keys_.push_back(key);
+  }
+
+private:
+  /// Return an expression that predicts the measurement given Values
+  Expression<T> expression() const override {
+    ArrayNKeys keys;
+    int idx = 0;
+    for (const auto &key : Factor::keys_)
+      keys[idx++] = key;
+    return expression(keys);
+  }
+
+  friend class boost::serialization::access;
+  template <class ARCHIVE>
+  void serialize(ARCHIVE &ar, const unsigned int /*version*/) {
+    ar &boost::serialization::make_nvp(
+        "ExpressionFactorN",
+        boost::serialization::base_object<ExpressionFactor<T>>(*this));
+  }
+};
+/// traits
+template <typename T, typename... Args>
+struct traits<ExpressionFactorN<T, Args...>>
+    : public Testable<ExpressionFactorN<T, Args...>> {};
+// ExpressionFactorN
+
+
+#if defined(GTSAM_ALLOW_DEPRECATED_SINCE_V41)
 /**
  * Binary specialization of ExpressionFactor meant as a base class for binary
- * factors. Enforces an 'expression' method with two keys, and provides 'evaluateError'.
+ * factors. Enforces an 'expression' method with two keys, and provides
- * Derived class (a binary factor!) needs to call 'initialize'.
+ * 'evaluateError'. Derived class (a binary factor!) needs to call 'initialize'.
+ *
+ * \sa ExpressionFactorN
+ * \deprecated Prefer the more general ExpressionFactorN<>.
  */
 template <typename T, typename A1, typename A2>
-class ExpressionFactor2 : public ExpressionFactor<T> {
+class ExpressionFactor2 : public ExpressionFactorN<T, A1, A2> {
- public:
+public:
   /// Destructor
-  virtual ~ExpressionFactor2() {}
+  ~ExpressionFactor2() override {}
 
   /// Backwards compatible evaluateError, to make existing tests compile
-  Vector evaluateError(const A1& a1, const A2& a2,
+  Vector evaluateError(const A1 &a1, const A2 &a2,
-                       boost::optional<Matrix&> H1 = boost::none,
+                       boost::optional<Matrix &> H1 = boost::none,
-                       boost::optional<Matrix&> H2 = boost::none) const {
+                       boost::optional<Matrix &> H2 = boost::none) const {
     Values values;
     values.insert(this->keys_[0], a1);
     values.insert(this->keys_[1], a2);
     std::vector<Matrix> H(2);
-    Vector error = this->unwhitenedError(values, H);
+    Vector error = ExpressionFactor<T>::unwhitenedError(values, H);
     if (H1) (*H1) = H[0];
     if (H2) (*H2) = H[1];
     return error;
@@ -245,35 +327,25 @@ class ExpressionFactor2 : public ExpressionFactor<T> {
   /// Recreate expression from given keys_ and measured_, used in load
   /// Needed to deserialize a derived factor
   virtual Expression<T> expression(Key key1, Key key2) const {
-    throw std::runtime_error("ExpressionFactor2::expression not provided: cannot deserialize.");
+    throw std::runtime_error(
+        "ExpressionFactor2::expression not provided: cannot deserialize.");
+  }
+  virtual Expression<T>
+  expression(const typename ExpressionFactorN<T, A1, A2>::ArrayNKeys &keys)
+      const override {
+    return expression(keys[0], keys[1]);
   }
 
- protected:
+protected:
   /// Default constructor, for serialization
   ExpressionFactor2() {}
 
   /// Constructor takes care of keys, but still need to call initialize
-  ExpressionFactor2(Key key1, Key key2,
+  ExpressionFactor2(Key key1, Key key2, const SharedNoiseModel &noiseModel,
-                                const SharedNoiseModel& noiseModel,
+                    const T &measurement)
-                                const T& measurement)
+      : ExpressionFactorN<T, A1, A2>({key1, key2}, noiseModel, measurement) {}
-      : ExpressionFactor<T>(noiseModel, measurement) {
-    this->keys_.push_back(key1);
-    this->keys_.push_back(key2);
-  }
-
- private:
-  /// Return an expression that predicts the measurement given Values
-  virtual Expression<T> expression() const {
-    return expression(this->keys_[0], this->keys_[1]);
-  }
-
-  friend class boost::serialization::access;
-  template <class ARCHIVE>
-  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
-    ar& boost::serialization::make_nvp(
-        "ExpressionFactor", boost::serialization::base_object<ExpressionFactor<T> >(*this));
-  }
 };
 // ExpressionFactor2
+#endif
 
-}// \ namespace gtsam
+} // namespace gtsam

gtsam/nonlinear/FunctorizedFactor.h

@@ -82,13 +82,13 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
   virtual ~FunctorizedFactor() {}
 
   /// @return a deep copy of this factor
-  virtual NonlinearFactor::shared_ptr clone() const {
+  NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<NonlinearFactor>(
         NonlinearFactor::shared_ptr(new FunctorizedFactor<R, T>(*this)));
   }
 
   Vector evaluateError(const T &params,
-                       boost::optional<Matrix &> H = boost::none) const {
+                       boost::optional<Matrix &> H = boost::none) const override {
     R x = func_(params, H);
     Vector error = traits<R>::Local(measured_, x);
     return error;
@@ -97,7 +97,7 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
   /// @name Testable
   /// @{
   void print(const std::string &s = "",
-             const KeyFormatter &keyFormatter = DefaultKeyFormatter) const {
+             const KeyFormatter &keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s, keyFormatter);
     std::cout << s << (s != "" ? " " : "") << "FunctorizedFactor("
               << keyFormatter(this->key()) << ")" << std::endl;
@@ -106,10 +106,9 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1<T> {
               << std::endl;
   }
 
-  virtual bool equals(const NonlinearFactor &other, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor &other, double tol = 1e-9) const override {
     const FunctorizedFactor<R, T> *e =
         dynamic_cast<const FunctorizedFactor<R, T> *>(&other);
-    const bool base = Base::equals(*e, tol);
     return e && Base::equals(other, tol) &&
            traits<R>::Equals(this->measured_, e->measured_, tol);
   }

gtsam/nonlinear/ISAM2Clique.h

@@ -51,6 +51,7 @@ class GTSAM_EXPORT ISAM2Clique
 
   /// Default constructor
   ISAM2Clique() : Base() {}
+  virtual ~ISAM2Clique() = default;
 
   /// Copy constructor, does *not* copy solution pointers as these are invalid
   /// in different trees.
@@ -85,7 +86,7 @@ class GTSAM_EXPORT ISAM2Clique
 
   /** print this node */
   void print(const std::string& s = "",
-             const KeyFormatter& formatter = DefaultKeyFormatter) const;
+             const KeyFormatter& formatter = DefaultKeyFormatter) const override;
 
   void optimizeWildfire(const KeySet& replaced, double threshold,
                         KeySet* changed, VectorValues* delta,

gtsam/nonlinear/LinearContainerFactor.h

@@ -59,10 +59,10 @@ public:
   // Testable
 
   /** print */
-  GTSAM_EXPORT void print(const std::string& s = "", const KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
+  GTSAM_EXPORT void print(const std::string& s = "", const KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const override;
 
   /** Check if two factors are equal */
-  GTSAM_EXPORT bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
+  GTSAM_EXPORT bool equals(const NonlinearFactor& f, double tol = 1e-9) const override;
 
   // NonlinearFactor
 
@@ -74,10 +74,10 @@ public:
    *
    * @return nonlinear error if linearizationPoint present, zero otherwise
    */
-  GTSAM_EXPORT double error(const Values& c) const;
+  GTSAM_EXPORT double error(const Values& c) const override;
 
   /** get the dimension of the factor: rows of linear factor */
-  GTSAM_EXPORT size_t dim() const;
+  GTSAM_EXPORT size_t dim() const override;
 
   /** Extract the linearization point used in recalculating error */
   const boost::optional<Values>& linearizationPoint() const { return linearizationPoint_; }
@@ -98,7 +98,7 @@ public:
    * TODO: better approximation of relinearization
    * TODO: switchable modes for approximation technique
    */
-  GTSAM_EXPORT GaussianFactor::shared_ptr linearize(const Values& c) const;
+  GTSAM_EXPORT GaussianFactor::shared_ptr linearize(const Values& c) const override;
 
   /**
    * Creates an anti-factor directly
@@ -116,7 +116,7 @@ public:
    *
    * Clones the underlying linear factor
    */
-  NonlinearFactor::shared_ptr clone() const {
+  NonlinearFactor::shared_ptr clone() const override {
     return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_,linearizationPoint_));
   }
 

gtsam/nonlinear/NonlinearEquality.h

@@ -107,15 +107,15 @@ public:
   /// @name Testable
   /// @{
 
-  virtual void print(const std::string& s = "",
+  void print(const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n";
     traits<VALUE>::Print(feasible_, "Feasible Point:\n");
     std::cout << "Variable Dimension: " << traits<T>::GetDimension(feasible_) << std::endl;
   }
 
   /** Check if two factors are equal */
-  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
     const This* e = dynamic_cast<const This*>(&f);
     return e && Base::equals(f) && traits<T>::Equals(feasible_,e->feasible_, tol)
         && std::abs(error_gain_ - e->error_gain_) < tol;
@@ -126,7 +126,7 @@ public:
   /// @{
 
   /** actual error function calculation */
-  virtual double error(const Values& c) const {
+  double error(const Values& c) const override {
     const T& xj = c.at<T>(this->key());
     Vector e = this->unwhitenedError(c);
     if (allow_error_ || !compare_(xj, feasible_)) {
@@ -138,7 +138,7 @@ public:
 
   /** error function */
   Vector evaluateError(const T& xj,
-      boost::optional<Matrix&> H = boost::none) const {
+      boost::optional<Matrix&> H = boost::none) const override {
     const size_t nj = traits<T>::GetDimension(feasible_);
     if (allow_error_) {
       if (H)
@@ -158,7 +158,7 @@ public:
   }
 
   // Linearize is over-written, because base linearization tries to whiten
-  virtual GaussianFactor::shared_ptr linearize(const Values& x) const {
+  GaussianFactor::shared_ptr linearize(const Values& x) const override {
     const T& xj = x.at<T>(this->key());
     Matrix A;
     Vector b = evaluateError(xj, A);
@@ -168,7 +168,7 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
@@ -242,14 +242,14 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   /** g(x) with optional derivative */
   Vector evaluateError(const X& x1,
-      boost::optional<Matrix&> H = boost::none) const {
+      boost::optional<Matrix&> H = boost::none) const override {
     if (H)
       (*H) = Matrix::Identity(traits<X>::GetDimension(x1),traits<X>::GetDimension(x1));
     // manifold equivalent of h(x)-z -> log(z,h(x))
@@ -257,8 +257,8 @@ public:
   }
 
   /** Print */
-  virtual void print(const std::string& s = "",
+  void print(const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key())
         << ")," << "\n";
     this->noiseModel_->print();
@@ -317,14 +317,14 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   /** g(x) with optional derivative2 */
   Vector evaluateError(const X& x1, const X& x2, boost::optional<Matrix&> H1 =
-      boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+      boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
     static const size_t p = traits<X>::dimension;
     if (H1) *H1 = -Matrix::Identity(p,p);
     if (H2) *H2 =  Matrix::Identity(p,p);

gtsam/nonlinear/NonlinearFactor.h

@@ -188,14 +188,14 @@ protected:
 public:
 
   /** Print */
-  virtual void print(const std::string& s = "",
+  void print(const std::string& s = "",
-    const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+    const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
   /** Check if two factors are equal */
-  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor& f, double tol = 1e-9) const override;
 
   /** get the dimension of the factor (number of rows on linearization) */
-  virtual size_t dim() const {
+  size_t dim() const override {
     return noiseModel_->dim();
   }
 
@@ -235,14 +235,14 @@ public:
    * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
    * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
    */
-  virtual double error(const Values& c) const;
+  double error(const Values& c) const override;
 
   /**
    * Linearize a non-linearFactorN to get a GaussianFactor,
    * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
    * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
    */
-  boost::shared_ptr<GaussianFactor> linearize(const Values& x) const;
+  boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override;
 
  private:
   /** Serialization function */
@@ -300,7 +300,7 @@ public:
   /** Calls the 1-key specific version of evaluateError, which is pure virtual
    *  so must be implemented in the derived class.
    */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
     if(this->active(x)) {
       const X& x1 = x.at<X>(keys_[0]);
       if(H) {
@@ -374,7 +374,7 @@ public:
 
   /** Calls the 2-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
     if(this->active(x)) {
       const X1& x1 = x.at<X1>(keys_[0]);
       const X2& x2 = x.at<X2>(keys_[1]);
@@ -452,7 +452,7 @@ public:
 
   /** Calls the 3-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
     if(this->active(x)) {
       if(H)
         return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), (*H)[0], (*H)[1], (*H)[2]);
@@ -532,7 +532,7 @@ public:
 
   /** Calls the 4-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
     if(this->active(x)) {
       if(H)
         return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
@@ -616,7 +616,7 @@ public:
 
   /** Calls the 5-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
     if(this->active(x)) {
       if(H)
         return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
@@ -704,7 +704,7 @@ public:
 
   /** Calls the 6-key specific version of evaluateError, which is pure virtual
    * so must be implemented in the derived class. */
-  virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
+  Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const override {
     if(this->active(x)) {
       if(H)
         return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);

gtsam/nonlinear/PriorFactor.h

@@ -65,15 +65,15 @@ namespace gtsam {
     }
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    gtsam::NonlinearFactor::shared_ptr clone() const override {
       return boost::static_pointer_cast<gtsam::NonlinearFactor>(
           gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
     /** implement functions needed for Testable */
 
     /** print */
-    virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
+    void print(const std::string& s,
-                                                 DefaultKeyFormatter) const {
+       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
       traits<T>::Print(prior_, "  prior mean: ");
       if (this->noiseModel_)
@@ -83,7 +83,7 @@ namespace gtsam {
     }
 
     /** equals */
-    virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+    bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
       const This* e = dynamic_cast<const This*> (&expected);
       return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(prior_, e->prior_, tol);
     }
@@ -91,7 +91,7 @@ namespace gtsam {
     /** implement functions needed to derive from Factor */
 
     /** vector of errors */
-    Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const {
+    Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const override {
       if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
       // manifold equivalent of z-x -> Local(x,z)
       // TODO(ASL) Add Jacobians.
@@ -117,4 +117,9 @@ namespace gtsam {
 	GTSAM_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
   };
 
+  /// traits
+  template<class VALUE>
+  struct traits<PriorFactor<VALUE> > : public Testable<PriorFactor<VALUE> > {};
+
+
 } /// namespace gtsam

gtsam/nonlinear/Values.cpp

@@ -52,6 +52,12 @@ namespace gtsam {
   Values::Values(Values&& other) : values_(std::move(other.values_)) {
   }
 
+  /* ************************************************************************* */
+  Values::Values(std::initializer_list<ConstKeyValuePair> init) {
+    for (const auto &kv : init)
+      insert(kv.key, kv.value);
+  }
+
   /* ************************************************************************* */
   Values::Values(const Values& other, const VectorValues& delta) {
     for (const_iterator key_value = other.begin(); key_value != other.end(); ++key_value) {
@@ -214,7 +220,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  const char* ValuesKeyAlreadyExists::what() const throw() {
+  const char* ValuesKeyAlreadyExists::what() const noexcept {
     if(message_.empty())
       message_ =
           "Attempting to add a key-value pair with key \"" + DefaultKeyFormatter(key_) + "\", key already exists.";
@@ -222,7 +228,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  const char* ValuesKeyDoesNotExist::what() const throw() {
+  const char* ValuesKeyDoesNotExist::what() const noexcept {
     if(message_.empty())
       message_ =
           "Attempting to " + std::string(operation_) + " the key \"" +
@@ -231,7 +237,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  const char* ValuesIncorrectType::what() const throw() {
+  const char* ValuesIncorrectType::what() const noexcept {
     if(message_.empty()) {
       std::string storedTypeName = demangle(storedTypeId_.name());
       std::string requestedTypeName = demangle(requestedTypeId_.name());
@@ -251,7 +257,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  const char* NoMatchFoundForFixed::what() const throw() {
+  const char* NoMatchFoundForFixed::what() const noexcept {
     if(message_.empty()) {
       ostringstream oss;
     oss

gtsam/nonlinear/Values.h

@@ -149,6 +149,13 @@ namespace gtsam {
 
     /** Move constructor */
     Values(Values&& other);
+    
+    /** Constructor from initializer list. Example usage:
+     * \code
+     * Values v = {{k1, genericValue(pose1)}, {k2, genericValue(point2)}};
+     * \endcode
+     */
+    Values(std::initializer_list<ConstKeyValuePair> init);
 
     /** Construct from a Values and an update vector: identical to other.retract(delta) */
     Values(const Values& other, const VectorValues& delta);
@@ -432,16 +439,16 @@ namespace gtsam {
 
   public:
     /// Construct with the key-value pair attempted to be added
-    ValuesKeyAlreadyExists(Key key) throw() :
+    ValuesKeyAlreadyExists(Key key) noexcept :
       key_(key) {}
 
-    virtual ~ValuesKeyAlreadyExists() throw() {}
+    virtual ~ValuesKeyAlreadyExists() noexcept {}
 
     /// The duplicate key that was attempted to be added
-    Key key() const throw() { return key_; }
+    Key key() const noexcept { return key_; }
 
     /// The message to be displayed to the user
-    GTSAM_EXPORT virtual const char* what() const throw();
+    GTSAM_EXPORT const char* what() const noexcept override;
   };
 
   /* ************************************************************************* */
@@ -455,16 +462,16 @@ namespace gtsam {
 
   public:
     /// Construct with the key that does not exist in the values
-    ValuesKeyDoesNotExist(const char* operation, Key key) throw() :
+    ValuesKeyDoesNotExist(const char* operation, Key key) noexcept :
       operation_(operation), key_(key) {}
 
-    virtual ~ValuesKeyDoesNotExist() throw() {}
+    virtual ~ValuesKeyDoesNotExist() noexcept {}
 
     /// The key that was attempted to be accessed that does not exist
-    Key key() const throw() { return key_; }
+    Key key() const noexcept { return key_; }
 
     /// The message to be displayed to the user
-    GTSAM_EXPORT virtual const char* what() const throw();
+    GTSAM_EXPORT const char* what() const noexcept override;
   };
 
   /* ************************************************************************* */
@@ -480,13 +487,13 @@ namespace gtsam {
   public:
     /// Construct with the key that does not exist in the values
     ValuesIncorrectType(Key key,
-        const std::type_info& storedTypeId, const std::type_info& requestedTypeId) throw() :
+        const std::type_info& storedTypeId, const std::type_info& requestedTypeId) noexcept :
       key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {}
 
-    virtual ~ValuesIncorrectType() throw() {}
+    virtual ~ValuesIncorrectType() noexcept {}
 
     /// The key that was attempted to be accessed that does not exist
-    Key key() const throw() { return key_; }
+    Key key() const noexcept { return key_; }
 
     /// The typeid of the value stores in the Values
     const std::type_info& storedTypeId() const { return storedTypeId_; }
@@ -495,18 +502,18 @@ namespace gtsam {
     const std::type_info& requestedTypeId() const { return requestedTypeId_; }
 
     /// The message to be displayed to the user
-    GTSAM_EXPORT virtual const char* what() const throw();
+    GTSAM_EXPORT const char* what() const noexcept override;
   };
 
   /* ************************************************************************* */
   class DynamicValuesMismatched : public std::exception {
 
   public:
-    DynamicValuesMismatched() throw() {}
+    DynamicValuesMismatched() noexcept {}
 
-    virtual ~DynamicValuesMismatched() throw() {}
+    virtual ~DynamicValuesMismatched() noexcept {}
 
-    virtual const char* what() const throw() {
+    const char* what() const noexcept override {
       return "The Values 'this' and the argument passed to Values::localCoordinates have mismatched keys and values";
     }
   };
@@ -522,14 +529,14 @@ namespace gtsam {
     mutable std::string message_;
 
   public:
-    NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) throw () :
+    NoMatchFoundForFixed(size_t M1, size_t N1, size_t M2, size_t N2) noexcept :
         M1_(M1), N1_(N1), M2_(M2), N2_(N2) {
     }
 
-    virtual ~NoMatchFoundForFixed() throw () {
+    virtual ~NoMatchFoundForFixed() noexcept {
     }
 
-    GTSAM_EXPORT virtual const char* what() const throw ();
+    GTSAM_EXPORT const char* what() const noexcept override;
   };
 
   /* ************************************************************************* */

gtsam/nonlinear/WhiteNoiseFactor.h

@@ -109,7 +109,7 @@ namespace gtsam {
 
     /// Print
     void print(const std::string& p = "WhiteNoiseFactor",
-        const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+        const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
       Base::print(p, keyFormatter);
       std::cout << p + ".z: " << z_ << std::endl;
     }
@@ -119,12 +119,12 @@ namespace gtsam {
     /// @{
 
     /// get the dimension of the factor (number of rows on linearization)
-    virtual size_t dim() const {
+    size_t dim() const override {
       return 2;
     }
 
     /// Calculate the error of the factor, typically equal to log-likelihood
-    inline double error(const Values& x) const {
+    double error(const Values& x) const override {
       return f(z_, x.at<double>(meanKey_), x.at<double>(precisionKey_));
     }
 
@@ -153,7 +153,7 @@ namespace gtsam {
     /// @{
 
     /// linearize returns a Hessianfactor that is an approximation of error(p)
-    virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
+    boost::shared_ptr<GaussianFactor> linearize(const Values& x) const override {
       double u = x.at<double>(meanKey_);
       double p = x.at<double>(precisionKey_);
       Key j1 = meanKey_;
@@ -163,7 +163,7 @@ namespace gtsam {
 
     // TODO: Frank commented this out for now, can it go?
     //    /// @return a deep copy of this factor
-    //    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    //    gtsam::NonlinearFactor::shared_ptr clone() const override {
     //      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
     //          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 

gtsam/nonlinear/internal/CallRecord.h

@@ -144,43 +144,43 @@ private:
     return static_cast<const Derived&>(*this);
   }
 
-  virtual void _print(const std::string& indent) const {
+  void _print(const std::string& indent) const override {
     derived().print(indent);
   }
 
   // Called from base class non-virtual inline method startReverseAD2
   // Calls non-virtual function startReverseAD4, implemented in Derived (ExpressionNode::Record)
-  virtual void _startReverseAD3(JacobianMap& jacobians) const {
+  void _startReverseAD3(JacobianMap& jacobians) const override {
     derived().startReverseAD4(jacobians);
   }
 
-  virtual void _reverseAD3(const Matrix & dFdT, JacobianMap& jacobians) const {
+  void _reverseAD3(const Matrix & dFdT, JacobianMap& jacobians) const override {
     derived().reverseAD4(dFdT, jacobians);
   }
 
-  virtual void _reverseAD3(
+  void _reverseAD3(
       const Eigen::Matrix<double, Eigen::Dynamic, Cols> & dFdT,
-      JacobianMap& jacobians) const {
+      JacobianMap& jacobians) const override {
     derived().reverseAD4(dFdT, jacobians);
   }
-  virtual void _reverseAD3(const Eigen::Matrix<double, 1, Cols> & dFdT,
+  void _reverseAD3(const Eigen::Matrix<double, 1, Cols> & dFdT,
-      JacobianMap& jacobians) const {
+      JacobianMap& jacobians) const override {
     derived().reverseAD4(dFdT, jacobians);
   }
-  virtual void _reverseAD3(const Eigen::Matrix<double, 2, Cols> & dFdT,
+  void _reverseAD3(const Eigen::Matrix<double, 2, Cols> & dFdT,
-      JacobianMap& jacobians) const {
+      JacobianMap& jacobians) const override {
     derived().reverseAD4(dFdT, jacobians);
   }
-  virtual void _reverseAD3(const Eigen::Matrix<double, 3, Cols> & dFdT,
+  void _reverseAD3(const Eigen::Matrix<double, 3, Cols> & dFdT,
-      JacobianMap& jacobians) const {
+      JacobianMap& jacobians) const override {
     derived().reverseAD4(dFdT, jacobians);
   }
-  virtual void _reverseAD3(const Eigen::Matrix<double, 4, Cols> & dFdT,
+  void _reverseAD3(const Eigen::Matrix<double, 4, Cols> & dFdT,
-      JacobianMap& jacobians) const {
+      JacobianMap& jacobians) const override {
     derived().reverseAD4(dFdT, jacobians);
   }
-  virtual void _reverseAD3(const Eigen::Matrix<double, 5, Cols> & dFdT,
+  void _reverseAD3(const Eigen::Matrix<double, 5, Cols> & dFdT,
-      JacobianMap& jacobians) const {
+      JacobianMap& jacobians) const override {
     derived().reverseAD4(dFdT, jacobians);
   }
 };

gtsam/nonlinear/internal/ExpressionNode.h

@@ -135,18 +135,18 @@ public:
   }
 
   /// Print
-  virtual void print(const std::string& indent = "") const {
+  void print(const std::string& indent = "") const override {
     std::cout << indent << "Constant" << std::endl;
   }
 
   /// Return value
-  virtual T value(const Values& values) const {
+  T value(const Values& values) const override {
     return constant_;
   }
 
   /// Construct an execution trace for reverse AD
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+  T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-      ExecutionTraceStorage* traceStorage) const {
+      ExecutionTraceStorage* traceStorage) const override {
     return constant_;
   }
 
@@ -176,30 +176,30 @@ public:
   }
 
   /// Print
-  virtual void print(const std::string& indent = "") const {
+  void print(const std::string& indent = "") const override {
     std::cout << indent << "Leaf, key = " << DefaultKeyFormatter(key_) << std::endl;
   }
 
   /// Return keys that play in this expression
-  virtual std::set<Key> keys() const {
+  std::set<Key> keys() const override {
     std::set<Key> keys;
     keys.insert(key_);
     return keys;
   }
 
   /// Return dimensions for each argument
-  virtual void dims(std::map<Key, int>& map) const {
+  void dims(std::map<Key, int>& map) const override {
     map[key_] = traits<T>::dimension;
   }
 
   /// Return value
-  virtual T value(const Values& values) const {
+  T value(const Values& values) const override {
     return values.at<T>(key_);
   }
 
   /// Construct an execution trace for reverse AD
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+  T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-      ExecutionTraceStorage* traceStorage) const {
+      ExecutionTraceStorage* traceStorage) const override {
     trace.setLeaf(key_);
     return values.at<T>(key_);
   }
@@ -248,23 +248,23 @@ public:
   }
 
   /// Print
-  virtual void print(const std::string& indent = "") const {
+  void print(const std::string& indent = "") const override {
     std::cout << indent << "UnaryExpression" << std::endl;
     expression1_->print(indent + "  ");
   }
 
   /// Return value
-  virtual T value(const Values& values) const {
+  T value(const Values& values) const override {
     return function_(expression1_->value(values), boost::none);
   }
 
   /// Return keys that play in this expression
-  virtual std::set<Key> keys() const {
+  std::set<Key> keys() const override {
     return expression1_->keys();
   }
 
   /// Return dimensions for each argument
-  virtual void dims(std::map<Key, int>& map) const {
+  void dims(std::map<Key, int>& map) const override {
     expression1_->dims(map);
   }
 
@@ -307,8 +307,8 @@ public:
   };
 
   /// Construct an execution trace for reverse AD
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+  T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-      ExecutionTraceStorage* ptr) const {
+      ExecutionTraceStorage* ptr) const override {
     assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
 
     // Create a Record in the memory pointed to by ptr
@@ -357,21 +357,21 @@ public:
   }
 
   /// Print
-  virtual void print(const std::string& indent = "") const {
+  void print(const std::string& indent = "") const override {
     std::cout << indent << "BinaryExpression" << std::endl;
     expression1_->print(indent + "  ");
     expression2_->print(indent + "  ");
   }
 
   /// Return value
-  virtual T value(const Values& values) const {
+  T value(const Values& values) const override {
     using boost::none;
     return function_(expression1_->value(values), expression2_->value(values),
         none, none);
   }
 
   /// Return keys that play in this expression
-  virtual std::set<Key> keys() const {
+  std::set<Key> keys() const override {
     std::set<Key> keys = expression1_->keys();
     std::set<Key> myKeys = expression2_->keys();
     keys.insert(myKeys.begin(), myKeys.end());
@@ -379,7 +379,7 @@ public:
   }
 
   /// Return dimensions for each argument
-  virtual void dims(std::map<Key, int>& map) const {
+  void dims(std::map<Key, int>& map) const override {
     expression1_->dims(map);
     expression2_->dims(map);
   }
@@ -426,8 +426,8 @@ public:
   };
 
   /// Construct an execution trace for reverse AD, see UnaryExpression for explanation
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+  T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-      ExecutionTraceStorage* ptr) const {
+      ExecutionTraceStorage* ptr) const override {
     assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
     Record* record = new (ptr) Record(values, *expression1_, *expression2_, ptr);
     trace.setFunction(record);
@@ -464,7 +464,7 @@ public:
   }
 
   /// Print
-  virtual void print(const std::string& indent = "") const {
+  void print(const std::string& indent = "") const override {
     std::cout << indent << "TernaryExpression" << std::endl;
     expression1_->print(indent + "  ");
     expression2_->print(indent + "  ");
@@ -472,14 +472,14 @@ public:
   }
 
   /// Return value
-  virtual T value(const Values& values) const {
+  T value(const Values& values) const override {
     using boost::none;
     return function_(expression1_->value(values), expression2_->value(values),
         expression3_->value(values), none, none, none);
   }
 
   /// Return keys that play in this expression
-  virtual std::set<Key> keys() const {
+  std::set<Key> keys() const override {
     std::set<Key> keys = expression1_->keys();
     std::set<Key> myKeys = expression2_->keys();
     keys.insert(myKeys.begin(), myKeys.end());
@@ -489,7 +489,7 @@ public:
   }
 
   /// Return dimensions for each argument
-  virtual void dims(std::map<Key, int>& map) const {
+  void dims(std::map<Key, int>& map) const override {
     expression1_->dims(map);
     expression2_->dims(map);
     expression3_->dims(map);
@@ -544,8 +544,8 @@ public:
   };
 
   /// Construct an execution trace for reverse AD, see UnaryExpression for explanation
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+  T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-                           ExecutionTraceStorage* ptr) const {
+                           ExecutionTraceStorage* ptr) const override {
     assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
     Record* record = new (ptr) Record(values, *expression1_, *expression2_, *expression3_, ptr);
     trace.setFunction(record);
@@ -574,23 +574,23 @@ class ScalarMultiplyNode : public ExpressionNode<T> {
   virtual ~ScalarMultiplyNode() {}
 
   /// Print
-  virtual void print(const std::string& indent = "") const {
+  void print(const std::string& indent = "") const override {
     std::cout << indent << "ScalarMultiplyNode" << std::endl;
     expression_->print(indent + "  ");
   }
 
   /// Return value
-  virtual T value(const Values& values) const {
+  T value(const Values& values) const override {
     return scalar_ * expression_->value(values);
   }
 
   /// Return keys that play in this expression
-  virtual std::set<Key> keys() const {
+  std::set<Key> keys() const override {
     return expression_->keys();
   }
 
   /// Return dimensions for each argument
-  virtual void dims(std::map<Key, int>& map) const {
+  void dims(std::map<Key, int>& map) const override {
     expression_->dims(map);
   }
 
@@ -624,8 +624,8 @@ class ScalarMultiplyNode : public ExpressionNode<T> {
   };
 
   /// Construct an execution trace for reverse AD
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+  T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-                           ExecutionTraceStorage* ptr) const {
+                           ExecutionTraceStorage* ptr) const override {
     assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
     Record* record = new (ptr) Record();
     ptr += upAligned(sizeof(Record));
@@ -662,19 +662,19 @@ class BinarySumNode : public ExpressionNode<T> {
   virtual ~BinarySumNode() {}
 
   /// Print
-  virtual void print(const std::string& indent = "") const {
+  void print(const std::string& indent = "") const override {
     std::cout << indent << "BinarySumNode" << std::endl;
     expression1_->print(indent + "  ");
     expression2_->print(indent + "  ");
   }
 
   /// Return value
-  virtual T value(const Values& values) const {
+  T value(const Values& values) const override {
     return expression1_->value(values) + expression2_->value(values);
   }
 
   /// Return keys that play in this expression
-  virtual std::set<Key> keys() const {
+  std::set<Key> keys() const override {
     std::set<Key> keys = expression1_->keys();
     std::set<Key> myKeys = expression2_->keys();
     keys.insert(myKeys.begin(), myKeys.end());
@@ -682,7 +682,7 @@ class BinarySumNode : public ExpressionNode<T> {
   }
 
   /// Return dimensions for each argument
-  virtual void dims(std::map<Key, int>& map) const {
+  void dims(std::map<Key, int>& map) const override {
     expression1_->dims(map);
     expression2_->dims(map);
   }
@@ -717,8 +717,8 @@ class BinarySumNode : public ExpressionNode<T> {
   };
 
   /// Construct an execution trace for reverse AD
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+  T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-                           ExecutionTraceStorage* ptr) const {
+                           ExecutionTraceStorage* ptr) const override {
     assert(reinterpret_cast<size_t>(ptr) % TraceAlignment == 0);
     Record* record = new (ptr) Record();
     trace.setFunction(record);

gtsam/nonlinear/nonlinearExceptions.h

@@ -35,11 +35,11 @@ namespace gtsam {
     KeyFormatter formatter_;
     mutable std::string what_;
   public:
-    MarginalizeNonleafException(Key key, KeyFormatter formatter = DefaultKeyFormatter) throw() :
+    MarginalizeNonleafException(Key key, KeyFormatter formatter = DefaultKeyFormatter) noexcept :
       key_(key), formatter_(formatter) {}
-    virtual ~MarginalizeNonleafException() throw() {}
+    virtual ~MarginalizeNonleafException() noexcept {}
     Key key() const { return key_; }
-    virtual const char* what() const throw() {
+    const char* what() const noexcept override {
       if(what_.empty())
         what_ =
 "\nRequested to marginalize out variable " + formatter_(key_) + ", but this variable\n\

gtsam/nonlinear/tests/testCallRecord.cpp

@@ -98,7 +98,8 @@ struct Record: public internal::CallRecordImplementor<Record, Cols> {
   friend struct internal::CallRecordImplementor;
 };
 
-internal::JacobianMap & NJM= *static_cast<internal::JacobianMap *>(nullptr);
+internal::JacobianMap* NJM_ptr = static_cast<internal::JacobianMap *>(nullptr);
+internal::JacobianMap & NJM = *NJM_ptr;
 
 /* ************************************************************************* */
 typedef Eigen::Matrix<double, Eigen::Dynamic, Cols> DynRowMat;

gtsam/nonlinear/tests/testFunctorizedFactor.cpp

@@ -131,7 +131,7 @@ TEST(FunctorizedFactor, Print) {
       "FunctorizedFactor(X0)\n"
       "  measurement: [\n"
       "	1, 0;\n"
-      " 	0, 1\n"
+      "	0, 1\n"
       "]\n"
       "  noise model sigmas: 1 1 1 1 1 1 1 1 1\n";
 

gtsam/nonlinear/tests/testValues.cpp

@@ -606,6 +606,36 @@ TEST(Values, Demangle) {
   EXPECT(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
+TEST(Values, brace_initializer) {
+  const Pose2 poseA(1.0, 2.0, 0.3), poseC(.0, .0, .0);
+  const Pose3 poseB(Pose2(0.1, 0.2, 0.3));
+
+  {
+    Values values;
+    EXPECT_LONGS_EQUAL(0, values.size());
+    values = { {key1, genericValue(1.0)} };
+    EXPECT_LONGS_EQUAL(1, values.size());
+    CHECK(values.at<double>(key1) == 1.0);
+  }
+  {
+    Values values = { {key1, genericValue(poseA)}, {key2, genericValue(poseB)} };
+    EXPECT_LONGS_EQUAL(2, values.size());
+    EXPECT(assert_equal(values.at<Pose2>(key1), poseA));
+    EXPECT(assert_equal(values.at<Pose3>(key2), poseB));
+  }
+  // Test exception: duplicated key:
+  {
+    Values values;
+    CHECK_EXCEPTION((values = {
+      {key1, genericValue(poseA)},
+      {key2, genericValue(poseB)},
+      {key1, genericValue(poseC)} 
+      }), std::exception);
+  }
+}
+
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/sam/BearingFactor.h

@@ -34,8 +34,8 @@ struct Bearing;
  */
 template <typename A1, typename A2,
           typename T = typename Bearing<A1, A2>::result_type>
-struct BearingFactor : public ExpressionFactor2<T, A1, A2> {
+struct BearingFactor : public ExpressionFactorN<T, A1, A2> {
-  typedef ExpressionFactor2<T, A1, A2> Base;
+  typedef ExpressionFactorN<T, A1, A2> Base;
 
   /// default constructor
   BearingFactor() {}
@@ -43,23 +43,38 @@ struct BearingFactor : public ExpressionFactor2<T, A1, A2> {
   /// primary constructor
   BearingFactor(Key key1, Key key2, const T& measured,
                 const SharedNoiseModel& model)
-      : Base(key1, key2, model, measured) {
+      : Base({key1, key2}, model, measured) {
-    this->initialize(expression(key1, key2));
+    this->initialize(expression({key1, key2}));
   }
 
   // Return measurement expression
-  virtual Expression<T> expression(Key key1, Key key2) const {
+  Expression<T> expression(const typename Base::ArrayNKeys &keys) const override {
-    Expression<A1> a1_(key1);
+    Expression<A1> a1_(keys[0]);
-    Expression<A2> a2_(key2);
+    Expression<A2> a2_(keys[1]);
     return Expression<T>(Bearing<A1, A2>(), a1_, a2_);
   }
 
   /// print
   void print(const std::string& s = "",
-             const KeyFormatter& kf = DefaultKeyFormatter) const {
+             const KeyFormatter& kf = DefaultKeyFormatter) const override {
     std::cout << s << "BearingFactor" << std::endl;
     Base::print(s, kf);
   }
+  
+  Vector evaluateError(const A1& a1, const A2& a2,
+    boost::optional<Matrix&> H1 = boost::none,
+    boost::optional<Matrix&> H2 = boost::none) const
+  {
+    std::vector<Matrix> Hs(2);
+    const auto &keys = Factor::keys();
+    const Vector error = unwhitenedError(
+      {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, 
+      Hs);
+    if (H1) *H1 = Hs[0];
+    if (H2) *H2 = Hs[1];
+    return error;
+  }
+
 
  private:
   friend class boost::serialization::access;

gtsam/sam/BearingRangeFactor.h

@@ -33,10 +33,10 @@ template <typename A1, typename A2,
           typename B = typename Bearing<A1, A2>::result_type,
           typename R = typename Range<A1, A2>::result_type>
 class BearingRangeFactor
-    : public ExpressionFactor2<BearingRange<A1, A2>, A1, A2> {
+    : public ExpressionFactorN<BearingRange<A1, A2>, A1, A2> {
  private:
   typedef BearingRange<A1, A2> T;
-  typedef ExpressionFactor2<T, A1, A2> Base;
+  typedef ExpressionFactorN<T, A1, A2> Base;
   typedef BearingRangeFactor<A1, A2> This;
 
  public:
@@ -48,27 +48,41 @@ class BearingRangeFactor
   /// primary constructor
   BearingRangeFactor(Key key1, Key key2, const B& measuredBearing,
                      const R& measuredRange, const SharedNoiseModel& model)
-      : Base(key1, key2, model, T(measuredBearing, measuredRange)) {
+      : Base({key1, key2}, model, T(measuredBearing, measuredRange)) {
-    this->initialize(expression(key1, key2));
+    this->initialize(expression({key1, key2}));
   }
 
   virtual ~BearingRangeFactor() {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   // Return measurement expression
-  virtual Expression<T> expression(Key key1, Key key2) const {
+  Expression<T> expression(const typename Base::ArrayNKeys& keys) const override {
-    return Expression<T>(T::Measure, Expression<A1>(key1),
+    return Expression<T>(T::Measure, Expression<A1>(keys[0]),
-                         Expression<A2>(key2));
+                         Expression<A2>(keys[1]));
+  }
+
+  Vector evaluateError(const A1& a1, const A2& a2,
+      boost::optional<Matrix&> H1 = boost::none,
+      boost::optional<Matrix&> H2 = boost::none) const
+  {
+    std::vector<Matrix> Hs(2);
+    const auto &keys = Factor::keys();
+    const Vector error = unwhitenedError(
+      {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, 
+      Hs);
+    if (H1) *H1 = Hs[0];
+    if (H2) *H2 = Hs[1];
+    return error;
   }
 
   /// print
-  virtual void print(const std::string& s = "",
+  void print(const std::string& s = "",
-                     const KeyFormatter& kf = DefaultKeyFormatter) const {
+                     const KeyFormatter& kf = DefaultKeyFormatter) const override {
     std::cout << s << "BearingRangeFactor" << std::endl;
     Base::print(s, kf);
   }

gtsam/sam/RangeFactor.h

@@ -32,36 +32,50 @@ struct Range;
  * @addtogroup SAM
  */
 template <typename A1, typename A2 = A1, typename T = double>
-class RangeFactor : public ExpressionFactor2<T, A1, A2> {
+class RangeFactor : public ExpressionFactorN<T, A1, A2> {
  private:
   typedef RangeFactor<A1, A2> This;
-  typedef ExpressionFactor2<T, A1, A2> Base;
+  typedef ExpressionFactorN<T, A1, A2> Base;
 
  public:
   /// default constructor
   RangeFactor() {}
 
   RangeFactor(Key key1, Key key2, T measured, const SharedNoiseModel& model)
-      : Base(key1, key2, model, measured) {
+      : Base({key1, key2}, model, measured) {
-    this->initialize(expression(key1, key2));
+    this->initialize(expression({key1, key2}));
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   // Return measurement expression
-  virtual Expression<T> expression(Key key1, Key key2) const {
+  Expression<T> expression(const typename Base::ArrayNKeys& keys) const override {
-    Expression<A1> a1_(key1);
+    Expression<A1> a1_(keys[0]);
-    Expression<A2> a2_(key2);
+    Expression<A2> a2_(keys[1]);
     return Expression<T>(Range<A1, A2>(), a1_, a2_);
   }
+  
+  Vector evaluateError(const A1& a1, const A2& a2,
+      boost::optional<Matrix&> H1 = boost::none,
+      boost::optional<Matrix&> H2 = boost::none) const
+  {
+    std::vector<Matrix> Hs(2);
+    const auto &keys = Factor::keys();
+    const Vector error = Base::unwhitenedError(
+      {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, 
+      Hs);
+    if (H1) *H1 = Hs[0];
+    if (H2) *H2 = Hs[1];
+    return error;
+  }
 
   /// print
   void print(const std::string& s = "",
-             const KeyFormatter& kf = DefaultKeyFormatter) const {
+             const KeyFormatter& kf = DefaultKeyFormatter) const override {
     std::cout << s << "RangeFactor" << std::endl;
     Base::print(s, kf);
   }
@@ -86,10 +100,10 @@ struct traits<RangeFactor<A1, A2, T> >
  */
 template <typename A1, typename A2 = A1,
           typename T = typename Range<A1, A2>::result_type>
-class RangeFactorWithTransform : public ExpressionFactor2<T, A1, A2> {
+class RangeFactorWithTransform : public ExpressionFactorN<T, A1, A2> {
  private:
   typedef RangeFactorWithTransform<A1, A2> This;
-  typedef ExpressionFactor2<T, A1, A2> Base;
+  typedef ExpressionFactorN<T, A1, A2> Base;
 
   A1 body_T_sensor_;  ///< The pose of the sensor in the body frame
 
@@ -100,31 +114,45 @@ class RangeFactorWithTransform : public ExpressionFactor2<T, A1, A2> {
   RangeFactorWithTransform(Key key1, Key key2, T measured,
                            const SharedNoiseModel& model,
                            const A1& body_T_sensor)
-      : Base(key1, key2, model, measured), body_T_sensor_(body_T_sensor) {
+      : Base({key1, key2}, model, measured), body_T_sensor_(body_T_sensor) {
-    this->initialize(expression(key1, key2));
+    this->initialize(expression({key1, key2}));
   }
 
   virtual ~RangeFactorWithTransform() {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   // Return measurement expression
-  virtual Expression<T> expression(Key key1, Key key2) const {
+  Expression<T> expression(const typename Base::ArrayNKeys& keys) const override {
     Expression<A1> body_T_sensor__(body_T_sensor_);
-    Expression<A1> nav_T_body_(key1);
+    Expression<A1> nav_T_body_(keys[0]);
     Expression<A1> nav_T_sensor_(traits<A1>::Compose, nav_T_body_,
                                  body_T_sensor__);
-    Expression<A2> a2_(key2);
+    Expression<A2> a2_(keys[1]);
     return Expression<T>(Range<A1, A2>(), nav_T_sensor_, a2_);
   }
 
+  Vector evaluateError(const A1& a1, const A2& a2,
+      boost::optional<Matrix&> H1 = boost::none,
+      boost::optional<Matrix&> H2 = boost::none) const
+  {
+    std::vector<Matrix> Hs(2);
+    const auto &keys = Factor::keys();
+    const Vector error = Base::unwhitenedError(
+      {{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, 
+      Hs);
+    if (H1) *H1 = Hs[0];
+    if (H2) *H2 = Hs[1];
+    return error;
+  }
+
   /** print contents */
   void print(const std::string& s = "",
-             const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+             const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << "RangeFactorWithTransform" << std::endl;
     this->body_T_sensor_.print("  sensor pose in body frame: ");
     Base::print(s, keyFormatter);
@@ -135,9 +163,12 @@ class RangeFactorWithTransform : public ExpressionFactor2<T, A1, A2> {
   friend class boost::serialization::access;
   template <typename ARCHIVE>
   void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
+    // **IMPORTANT** We need to (de)serialize parameters before the base class,
+    // since it calls expression() and we need all parameters ready at that
+    // point.
+    ar& BOOST_SERIALIZATION_NVP(body_T_sensor_);
     ar& boost::serialization::make_nvp(
         "Base", boost::serialization::base_object<Base>(*this));
-    ar& BOOST_SERIALIZATION_NVP(body_T_sensor_);
   }
 };  // \ RangeFactorWithTransform
 

gtsam/sam/tests/testBearingFactor.cpp

@@ -68,7 +68,7 @@ TEST(BearingFactor, 2D) {
   values.insert(poseKey, Pose2(1.0, 2.0, 0.57));
   values.insert(pointKey, Point2(-4.0, 11.0));
 
-  EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey),
+  EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}),
                                       values, 1e-7, 1e-5);
   EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
 }
@@ -104,7 +104,7 @@ TEST(BearingFactor, Serialization3D) {
 //  values.insert(poseKey, Pose3());
 //  values.insert(pointKey, Point3(1, 0, 0));
 //
-//  EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey),
+//  EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}),
 //                                      values, 1e-7, 1e-5);
 //  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
 //}

gtsam/sam/tests/testBearingRangeFactor.cpp

@@ -67,7 +67,7 @@ TEST(BearingRangeFactor, 2D) {
   values.insert(poseKey, Pose2(1.0, 2.0, 0.57));
   values.insert(pointKey, Point2(-4.0, 11.0));
 
-  EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey),
+  EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}),
                                       values, 1e-7, 1e-5);
   EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
 }
@@ -95,7 +95,7 @@ TEST(BearingRangeFactor, Serialization3D) {
 //  values.insert(poseKey, Pose3());
 //  values.insert(pointKey, Point3(1, 0, 0));
 //
-//  EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(poseKey, pointKey),
+//  EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}),
 //                                      values, 1e-7, 1e-5);
 //  EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
 //}

gtsam/sam/tests/testRangeFactor.cpp

@@ -141,6 +141,27 @@ TEST( RangeFactor, EqualsWithTransform ) {
       body_P_sensor_3D);
   CHECK(assert_equal(factor3D_1, factor3D_2));
 }
+/* ************************************************************************* */
+TEST( RangeFactor, EqualsAfterDeserializing) {
+  // Check that the same results are obtained after deserializing:
+  Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
+      Point3(0.25, -0.10, 1.0));
+
+  RangeFactorWithTransform3D factor3D_1(poseKey, pointKey, measurement, model,
+      body_P_sensor_3D), factor3D_2;
+
+  // check with Equal() trait:
+  gtsam::serializationTestHelpers::roundtripXML(factor3D_1, factor3D_2);
+  CHECK(assert_equal(factor3D_1, factor3D_2));
+
+  const Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0));
+  const Point3 point(-2.0, 11.0, 1.0);
+  const Values values = {{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}};
+  
+  const Vector error_1 = factor3D_1.unwhitenedError(values);
+  const Vector error_2 = factor3D_2.unwhitenedError(values);
+  CHECK(assert_equal(error_1, error_2));
+}
 
 /* ************************************************************************* */
 TEST( RangeFactor, Error2D ) {
@@ -152,7 +173,7 @@ TEST( RangeFactor, Error2D ) {
   Point2 point(-4.0, 11.0);
 
   // Use the factor to calculate the error
-  Vector actualError(factor.evaluateError(pose, point));
+  Vector actualError(factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}));
 
   // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
   Vector expectedError = (Vector(1) << 0.295630141).finished();
@@ -175,7 +196,7 @@ TEST( RangeFactor, Error2DWithTransform ) {
   Point2 point(-4.0, 11.0);
 
   // Use the factor to calculate the error
-  Vector actualError(factor.evaluateError(pose, point));
+  Vector actualError(factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}));
 
   // The expected error is ||(5.0, 9.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
   Vector expectedError = (Vector(1) << 0.295630141).finished();
@@ -194,7 +215,7 @@ TEST( RangeFactor, Error3D ) {
   Point3 point(-2.0, 11.0, 1.0);
 
   // Use the factor to calculate the error
-  Vector actualError(factor.evaluateError(pose, point));
+  Vector actualError(factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}));
 
   // The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
   Vector expectedError = (Vector(1) << 0.295630141).finished();
@@ -218,7 +239,7 @@ TEST( RangeFactor, Error3DWithTransform ) {
   Point3 point(-2.0, 11.0, 1.0);
 
   // Use the factor to calculate the error
-  Vector actualError(factor.evaluateError(pose, point));
+  Vector actualError(factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}));
 
   // The expected error is ||(3.0, 9.0, 4.0)|| - 10.0 = 0.295630141 meter / UnitCovariance
   Vector expectedError = (Vector(1) << 0.295630141).finished();
@@ -266,8 +287,10 @@ TEST( RangeFactor, Jacobian2DWithTransform ) {
   Point2 point(-4.0, 11.0);
 
   // Use the factor to calculate the Jacobians
-  Matrix H1Actual, H2Actual;
+  std::vector<Matrix> actualHs(2);
-  factor.evaluateError(pose, point, H1Actual, H2Actual);
+  factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}, actualHs); 
+  const Matrix& H1Actual = actualHs.at(0);
+  const Matrix& H2Actual = actualHs.at(1);
 
   // Use numerical derivatives to calculate the Jacobians
   Matrix H1Expected, H2Expected;
@@ -291,8 +314,10 @@ TEST( RangeFactor, Jacobian3D ) {
   Point3 point(-2.0, 11.0, 1.0);
 
   // Use the factor to calculate the Jacobians
-  Matrix H1Actual, H2Actual;
+  std::vector<Matrix> actualHs(2);
-  factor.evaluateError(pose, point, H1Actual, H2Actual);
+  factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}, actualHs); 
+  const Matrix& H1Actual = actualHs.at(0);
+  const Matrix& H2Actual = actualHs.at(1);
 
   // Use numerical derivatives to calculate the Jacobians
   Matrix H1Expected, H2Expected;
@@ -321,8 +346,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
   Point3 point(-2.0, 11.0, 1.0);
 
   // Use the factor to calculate the Jacobians
-  Matrix H1Actual, H2Actual;
+  std::vector<Matrix> actualHs(2);
-  factor.evaluateError(pose, point, H1Actual, H2Actual);
+  factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}, actualHs); 
+  const Matrix& H1Actual = actualHs.at(0);
+  const Matrix& H2Actual = actualHs.at(1);
 
   // Use numerical derivatives to calculate the Jacobians
   Matrix H1Expected, H2Expected;
@@ -350,7 +377,7 @@ TEST(RangeFactor, Point3) {
   Vector expectedError = (Vector(1) << 0.295630141).finished();
 
   // Verify we get the expected error
-  CHECK(assert_equal(expectedError, factor.evaluateError(pose, point), 1e-9));
+  CHECK(assert_equal(expectedError, factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}), 1e-9));
 }
 
 /* ************************************************************************* */
@@ -393,7 +420,7 @@ TEST(RangeFactor, NonGTSAM) {
   Vector expectedError = (Vector(1) << 0.295630141).finished();
 
   // Verify we get the expected error
-  CHECK(assert_equal(expectedError, factor.evaluateError(pose, point), 1e-9));
+  CHECK(assert_equal(expectedError, factor.unwhitenedError({{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}}), 1e-9));
 }
 
 /* ************************************************************************* */

gtsam/slam/AntiFactor.h

@@ -52,20 +52,20 @@ namespace gtsam {
     virtual ~AntiFactor() {}
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    gtsam::NonlinearFactor::shared_ptr clone() const override {
       return boost::static_pointer_cast<gtsam::NonlinearFactor>(
           gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
     /** implement functions needed for Testable */
 
     /** 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 {
       std::cout << s << "AntiFactor version of:" << std::endl;
       factor_->print(s, keyFormatter);
     }
 
     /** equals */
-    virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+    bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
       const This *e =  dynamic_cast<const This*> (&expected);
       return e != nullptr && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol);
     }
@@ -77,16 +77,16 @@ namespace gtsam {
      * For the AntiFactor, this will have the same magnitude of the original factor,
      * but the opposite sign.
      */
-    double error(const Values& c) const { return -factor_->error(c); }
+    double error(const Values& c) const override { return -factor_->error(c); }
 
     /** get the dimension of the factor (same as the original factor) */
-    size_t dim() const { return factor_->dim(); }
+    size_t dim() const override { return factor_->dim(); }
 
     /**
      * Checks whether this factor should be used based on a set of values.
      * The AntiFactor will have the same 'active' profile as the original factor.
      */
-    bool active(const Values& c) const { return factor_->active(c); }
+    bool active(const Values& c) const override { return factor_->active(c); }
 
     /**
      * Linearize to a GaussianFactor. The AntiFactor always returns a Hessian Factor
@@ -94,7 +94,7 @@ namespace gtsam {
      * returns a Jacobian instead of a full Hessian), but with the opposite sign. This
      * effectively cancels the effect of the original factor on the factor graph.
      */
-    boost::shared_ptr<GaussianFactor> linearize(const Values& c) const {
+    boost::shared_ptr<GaussianFactor> linearize(const Values& c) const override {
 
       // Generate the linearized factor from the contained nonlinear factor
       GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c);

gtsam/slam/BetweenFactor.h

@@ -63,14 +63,14 @@ namespace gtsam {
     virtual ~BetweenFactor() {}
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    gtsam::NonlinearFactor::shared_ptr clone() const override {
       return boost::static_pointer_cast<gtsam::NonlinearFactor>(
           gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
     /** implement functions needed for Testable */
 
     /** 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 {
       std::cout << s << "BetweenFactor("
           << keyFormatter(this->key1()) << ","
           << keyFormatter(this->key2()) << ")\n";
@@ -79,7 +79,7 @@ namespace gtsam {
     }
 
     /** equals */
-    virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+    bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
       const This *e =  dynamic_cast<const This*> (&expected);
       return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol);
     }
@@ -87,8 +87,8 @@ namespace gtsam {
     /** implement functions needed to derive from Factor */
 
     /** vector of errors */
-  Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
+    Vector evaluateError(const T& p1, const T& p2, boost::optional<Matrix&> H1 =
-      boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+      boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
       T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
       // manifold equivalent of h(x)-z -> log(z,h(x))
 #ifdef SLOW_BUT_CORRECT_BETWEENFACTOR

gtsam/slam/BoundingConstraint.h

@@ -58,14 +58,14 @@ struct BoundingConstraint1: public NoiseModelFactor1<VALUE> {
       boost::none) const = 0;
 
   /** active when constraint *NOT* met */
-  bool active(const Values& c) const {
+  bool active(const Values& c) const override {
     // note: still active at equality to avoid zigzagging
     double x = value(c.at<X>(this->key()));
     return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
   }
 
   Vector evaluateError(const X& x, boost::optional<Matrix&> H =
-      boost::none) const {
+      boost::none) const override {
     Matrix D;
     double error = value(x, D) - threshold_;
     if (H) {
@@ -126,7 +126,7 @@ struct BoundingConstraint2: public NoiseModelFactor2<VALUE1, VALUE2> {
       boost::optional<Matrix&> H2 = boost::none) const = 0;
 
   /** active when constraint *NOT* met */
-  bool active(const Values& c) const {
+  bool active(const Values& c) const override {
     // note: still active at equality to avoid zigzagging
     double x = value(c.at<X1>(this->key1()), c.at<X2>(this->key2()));
     return (isGreaterThan_) ? x <= threshold_ : x >= threshold_;
@@ -134,7 +134,7 @@ struct BoundingConstraint2: public NoiseModelFactor2<VALUE1, VALUE2> {
 
   Vector evaluateError(const X1& x1, const X2& x2,
       boost::optional<Matrix&> H1 = boost::none,
-      boost::optional<Matrix&> H2 = boost::none) const {
+      boost::optional<Matrix&> H2 = boost::none) const override {
     Matrix D1, D2;
     double error = value(x1, x2, D1, D2) - threshold_;
     if (H1) {

gtsam/slam/EssentialMatrixConstraint.h

@@ -61,7 +61,7 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
@@ -69,18 +69,18 @@ public:
   /** implement functions needed for Testable */
 
   /** print */
-  virtual void print(const std::string& s = "",
+  void print(const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
   /** equals */
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
   /** implement functions needed to derive from Factor */
 
   /** vector of errors */
-  virtual Vector evaluateError(const Pose3& p1, const Pose3& p2,
+  Vector evaluateError(const Pose3& p1, const Pose3& p2,
       boost::optional<Matrix&> Hp1 = boost::none, //
-      boost::optional<Matrix&> Hp2 = boost::none) const;
+      boost::optional<Matrix&> Hp2 = boost::none) const override;
 
   /** return the measured */
   const EssentialMatrix& measured() const {

gtsam/slam/EssentialMatrixFactor.h

@@ -58,14 +58,14 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   /// print
-  virtual void print(const std::string& s = "",
+  void print(const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s);
     std::cout << "  EssentialMatrixFactor with measurements\n  ("
         << vA_.transpose() << ")' and (" << vB_.transpose() << ")'"
@@ -74,7 +74,7 @@ public:
 
   /// vector of errors returns 1D vector
   Vector evaluateError(const EssentialMatrix& E, boost::optional<Matrix&> H =
-      boost::none) const {
+      boost::none) const override {
     Vector error(1);
     error << E.error(vA_, vB_, H);
     return error;
@@ -131,14 +131,14 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   /// print
-  virtual void print(const std::string& s = "",
+  void print(const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s);
     std::cout << "  EssentialMatrixFactor2 with measurements\n  ("
         << dP1_.transpose() << ")' and (" << pn_.transpose()
@@ -152,7 +152,7 @@ public:
    */
   Vector evaluateError(const EssentialMatrix& E, const double& d,
       boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
-          boost::none) const {
+          boost::none) const override {
 
     // We have point x,y in image 1
     // Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
@@ -250,14 +250,14 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));
   }
 
   /// print
-  virtual void print(const std::string& s = "",
+  void print(const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s);
     std::cout << "  EssentialMatrixFactor3 with rotation " << cRb_ << std::endl;
   }
@@ -269,7 +269,7 @@ public:
    */
   Vector evaluateError(const EssentialMatrix& E, const double& d,
       boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
-          boost::none) const {
+          boost::none) const override {
     if (!DE) {
       // Convert E from body to camera frame
       EssentialMatrix cameraE = cRb_ * E;

gtsam/slam/FrobeniusFactor.h

@@ -56,7 +56,7 @@ class FrobeniusPrior : public NoiseModelFactor1<Rot> {
 
   /// Error is just Frobenius norm between Rot element and vectorized matrix M.
   Vector evaluateError(const Rot& R,
-                       boost::optional<Matrix&> H = boost::none) const {
+                       boost::optional<Matrix&> H = boost::none) const override {
     return R.vec(H) - vecM_;  // Jacobian is computed only when needed.
   }
 };
@@ -78,7 +78,7 @@ class FrobeniusFactor : public NoiseModelFactor2<Rot, Rot> {
   /// Error is just Frobenius norm between rotation matrices.
   Vector evaluateError(const Rot& R1, const Rot& R2,
                        boost::optional<Matrix&> H1 = boost::none,
-                       boost::optional<Matrix&> H2 = boost::none) const {
+                       boost::optional<Matrix&> H2 = boost::none) const override {
     Vector error = R2.vec(H2) - R1.vec(H1);
     if (H1) *H1 = -*H1;
     return error;
@@ -110,7 +110,7 @@ class FrobeniusBetweenFactor : public NoiseModelFactor2<Rot, Rot> {
   /// Error is Frobenius norm between R1*R12 and R2.
   Vector evaluateError(const Rot& R1, const Rot& R2,
                        boost::optional<Matrix&> H1 = boost::none,
-                       boost::optional<Matrix&> H2 = boost::none) const {
+                       boost::optional<Matrix&> H2 = boost::none) const override {
     const Rot R2hat = R1.compose(R12_);
     Eigen::Matrix<double, Dim, Rot::dimension> vec_H_R2hat;
     Vector error = R2.vec(H2) - R2hat.vec(H1 ? &vec_H_R2hat : nullptr);
@@ -139,7 +139,7 @@ class GTSAM_EXPORT FrobeniusWormholeFactor : public NoiseModelFactor2<SOn, SOn>
   /// projects down from SO(p) to the Stiefel manifold of px3 matrices.
   Vector evaluateError(const SOn& Q1, const SOn& Q2,
                        boost::optional<Matrix&> H1 = boost::none,
-                       boost::optional<Matrix&> H2 = boost::none) const;
+                       boost::optional<Matrix&> H2 = boost::none) const override;
 };
 
 }  // namespace gtsam

gtsam/slam/GeneralSFMFactor.h

@@ -96,7 +96,7 @@ public:
   virtual ~GeneralSFMFactor() {} ///< destructor
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
 
@@ -105,7 +105,7 @@ public:
    * @param s optional string naming the factor
    * @param keyFormatter optional formatter for printing out Symbols
    */
-  void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s, keyFormatter);
     traits<Point2>::Print(measured_, s + ".z");
   }
@@ -113,14 +113,14 @@ public:
   /**
    * equals
    */
-  bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor &p, double tol = 1e-9) const override {
     const This* e = dynamic_cast<const This*>(&p);
     return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
   }
 
   /** h(x)-z */
   Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
-      boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
+      boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const override {
     try {
       return camera.project2(point,H1,H2) - measured_;
     }
@@ -133,7 +133,7 @@ public:
   }
 
   /// Linearize using fixed-size matrices
-  boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {
+  boost::shared_ptr<GaussianFactor> linearize(const Values& values) const override {
     // Only linearize if the factor is active
     if (!this->active(values)) return boost::shared_ptr<JacobianFactor>();
 
@@ -230,7 +230,7 @@ public:
   virtual ~GeneralSFMFactor2() {} ///< destructor
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this)));}
 
@@ -239,7 +239,7 @@ public:
    * @param s optional string naming the factor
    * @param keyFormatter optional formatter useful for printing Symbols
    */
-  void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s, keyFormatter);
     traits<Point2>::Print(measured_, s + ".z");
   }
@@ -247,7 +247,7 @@ public:
   /**
    * equals
    */
-  bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
+  bool equals(const NonlinearFactor &p, double tol = 1e-9) const override {
     const This* e = dynamic_cast<const This*>(&p);
     return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
   }
@@ -256,7 +256,7 @@ public:
   Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib,
       boost::optional<Matrix&> H1=boost::none,
       boost::optional<Matrix&> H2=boost::none,
-      boost::optional<Matrix&> H3=boost::none) const
+      boost::optional<Matrix&> H3=boost::none) const override
   {
     try {
       Camera camera(pose3,calib);

gtsam/slam/OrientedPlane3Factor.h

@@ -41,13 +41,13 @@ public:
   }
 
   /// print
-  virtual void print(const std::string& s = "OrientedPlane3Factor",
+  void print(const std::string& s = "OrientedPlane3Factor",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
   /// evaluateError
-  virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane,
+  Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane,
       boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
-          boost::none) const {
+          boost::none) const override {
     OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1,
         H2);
     Vector err(3);
@@ -78,14 +78,14 @@ public:
   }
 
   /// print
-  virtual void print(const std::string& s = "OrientedPlane3DirectionPrior",
+  void print(const std::string& s = "OrientedPlane3DirectionPrior",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
 
   /// equals
-  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
+  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
 
-  virtual Vector evaluateError(const OrientedPlane3& plane,
+  Vector evaluateError(const OrientedPlane3& plane,
-      boost::optional<Matrix&> H = boost::none) const;
+      boost::optional<Matrix&> H = boost::none) const override;
 };
 
 } // gtsam

gtsam/slam/PoseRotationPrior.h

@@ -50,7 +50,7 @@ public:
   virtual ~PoseRotationPrior() {}
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
@@ -60,19 +60,19 @@ public:
   // testable
 
   /** equals specialized to this factor */
-  virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
     const This *e = dynamic_cast<const This*> (&expected);
     return e != nullptr && Base::equals(*e, tol) && measured_.equals(e->measured_, tol);
   }
 
   /** print contents */
-  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s + "PoseRotationPrior", keyFormatter);
     measured_.print("Measured Rotation");
   }
 
   /** h(x)-z */
-  Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const {
+  Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const override {
     const Rotation& newR = pose.rotation();
     if (H) {
       *H = Matrix::Zero(rDim, xDim);

gtsam/slam/PoseTranslationPrior.h

@@ -54,12 +54,12 @@ public:
   const Translation& measured() const { return measured_; }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
   /** h(x)-z */
-  Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const {
+  Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const override {
     const Translation& newTrans = pose.translation();
     const Rotation& R = pose.rotation();
     const int tDim = traits<Translation>::GetDimension(newTrans);
@@ -74,13 +74,13 @@ public:
   }
 
   /** equals specialized to this factor */
-  virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
     const This *e = dynamic_cast<const This*> (&expected);
     return e != nullptr && Base::equals(*e, tol) && traits<Translation>::Equals(measured_, e->measured_, tol);
   }
 
   /** print contents */
-  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s + "PoseTranslationPrior", keyFormatter);
     traits<Translation>::Print(measured_, "Measured Translation");
   }

gtsam/slam/ProjectionFactor.h

@@ -100,7 +100,7 @@ namespace gtsam {
     virtual ~GenericProjectionFactor() {}
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    gtsam::NonlinearFactor::shared_ptr clone() const override {
       return boost::static_pointer_cast<gtsam::NonlinearFactor>(
           gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
@@ -109,7 +109,7 @@ namespace gtsam {
      * @param s optional string naming the factor
      * @param keyFormatter optional formatter useful for printing Symbols
      */
-    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 << "GenericProjectionFactor, z = ";
       traits<Point2>::Print(measured_);
       if(this->body_P_sensor_)
@@ -118,7 +118,7 @@ namespace gtsam {
     }
 
     /// equals
-    virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+    bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
       const This *e = dynamic_cast<const This*>(&p);
       return e
           && Base::equals(p, tol)
@@ -129,7 +129,7 @@ namespace gtsam {
 
     /// Evaluate error h(x)-z and optionally derivatives
     Vector evaluateError(const Pose3& pose, const Point3& point,
-        boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+        boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
       try {
         if(body_P_sensor_) {
           if(H1) {

gtsam/slam/ReferenceFrameFactor.h

@@ -89,23 +89,23 @@ public:
 
   virtual ~ReferenceFrameFactor(){}
 
-  virtual NonlinearFactor::shared_ptr clone() const {
+  NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<NonlinearFactor>(
         NonlinearFactor::shared_ptr(new This(*this))); }
 
   /** Combined cost and derivative function using boost::optional */
-  virtual Vector evaluateError(const Point& global, const Transform& trans, const Point& local,
+  Vector evaluateError(const Point& global, const Transform& trans, const Point& local,
         boost::optional<Matrix&> Dforeign = boost::none,
         boost::optional<Matrix&> Dtrans = boost::none,
-        boost::optional<Matrix&> Dlocal = boost::none) const  {
+        boost::optional<Matrix&> Dlocal = boost::none) const override {
     Point newlocal = transform_point<Transform,Point>(trans, global, Dtrans, Dforeign);
     if (Dlocal)
       *Dlocal = -1* Matrix::Identity(traits<Point>::dimension, traits<Point>::dimension);
     return traits<Point>::Local(local,newlocal);
   }
 
-  virtual void print(const std::string& s="",
+  void print(const std::string& s="",
-      const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+      const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     std::cout << s << ": ReferenceFrameFactor("
         << "Global: " << keyFormatter(this->key1()) << ","
         << " Transform: " << keyFormatter(this->key2()) << ","

gtsam/slam/RotateFactor.h

@@ -36,13 +36,13 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
   /// print
-  virtual void print(const std::string& s = "",
+  void print(const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s);
     std::cout << "RotateFactor:]\n";
     std::cout << "p: " << p_.transpose() << std::endl;
@@ -51,7 +51,7 @@ public:
 
   /// vector of errors returns 2D vector
   Vector evaluateError(const Rot3& R,
-      boost::optional<Matrix&> H = boost::none) const {
+      boost::optional<Matrix&> H = boost::none) const override {
     // predict p_ as q = R*z_, derivative H will be filled if not none
     Point3 q = R.rotate(z_,H);
     // error is just difference, and note derivative of that wrpt q is I3
@@ -88,13 +88,13 @@ public:
   }
 
   /// @return a deep copy of this factor
-  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  gtsam::NonlinearFactor::shared_ptr clone() const override {
     return boost::static_pointer_cast<gtsam::NonlinearFactor>(
         gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
   /// print
-  virtual void print(const std::string& s = "",
+  void print(const std::string& s = "",
-      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
     Base::print(s);
     std::cout << "RotateDirectionsFactor:" << std::endl;
     i_p_.print("p");
@@ -102,7 +102,7 @@ public:
   }
 
   /// vector of errors returns 2D vector
-  Vector evaluateError(const Rot3& iRc, boost::optional<Matrix&> H = boost::none) const {
+  Vector evaluateError(const Rot3& iRc, boost::optional<Matrix&> H = boost::none) const override {
     Unit3 i_q = iRc * c_z_;
     Vector error = i_p_.error(i_q, H);
     if (H) {