Merge branch 'develop' into feature/python-plotting

.travis.sh

@@ -1,5 +1,37 @@
 #!/bin/bash
 
+# install TBB with _debug.so files
+function install_tbb()
+{
+  TBB_BASEURL=https://github.com/oneapi-src/oneTBB/releases/download
+  TBB_VERSION=4.4.2
+  TBB_DIR=tbb44_20151115oss
+  TBB_SAVEPATH="/tmp/tbb.tgz"
+
+  if [ "$TRAVIS_OS_NAME" == "linux" ]; then
+    OS_SHORT="lin"
+    TBB_LIB_DIR="intel64/gcc4.4"
+    SUDO="sudo"
+
+  elif [ "$TRAVIS_OS_NAME" == "osx" ]; then
+    OS_SHORT="lin"
+    TBB_LIB_DIR=""
+    SUDO=""
+
+  fi
+
+  wget "${TBB_BASEURL}/${TBB_VERSION}/${TBB_DIR}_${OS_SHORT}.tgz" -O $TBB_SAVEPATH
+  tar -C /tmp -xf $TBB_SAVEPATH
+
+  TBBROOT=/tmp/$TBB_DIR
+  # Copy the needed files to the correct places.
+  # This works correctly for travis builds, instead of setting path variables.
+  # This is what Homebrew does to install TBB on Macs
+  $SUDO cp -R $TBBROOT/lib/$TBB_LIB_DIR/* /usr/local/lib/
+  $SUDO cp -R $TBBROOT/include/ /usr/local/include/
+
+}
+
 # common tasks before either build or test
 function configure()
 {
@@ -14,6 +46,8 @@ function configure()
   rm -fr $BUILD_DIR || true
   mkdir $BUILD_DIR && cd $BUILD_DIR
 
+  install_tbb
+
   if [ ! -z "$GCC_VERSION" ]; then
     export CC=gcc-$GCC_VERSION
     export CXX=g++-$GCC_VERSION
@@ -24,6 +58,7 @@ function configure()
       -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE:-Debug} \
       -DGTSAM_BUILD_TESTS=${GTSAM_BUILD_TESTS:-OFF} \
       -DGTSAM_BUILD_UNSTABLE=${GTSAM_BUILD_UNSTABLE:-ON} \
+      -DGTSAM_WITH_TBB=${GTSAM_WITH_TBB:-OFF} \
       -DGTSAM_USE_QUATERNIONS=${GTSAM_USE_QUATERNIONS:-OFF} \
       -DGTSAM_BUILD_EXAMPLES_ALWAYS=${GTSAM_BUILD_EXAMPLES_ALWAYS:-ON} \
       -DGTSAM_ALLOW_DEPRECATED_SINCE_V4=${GTSAM_ALLOW_DEPRECATED_SINCE_V4:-OFF} \

.travis.yml

@@ -18,7 +18,7 @@ addons:
     - libboost-all-dev
 
 # before_install:
-#  - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update          ; fi
+  # - if [ "$TRAVIS_OS_NAME" == "osx" ]; then brew update; fi
 
 install:
   - if [ "$TRAVIS_OS_NAME" == "osx" ]; then HOMEBREW_NO_AUTO_UPDATE=1 brew install ccache  ; fi
@@ -89,6 +89,12 @@ jobs:
     compiler: clang
     env: CC=clang-9 CXX=clang++-9 CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_ALLOW_DEPRECATED_SINCE_V4=ON
     script: bash .travis.sh -b
+# on Linux, with GTSAM_WITH_TBB on to make sure GTSAM still compiles/tests
+  - stage: special
+    os: linux
+    compiler: gcc
+    env: CMAKE_BUILD_TYPE=Debug GTSAM_BUILD_UNSTABLE=OFF GTSAM_WITH_TBB=ON
+    script: bash .travis.sh -b
 # -------- STAGE 2: TESTS -----------
 # on Mac, GCC
   - stage: test

CMakeLists.txt

@@ -199,12 +199,17 @@ find_package(TBB 4.4 COMPONENTS tbb tbbmalloc)
 
 # Set up variables if we're using TBB
 if(TBB_FOUND AND GTSAM_WITH_TBB)
-	set(GTSAM_USE_TBB 1)  # This will go into config.h
+    set(GTSAM_USE_TBB 1)  # This will go into config.h
-  # all definitions and link requisites will go via imported targets:
+    if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020))
-  # tbb & tbbmalloc
+        set(TBB_GREATER_EQUAL_2020 1)
-  list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc)
+    else()
+        set(TBB_GREATER_EQUAL_2020 0)
+    endif()
+    # all definitions and link requisites will go via imported targets:
+    # tbb & tbbmalloc
+    list(APPEND GTSAM_ADDITIONAL_LIBRARIES tbb tbbmalloc)
 else()
-	set(GTSAM_USE_TBB 0)  # This will go into config.h
+    set(GTSAM_USE_TBB 0)  # This will go into config.h
 endif()
 
 ###############################################################################
@@ -416,6 +421,10 @@ add_subdirectory(CppUnitLite)
 # Build wrap
 if (GTSAM_BUILD_WRAP)
     add_subdirectory(wrap)
+	# suppress warning of cython line being too long
+	if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+		set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-misleading-indentation")
+	endif()
 endif(GTSAM_BUILD_WRAP)
 
 # Build GTSAM library

cython/gtsam/examples/Pose2SLAMExample_g2o.py

@@ -53,16 +53,15 @@ graph, initial = gtsam.readG2o(g2oFile, is3D)
 assert args.kernel == "none", "Supplied kernel type is not yet implemented"
 
 # Add prior on the pose having index (key) = 0
-graphWithPrior = graph
 priorModel = gtsam.noiseModel_Diagonal.Variances(vector3(1e-6, 1e-6, 1e-8))
-graphWithPrior.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
+graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))
 
 params = gtsam.GaussNewtonParams()
 params.setVerbosity("Termination")
 params.setMaxIterations(maxIterations)
 # parameters.setRelativeErrorTol(1e-5)
 # Create the optimizer ...
-optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
+optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
 # ... and optimize
 result = optimizer.optimize()
 

cython/gtsam/examples/Pose3SLAMExample_g2o.py

@@ -43,18 +43,17 @@ priorModel = gtsam.noiseModel_Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6,
                                                          1e-4, 1e-4, 1e-4))
 
 print("Adding prior to g2o file ")
-graphWithPrior = graph
 firstKey = initial.keys().at(0)
-graphWithPrior.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
+graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
 
 params = gtsam.GaussNewtonParams()
 params.setVerbosity("Termination")  # this will show info about stopping conds
-optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
+optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
 result = optimizer.optimize()
 print("Optimization complete")
 
-print("initial error = ", graphWithPrior.error(initial))
+print("initial error = ", graph.error(initial))
-print("final error = ", graphWithPrior.error(result))
+print("final error = ", graph.error(result))
 
 if args.output is None:
     print("Final Result:\n{}".format(result))

examples/Data/pose3Localizationexample.txt

@@ -0,0 +1,9 @@
+VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
+VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230
+VERTEX_SE3:QUAT 2 1.993500 0.023275 0.003793 -0.351729 -0.597838 0.584174 0.421446
+VERTEX_SE3:QUAT 3 2.004291 1.024305 0.018047 0.331798 -0.200659 0.919323 0.067024
+VERTEX_SE3:QUAT 4 0.999908 1.055073 0.020212 -0.035697 -0.462490 0.445933 0.765488
+EDGE_SE3:QUAT 0 1   1.001367 0.015390 0.004948   0.190253 0.283162 -0.392318 0.854230   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000
+EDGE_SE3:QUAT 1 2   0.523923 0.776654 0.326659   0.311512 0.656877 -0.678505 0.105373   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000
+EDGE_SE3:QUAT 2 3   0.910927 0.055169 -0.411761   0.595795 -0.561677 0.079353 0.568551   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000
+EDGE_SE3:QUAT 3 4   0.775288 0.228798 -0.596923   -0.592077 0.303380 -0.513226 0.542221   10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000

examples/Pose2SLAMExample_g2o.cpp

@@ -63,10 +63,9 @@ int main(const int argc, const char *argv[]) {
   }
 
   // Add prior on the pose having index (key) = 0
-  NonlinearFactorGraph graphWithPrior = *graph;
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
-  graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
+  graph->add(PriorFactor<Pose2>(0, Pose2(), priorModel));
   std::cout << "Adding prior on pose 0 " << std::endl;
 
   GaussNewtonParams params;
@@ -77,7 +76,7 @@ int main(const int argc, const char *argv[]) {
   }
 
   std::cout << "Optimizing the factor graph" << std::endl;
-  GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params);
+  GaussNewtonOptimizer optimizer(*graph, *initial, params);
   Values result = optimizer.optimize();
   std::cout << "Optimization complete" << std::endl;
 

examples/Pose2SLAMExample_lago.cpp

@@ -42,14 +42,13 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile);
 
   // Add prior on the pose having index (key) = 0
-  NonlinearFactorGraph graphWithPrior = *graph;
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
-  graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
+  graph->add(PriorFactor<Pose2>(0, Pose2(), priorModel));
-  graphWithPrior.print();
+  graph->print();
 
   std::cout << "Computing LAGO estimate" << std::endl;
-  Values estimateLago = lago::initialize(graphWithPrior);
+  Values estimateLago = lago::initialize(*graph);
   std::cout << "done!" << std::endl;
 
   if (argc < 3) {
@@ -57,7 +56,10 @@ int main(const int argc, const char *argv[]) {
   } else {
     const string outputFile = argv[2];
     std::cout << "Writing results to file: " << outputFile << std::endl;
-    writeG2o(*graph, estimateLago, outputFile);
+    NonlinearFactorGraph::shared_ptr graphNoKernel;
+    Values::shared_ptr initial2;
+    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    writeG2o(*graphNoKernel, estimateLago, outputFile);
     std::cout << "done! " << std::endl;
   }
 

examples/Pose3Localization.cpp

@@ -0,0 +1,85 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Pose3SLAMExample_initializePose3.cpp
+ * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
+ * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
+ * @date Aug 25, 2014
+ * @author Luca Carlone
+ */
+
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+#include <fstream>
+
+using namespace std;
+using namespace gtsam;
+
+int main(const int argc, const char *argv[]) {
+
+  // Read graph from file
+  string g2oFile;
+  if (argc < 2)
+    g2oFile = findExampleDataFile("pose3Localizationexample.txt");
+  else
+    g2oFile = argv[1];
+
+  NonlinearFactorGraph::shared_ptr graph;
+  Values::shared_ptr initial;
+  bool is3D = true;
+  boost::tie(graph, initial) = readG2o(g2oFile, is3D);
+
+  // Add prior on the first key
+  noiseModel::Diagonal::shared_ptr priorModel = //
+      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
+  Key firstKey = 0;
+  for(const Values::ConstKeyValuePair& key_value: *initial) {
+    std::cout << "Adding prior to g2o file " << std::endl;
+    firstKey = key_value.key;
+    graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
+    break;
+  }
+
+  std::cout << "Optimizing the factor graph" << std::endl;
+  GaussNewtonParams params;
+  params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
+  GaussNewtonOptimizer optimizer(*graph, *initial, params);
+  Values result = optimizer.optimize();
+  std::cout << "Optimization complete" << std::endl;
+
+  std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
+  std::cout << "final error=" <<graph->error(result)<< std::endl;
+
+  if (argc < 3) {
+    result.print("result");
+  } else {
+    const string outputFile = argv[2];
+    std::cout << "Writing results to file: " << outputFile << std::endl;
+    NonlinearFactorGraph::shared_ptr graphNoKernel;
+    Values::shared_ptr initial2;
+    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    writeG2o(*graphNoKernel, result, outputFile);
+    std::cout << "done! " << std::endl;
+  }
+
+  // Calculate and print marginal covariances for all variables
+  Marginals marginals(*graph, result);
+  for(const auto& key_value: result) {
+    auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
+    if (!p) continue;
+    std::cout << marginals.marginalCovariance(key_value.key) << endl;
+  }
+  return 0;
+}

examples/Pose3SLAMExample_g2o.cpp

@@ -41,21 +41,20 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   // Add prior on the first key
-  NonlinearFactorGraph graphWithPrior = *graph;
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
   for(const Values::ConstKeyValuePair& key_value: *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
-    graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
+    graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
     break;
   }
 
   std::cout << "Optimizing the factor graph" << std::endl;
   GaussNewtonParams params;
   params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
-  GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params);
+  GaussNewtonOptimizer optimizer(*graph, *initial, params);
   Values result = optimizer.optimize();
   std::cout << "Optimization complete" << std::endl;
 
@@ -67,7 +66,10 @@ int main(const int argc, const char *argv[]) {
   } else {
     const string outputFile = argv[2];
     std::cout << "Writing results to file: " << outputFile << std::endl;
-    writeG2o(*graph, result, outputFile);
+    NonlinearFactorGraph::shared_ptr graphNoKernel;
+    Values::shared_ptr initial2;
+    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    writeG2o(*graphNoKernel, result, outputFile);
     std::cout << "done! " << std::endl;
   }
   return 0;

examples/Pose3SLAMExample_initializePose3Chordal.cpp

@@ -41,19 +41,18 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   // Add prior on the first key
-  NonlinearFactorGraph graphWithPrior = *graph;
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
   for(const Values::ConstKeyValuePair& key_value: *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
-    graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
+    graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
     break;
   }
 
   std::cout << "Initializing Pose3 - chordal relaxation" << std::endl;
-  Values initialization = InitializePose3::initialize(graphWithPrior);
+  Values initialization = InitializePose3::initialize(*graph);
   std::cout << "done!" << std::endl;
 
   if (argc < 3) {
@@ -61,7 +60,10 @@ int main(const int argc, const char *argv[]) {
   } else {
     const string outputFile = argv[2];
     std::cout << "Writing results to file: " << outputFile  << std::endl;
-    writeG2o(*graph, initialization, outputFile);
+    NonlinearFactorGraph::shared_ptr graphNoKernel;
+    Values::shared_ptr initial2;
+    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    writeG2o(*graphNoKernel, initialization, outputFile);
     std::cout << "done! " << std::endl;
   }
   return 0;

examples/Pose3SLAMExample_initializePose3Gradient.cpp

@@ -41,20 +41,19 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   // Add prior on the first key
-  NonlinearFactorGraph graphWithPrior = *graph;
   noiseModel::Diagonal::shared_ptr priorModel = //
       noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
   for(const Values::ConstKeyValuePair& key_value: *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
-    graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
+    graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
     break;
   }
 
   std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl;
   bool useGradient = true;
-  Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient);
+  Values initialization = InitializePose3::initialize(*graph, *initial, useGradient);
   std::cout << "done!" << std::endl;
 
   std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
@@ -65,7 +64,10 @@ int main(const int argc, const char *argv[]) {
   } else {
     const string outputFile = argv[2];
     std::cout << "Writing results to file: " << outputFile  << std::endl;
-    writeG2o(*graph, initialization, outputFile);
+    NonlinearFactorGraph::shared_ptr graphNoKernel;
+    Values::shared_ptr initial2;
+    boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
+    writeG2o(*graphNoKernel, initialization, outputFile);
     std::cout << "done! " << std::endl;
   }
   return 0;

examples/SFMExample_SmartFactor.cpp

@@ -117,7 +117,7 @@ int main(int argc, char* argv[]) {
       // The output of point() is in boost::optional<Point3>, as sometimes
       // the triangulation operation inside smart factor will encounter degeneracy.
       boost::optional<Point3> point = smart->point(result);
-      if (point) // ignore if boost::optional return NULL
+      if (point) // ignore if boost::optional return nullptr
         landmark_result.insert(j, *point);
     }
   }

examples/SFMExample_SmartFactorPCG.cpp

@@ -114,7 +114,7 @@ int main(int argc, char* argv[]) {
         boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
     if (smart) {
       boost::optional<Point3> point = smart->point(result);
-      if (point) // ignore if boost::optional return NULL
+      if (point) // ignore if boost::optional return nullptr
         landmark_result.insert(j, *point);
     }
   }

gtsam.h

@@ -1563,17 +1563,15 @@ virtual class Robust : gtsam::noiseModel::Base {
 
 #include <gtsam/linear/Sampler.h>
 class Sampler {
-    //Constructors
+  // Constructors
   Sampler(gtsam::noiseModel::Diagonal* model, int seed);
   Sampler(Vector sigmas, int seed);
-  Sampler(int seed);
 
-    //Standard Interface
+  // Standard Interface
   size_t dim() const;
   Vector sigmas() const;
   gtsam::noiseModel::Diagonal* model() const;
   Vector sample();
-  Vector sampleNewModel(gtsam::noiseModel::Diagonal* model);
 };
 
 #include <gtsam/linear/VectorValues.h>

gtsam/base/OptionalJacobian.h

@@ -55,7 +55,7 @@ private:
   }
 
   // Private and very dangerous constructor straight from memory
-  OptionalJacobian(double* data) : map_(NULL) {
+  OptionalJacobian(double* data) : map_(nullptr) {
     if (data) usurp(data);
   }
 
@@ -66,25 +66,25 @@ public:
 
   /// Default constructor acts like boost::none
   OptionalJacobian() :
-      map_(NULL) {
+      map_(nullptr) {
   }
 
   /// Constructor that will usurp data of a fixed-size matrix
   OptionalJacobian(Jacobian& fixed) :
-      map_(NULL) {
+      map_(nullptr) {
     usurp(fixed.data());
   }
 
   /// Constructor that will usurp data of a fixed-size matrix, pointer version
   OptionalJacobian(Jacobian* fixedPtr) :
-      map_(NULL) {
+      map_(nullptr) {
     if (fixedPtr)
       usurp(fixedPtr->data());
   }
 
   /// Constructor that will resize a dynamic matrix (unless already correct)
   OptionalJacobian(Eigen::MatrixXd& dynamic) :
-      map_(NULL) {
+      map_(nullptr) {
     dynamic.resize(Rows, Cols); // no malloc if correct size
     usurp(dynamic.data());
   }
@@ -93,12 +93,12 @@ public:
 
   /// Constructor with boost::none just makes empty
   OptionalJacobian(boost::none_t /*none*/) :
-      map_(NULL) {
+      map_(nullptr) {
   }
 
   /// Constructor compatible with old-style derivatives
   OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
-      map_(NULL) {
+      map_(nullptr) {
     if (optional) {
       optional->resize(Rows, Cols);
       usurp(optional->data());
@@ -110,11 +110,11 @@ public:
   /// Constructor that will usurp data of a block expression
   /// TODO(frank): unfortunately using a Map makes usurping non-contiguous memory impossible
   //  template <typename Derived, bool InnerPanel>
-  //  OptionalJacobian(Eigen::Block<Derived,Rows,Cols,InnerPanel> block) : map_(NULL) { ?? }
+  //  OptionalJacobian(Eigen::Block<Derived,Rows,Cols,InnerPanel> block) : map_(nullptr) { ?? }
 
   /// Return true is allocated, false if default constructor was used
   operator bool() const {
-    return map_.data() != NULL;
+    return map_.data() != nullptr;
   }
 
   /// De-reference, like boost optional
@@ -173,7 +173,7 @@ public:
 
   /// Default constructor acts like boost::none
   OptionalJacobian() :
-    pointer_(NULL) {
+    pointer_(nullptr) {
   }
 
   /// Construct from pointer to dynamic matrix
@@ -186,12 +186,12 @@ public:
 
   /// Constructor with boost::none just makes empty
   OptionalJacobian(boost::none_t /*none*/) :
-    pointer_(NULL) {
+    pointer_(nullptr) {
   }
 
   /// Constructor compatible with old-style derivatives
   OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
-      pointer_(NULL) {
+      pointer_(nullptr) {
     if (optional) pointer_ = &(*optional);
   }
 
@@ -199,7 +199,7 @@ public:
 
   /// Return true is allocated, false if default constructor was used
   operator bool() const {
-    return pointer_!=NULL;
+    return pointer_!=nullptr;
   }
 
   /// De-reference, like boost optional

gtsam/base/treeTraversal/parallelTraversalTasks.h

@@ -53,7 +53,7 @@ namespace gtsam {
           // Run the post-order visitor
           (void) visitorPost(treeNode, *myData);
 
-          return NULL;
+          return nullptr;
         }
       };
 
@@ -88,7 +88,7 @@ namespace gtsam {
           {
             // Run the post-order visitor since this task was recycled to run the post-order visitor
             (void) visitorPost(treeNode, *myData);
-            return NULL;
+            return nullptr;
           }
           else
           {
@@ -129,14 +129,14 @@ namespace gtsam {
               {
                 // Run the post-order visitor in this task if we have no children
                 (void) visitorPost(treeNode, *myData);
-                return NULL;
+                return nullptr;
               }
             }
             else
             {
               // Process this node and its children in this task
               processNodeRecursively(treeNode, *myData);
-              return NULL;
+              return nullptr;
             }
           }
         }
@@ -184,8 +184,8 @@ namespace gtsam {
           set_ref_count(1 + (int) roots.size());
           // Spawn tasks
           spawn_and_wait_for_all(tasks);
-          // Return NULL
+          // Return nullptr
-          return NULL;
+          return nullptr;
         }
       };
 

gtsam/discrete/AlgebraicDecisionTree.h

@@ -101,7 +101,7 @@ namespace gtsam {
     /** Create a new function splitting on a variable */
     template<typename Iterator>
     AlgebraicDecisionTree(Iterator begin, Iterator end, const L& label) :
-        Super(NULL) {
+        Super(nullptr) {
       this->root_ = compose(begin, end, label);
     }
 

gtsam/discrete/DiscreteFactorGraph.cpp

@@ -71,7 +71,7 @@ namespace gtsam {
     for (size_t i = 0; i < factors_.size(); i++) {
       std::stringstream ss;
       ss << "factor " << i << ": ";
-      if (factors_[i] != NULL) factors_[i]->print(ss.str(), formatter);
+      if (factors_[i] != nullptr) factors_[i]->print(ss.str(), formatter);
     }
   }
 

gtsam/geometry/tests/testEssentialMatrix.cpp

@@ -159,7 +159,7 @@ TEST (EssentialMatrix, rotate) {
   Matrix actH1, actH2;
   try {
     bodyE.rotate(cRb, actH1, actH2);
-  } catch (exception e) {
+  } catch (exception& e) {
   } // avoid exception
   Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), //
   expH2 = numericalDerivative22(rotate_, bodyE, cRb);

gtsam/geometry/tests/testOrientedPlane3.cpp

@@ -183,7 +183,7 @@ TEST (OrientedPlane3, jacobian_retract) {
 
 /* ************************************************************************* */
 int main() {
-  srand(time(NULL));
+  srand(time(nullptr));
   TestResult tr;
   return TestRegistry::runAllTests(tr);
 }

gtsam/geometry/tests/testPose3.cpp

@@ -1008,7 +1008,14 @@ TEST(Pose3, print) {
   // Generate the expected output
   std::stringstream expected;
   Point3 translation(1, 2, 3);
+
+#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
+  expected << "1\n"
+              "2\n"
+              "3;\n";
+#else
   expected << '[' << translation.x() << ", " << translation.y() << ", " << translation.z() << "]\';";
+#endif
 
   // reset cout to the original stream
   std::cout.rdbuf(oldbuf);

gtsam/geometry/tests/testUnit3.cpp

@@ -495,7 +495,7 @@ TEST(actualH, Serialization) {
 
 /* ************************************************************************* */
 int main() {
-  srand(time(NULL));
+  srand(time(nullptr));
   TestResult tr;
   return TestRegistry::runAllTests(tr);
 }

gtsam/inference/BayesTree-inst.h

@@ -125,7 +125,7 @@ namespace gtsam {
   void BayesTree<CLIQUE>::addClique(const sharedClique& clique, const sharedClique& parent_clique) {
     for(Key j: clique->conditional()->frontals())
       nodes_[j] = clique;
-    if (parent_clique != NULL) {
+    if (parent_clique != nullptr) {
       clique->parent_ = parent_clique;
       parent_clique->children.push_back(clique);
     } else {
@@ -430,7 +430,7 @@ namespace gtsam {
   template <class CLIQUE>
   void BayesTree<CLIQUE>::removePath(sharedClique clique, BayesNetType* bn,
                                      Cliques* orphans) {
-    // base case is NULL, if so we do nothing and return empties above
+    // base case is nullptr, if so we do nothing and return empties above
     if (clique) {
       // remove the clique from orphans in case it has been added earlier
       orphans->remove(clique);

gtsam/inference/FactorGraph-inst.h

@@ -55,8 +55,8 @@ bool FactorGraph<FACTOR>::equals(const This& fg, double tol) const {
   // check whether the factors are the same, in same order.
   for (size_t i = 0; i < factors_.size(); i++) {
     sharedFactor f1 = factors_[i], f2 = fg.factors_[i];
-    if (f1 == NULL && f2 == NULL) continue;
+    if (f1 == nullptr && f2 == nullptr) continue;
-    if (f1 == NULL || f2 == NULL) return false;
+    if (f1 == nullptr || f2 == nullptr) return false;
     if (!f1->equals(*f2, tol)) return false;
   }
   return true;

gtsam/inference/FactorGraph.h

@@ -353,7 +353,7 @@ class FactorGraph {
    */
   void resize(size_t size) { factors_.resize(size); }
 
-  /** delete factor without re-arranging indexes by inserting a NULL pointer
+  /** delete factor without re-arranging indexes by inserting a nullptr pointer
    */
   void remove(size_t i) { factors_[i].reset(); }
 

gtsam/inference/Ordering.cpp

@@ -91,7 +91,7 @@ Ordering Ordering::ColamdConstrained(const VariableIndex& variableIndex,
 
   assert((size_t)count == variableIndex.nEntries());
 
-  //double* knobs = NULL; /* colamd arg 6: parameters (uses defaults if NULL) */
+  //double* knobs = nullptr; /* colamd arg 6: parameters (uses defaults if nullptr) */
   double knobs[CCOLAMD_KNOBS];
   ccolamd_set_defaults(knobs);
   knobs[CCOLAMD_DENSE_ROW] = -1;
@@ -235,7 +235,7 @@ Ordering Ordering::Metis(const MetisIndex& met) {
 
   int outputError;
 
-  outputError = METIS_NodeND(&size, &xadj[0], &adj[0], NULL, NULL, &perm[0],
+  outputError = METIS_NodeND(&size, &xadj[0], &adj[0], nullptr, nullptr, &perm[0],
       &iperm[0]);
   Ordering result;
 

gtsam/linear/GaussianBayesNet.h

@@ -143,7 +143,7 @@ namespace gtsam {
      *        allocateVectorValues */
     VectorValues gradientAtZero() const;
 
-    /** Mahalanobis norm error. */
+    /** 0.5 * sum of squared Mahalanobis distances. */
     double error(const VectorValues& x) const;
 
     /**

gtsam/linear/GaussianBayesTree.h

@@ -106,7 +106,7 @@ namespace gtsam {
      * @return A VectorValues storing the gradient. */
     VectorValues gradientAtZero() const;
 
-    /** Mahalanobis norm error. */
+    /** 0.5 * sum of squared Mahalanobis distances. */
     double error(const VectorValues& x) const;
 
     /** Computes the determinant of a GassianBayesTree, as if the Bayes tree is reorganized into a

gtsam/linear/JacobianFactor.cpp

@@ -840,7 +840,7 @@ GaussianConditional::shared_ptr JacobianFactor::splitConditional(size_t nrFronta
 
   if (!model_) {
     throw std::invalid_argument(
-        "JacobianFactor::splitConditional cannot be  given a NULL noise model");
+        "JacobianFactor::splitConditional cannot be  given a nullptr noise model");
   }
 
   if (nrFrontals > size()) {

gtsam/linear/JacobianFactor.h

@@ -398,7 +398,7 @@ namespace gtsam {
      *  @param keys in some order
      *  @param diemnsions of the variables in same order
      *  @param m output dimension
-     *  @param model noise model (default NULL)
+     *  @param model noise model (default nullptr)
      */
     template<class KEYS, class DIMENSIONS>
     JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m,

gtsam/linear/LossFunctions.cpp

@@ -153,7 +153,7 @@ void Fair::print(const std::string &s="") const
 
 bool Fair::equals(const Base &expected, double tol) const {
   const Fair* p = dynamic_cast<const Fair*> (&expected);
-  if (p == NULL) return false;
+  if (p == nullptr) return false;
   return std::abs(c_ - p->c_ ) < tol;
 }
 
@@ -190,7 +190,7 @@ void Huber::print(const std::string &s="") const {
 
 bool Huber::equals(const Base &expected, double tol) const {
   const Huber* p = dynamic_cast<const Huber*>(&expected);
-  if (p == NULL) return false;
+  if (p == nullptr) return false;
   return std::abs(k_ - p->k_) < tol;
 }
 
@@ -223,7 +223,7 @@ void Cauchy::print(const std::string &s="") const {
 
 bool Cauchy::equals(const Base &expected, double tol) const {
   const Cauchy* p = dynamic_cast<const Cauchy*>(&expected);
-  if (p == NULL) return false;
+  if (p == nullptr) return false;
   return std::abs(ksquared_ - p->ksquared_) < tol;
 }
 
@@ -266,7 +266,7 @@ void Tukey::print(const std::string &s="") const {
 
 bool Tukey::equals(const Base &expected, double tol) const {
   const Tukey* p = dynamic_cast<const Tukey*>(&expected);
-  if (p == NULL) return false;
+  if (p == nullptr) return false;
   return std::abs(c_ - p->c_) < tol;
 }
 
@@ -296,7 +296,7 @@ void Welsch::print(const std::string &s="") const {
 
 bool Welsch::equals(const Base &expected, double tol) const {
   const Welsch* p = dynamic_cast<const Welsch*>(&expected);
-  if (p == NULL) return false;
+  if (p == nullptr) return false;
   return std::abs(c_ - p->c_) < tol;
 }
 
@@ -330,7 +330,7 @@ void GemanMcClure::print(const std::string &s="") const {
 
 bool GemanMcClure::equals(const Base &expected, double tol) const {
   const GemanMcClure* p = dynamic_cast<const GemanMcClure*>(&expected);
-  if (p == NULL) return false;
+  if (p == nullptr) return false;
   return std::abs(c_ - p->c_) < tol;
 }
 
@@ -372,7 +372,7 @@ void DCS::print(const std::string &s="") const {
 
 bool DCS::equals(const Base &expected, double tol) const {
   const DCS* p = dynamic_cast<const DCS*>(&expected);
-  if (p == NULL) return false;
+  if (p == nullptr) return false;
   return std::abs(c_ - p->c_) < tol;
 }
 
@@ -411,7 +411,7 @@ void L2WithDeadZone::print(const std::string &s="") const {
 
 bool L2WithDeadZone::equals(const Base &expected, double tol) const {
   const L2WithDeadZone* p = dynamic_cast<const L2WithDeadZone*>(&expected);
-  if (p == NULL) return false;
+  if (p == nullptr) return false;
   return std::abs(k_ - p->k_) < tol;
 }
 

gtsam/linear/NoiseModel.cpp

@@ -25,7 +25,6 @@
 #include <cmath>
 #include <iostream>
 #include <limits>
-#include <random>
 #include <stdexcept>
 #include <typeinfo>
 
@@ -134,7 +133,7 @@ void Gaussian::print(const string& name) const {
 /* ************************************************************************* */
 bool Gaussian::equals(const Base& expected, double tol) const {
   const Gaussian* p = dynamic_cast<const Gaussian*> (&expected);
-  if (p == NULL) return false;
+  if (p == nullptr) return false;
   if (typeid(*this) != typeid(*p)) return false;
   //if (!sqrt_information_) return true; // ALEX todo;
   return equal_with_abs_tol(R(), p->R(), sqrt(tol));
@@ -157,7 +156,7 @@ Vector Gaussian::unwhiten(const Vector& v) const {
 }
 
 /* ************************************************************************* */
-double Gaussian::Mahalanobis(const Vector& v) const {
+double Gaussian::squaredMahalanobisDistance(const Vector& v) const {
   // Note: for Diagonal, which does ediv_, will be correct for constraints
   Vector w = whiten(v);
   return w.dot(w);
@@ -573,7 +572,7 @@ void Isotropic::print(const string& name) const {
 }
 
 /* ************************************************************************* */
-double Isotropic::Mahalanobis(const Vector& v) const {
+double Isotropic::squaredMahalanobisDistance(const Vector& v) const {
   return v.dot(v) * invsigma_ * invsigma_;
 }
 
@@ -625,7 +624,7 @@ void Robust::print(const std::string& name) const {
 
 bool Robust::equals(const Base& expected, double tol) const {
   const Robust* p = dynamic_cast<const Robust*> (&expected);
-  if (p == NULL) return false;
+  if (p == nullptr) return false;
   return noise_->equals(*p->noise_,tol) && robust_->equals(*p->robust_,tol);
 }
 

gtsam/linear/NoiseModel.h

@@ -207,12 +207,25 @@ namespace gtsam {
       virtual Vector unwhiten(const Vector& v) const;
 
       /**
-       * Mahalanobis distance v'*R'*R*v = <R*v,R*v>
+       * Squared Mahalanobis distance v'*R'*R*v = <R*v,R*v>
        */
-      virtual double Mahalanobis(const Vector& v) const;
+      virtual double squaredMahalanobisDistance(const Vector& v) const;
+
+      /**
+       * Mahalanobis distance
+       */
+      virtual double mahalanobisDistance(const Vector& v) const {
+        return std::sqrt(squaredMahalanobisDistance(v));
+      }
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+      virtual double Mahalanobis(const Vector& v) const {
+        return squaredMahalanobisDistance(v);
+      }
+#endif
 
       inline virtual double distance(const Vector& v) const {
-        return Mahalanobis(v);
+        return squaredMahalanobisDistance(v);
       }
 
       /**
@@ -564,7 +577,7 @@ namespace gtsam {
       }
 
       virtual void print(const std::string& name) const;
-      virtual double Mahalanobis(const Vector& v) const;
+      virtual double squaredMahalanobisDistance(const Vector& v) const;
       virtual Vector whiten(const Vector& v) const;
       virtual Vector unwhiten(const Vector& v) const;
       virtual Matrix Whiten(const Matrix& H) const;
@@ -616,7 +629,7 @@ namespace gtsam {
       virtual bool isUnit() const { return true; }
 
       virtual void print(const std::string& name) const;
-      virtual double Mahalanobis(const Vector& v) const {return v.dot(v); }
+      virtual double squaredMahalanobisDistance(const Vector& v) const {return v.dot(v); }
       virtual Vector whiten(const Vector& v) const { return v; }
       virtual Vector unwhiten(const Vector& v) const { return v; }
       virtual Matrix Whiten(const Matrix& H) const { return H; }

gtsam/linear/Sampler.cpp

@@ -11,6 +11,8 @@
 
 /**
  * @file Sampler.cpp
+ * @brief sampling from a diagonal NoiseModel
+ * @author Frank Dellaert
  * @author Alex Cunningham
  */
 
@@ -18,25 +20,16 @@
 namespace gtsam {
 
 /* ************************************************************************* */
-Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed)
+Sampler::Sampler(const noiseModel::Diagonal::shared_ptr& model,
-  : model_(model), generator_(static_cast<unsigned>(seed))
+                 uint_fast64_t seed)
-{
+    : model_(model), generator_(seed) {}
-}
 
 /* ************************************************************************* */
-Sampler::Sampler(const Vector& sigmas, int32_t seed)
+Sampler::Sampler(const Vector& sigmas, uint_fast64_t seed)
-: model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(static_cast<unsigned>(seed))
+    : model_(noiseModel::Diagonal::Sigmas(sigmas, true)), generator_(seed) {}
-{
-}
 
 /* ************************************************************************* */
-Sampler::Sampler(int32_t seed)
+Vector Sampler::sampleDiagonal(const Vector& sigmas) const {
-: generator_(static_cast<unsigned>(seed))
-{
-}
-
-/* ************************************************************************* */
-Vector Sampler::sampleDiagonal(const Vector& sigmas) {
   size_t d = sigmas.size();
   Vector result(d);
   for (size_t i = 0; i < d; i++) {
@@ -55,18 +48,23 @@ Vector Sampler::sampleDiagonal(const Vector& sigmas) {
 }
 
 /* ************************************************************************* */
-Vector Sampler::sample() {
+Vector Sampler::sample() const {
   assert(model_.get());
   const Vector& sigmas = model_->sigmas();
   return sampleDiagonal(sigmas);
 }
 
 /* ************************************************************************* */
-Vector Sampler::sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) {
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+Sampler::Sampler(uint_fast64_t seed) : generator_(seed) {}
+
+Vector Sampler::sampleNewModel(
+    const noiseModel::Diagonal::shared_ptr& model) const {
   assert(model.get());
   const Vector& sigmas = model->sigmas();
   return sampleDiagonal(sigmas);
 }
+#endif
 /* ************************************************************************* */
 
-} // \namespace gtsam
+}  // namespace gtsam

gtsam/linear/Sampler.h

@@ -10,9 +10,9 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @brief sampling that can be parameterized using a NoiseModel to generate samples from
  * @file Sampler.h
- * the given distribution
+ * @brief sampling from a NoiseModel
+ * @author Frank Dellaert
  * @author Alex Cunningham
  */
 
@@ -27,67 +27,74 @@ namespace gtsam {
 /**
  * Sampling structure that keeps internal random number generators for
  * diagonal distributions specified by NoiseModel
- *
- * This is primarily to allow for variable seeds, and does roughly the same
- * thing as sample() in NoiseModel.
  */
 class GTSAM_EXPORT Sampler {
-protected:
+ protected:
   /** noiseModel created at generation */
   noiseModel::Diagonal::shared_ptr model_;
 
   /** generator */
-  std::mt19937_64 generator_;
+  mutable std::mt19937_64 generator_;
 
-public:
+ public:
   typedef boost::shared_ptr<Sampler> shared_ptr;
 
+  /// @name constructors
+  /// @{
+
   /**
    * Create a sampler for the distribution specified by a diagonal NoiseModel
    * with a manually specified seed
    *
    * NOTE: do not use zero as a seed, it will break the generator
    */
-  Sampler(const noiseModel::Diagonal::shared_ptr& model, int32_t seed = 42u);
+  explicit Sampler(const noiseModel::Diagonal::shared_ptr& model,
+                   uint_fast64_t seed = 42u);
 
   /**
-   * Create a sampler for a distribution specified by a vector of sigmas directly
+   * Create a sampler for a distribution specified by a vector of sigmas
+   * directly
    *
    * NOTE: do not use zero as a seed, it will break the generator
    */
-  Sampler(const Vector& sigmas, int32_t seed = 42u);
+  explicit Sampler(const Vector& sigmas, uint_fast64_t seed = 42u);
 
-  /**
+  /// @}
-   * Create a sampler without a given noisemodel - pass in to sample
+  /// @name access functions
-   *
+  /// @{
-   * NOTE: do not use zero as a seed, it will break the generator
+
-   */
+  size_t dim() const {
-  Sampler(int32_t seed = 42u);
+    assert(model_.get());
+    return model_->dim();
+  }
+
+  Vector sigmas() const {
+    assert(model_.get());
+    return model_->sigmas();
+  }
 
-  /** access functions */
-  size_t dim() const { assert(model_.get()); return model_->dim(); }
-  Vector sigmas() const { assert(model_.get()); return model_->sigmas(); }
   const noiseModel::Diagonal::shared_ptr& model() const { return model_; }
 
-  /**
+  /// @}
-   * sample from distribution
+  /// @name basic functionality
-   * NOTE: not const due to need to update the underlying generator
+  /// @{
-   */
-  Vector sample();
 
-  /**
+  /// sample from distribution
-   * Sample from noisemodel passed in as an argument,
+  Vector sample() const;
-   * can be used without having initialized a model for the system.
-   *
-   * NOTE: not const due to need to update the underlying generator
-   */
-  Vector sampleNewModel(const noiseModel::Diagonal::shared_ptr& model);
 
-protected:
+  /// @}
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
+  /// @name Deprecated
+  /// @{
+  explicit Sampler(uint_fast64_t seed = 42u);
+  Vector sampleNewModel(const noiseModel::Diagonal::shared_ptr& model) const;
+  /// @}
+#endif
+
+ protected:
   /** given sigmas for a diagonal model, returns a sample */
-  Vector sampleDiagonal(const Vector& sigmas);
+  Vector sampleDiagonal(const Vector& sigmas) const;
-
 };
 
-} // \namespace gtsam
+}  // namespace gtsam

gtsam/linear/VectorValues.cpp

@@ -51,7 +51,11 @@ namespace gtsam {
       Key key;
       size_t n;
       boost::tie(key, n) = v;
+#ifdef TBB_GREATER_EQUAL_2020
       values_.emplace(key, x.segment(j, n));
+#else
+      values_.insert(std::make_pair(key, x.segment(j, n)));
+#endif
       j += n;
     }
   }
@@ -60,7 +64,11 @@ namespace gtsam {
   VectorValues::VectorValues(const Vector& x, const Scatter& scatter) {
     size_t j = 0;
     for (const SlotEntry& v : scatter) {
+#ifdef TBB_GREATER_EQUAL_2020
       values_.emplace(v.key, x.segment(j, v.dimension));
+#else
+      values_.insert(std::make_pair(v.key, x.segment(j, v.dimension)));
+#endif
       j += v.dimension;
     }
   }
@@ -70,7 +78,11 @@ namespace gtsam {
   {
     VectorValues result;
     for(const KeyValuePair& v: other)
+#ifdef TBB_GREATER_EQUAL_2020
       result.values_.emplace(v.first, Vector::Zero(v.second.size()));
+#else
+      result.values_.insert(std::make_pair(v.first, Vector::Zero(v.second.size())));
+#endif
     return result;
   }
 
@@ -86,7 +98,11 @@ namespace gtsam {
 
   /* ************************************************************************* */
   VectorValues::iterator VectorValues::emplace(Key j, const Vector& value) {
+#ifdef TBB_GREATER_EQUAL_2020
     std::pair<iterator, bool> result = values_.emplace(j, value);
+#else
+    std::pair<iterator, bool> result = values_.insert(std::make_pair(j, value));
+#endif
     if(!result.second)
       throw std::invalid_argument(
       "Requested to emplace variable '" + DefaultKeyFormatter(j)
@@ -266,7 +282,11 @@ namespace gtsam {
     VectorValues result;
     // The result.end() hint here should result in constant-time inserts
     for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2)
+#ifdef TBB_GREATER_EQUAL_2020
       result.values_.emplace(j1->first, j1->second + j2->second);
+#else
+      result.values_.insert(std::make_pair(j1->first, j1->second + j2->second));
+#endif
 
     return result;
   }
@@ -324,7 +344,11 @@ namespace gtsam {
     VectorValues result;
     // The result.end() hint here should result in constant-time inserts
     for(const_iterator j1 = begin(), j2 = c.begin(); j1 != end(); ++j1, ++j2)
+#ifdef TBB_GREATER_EQUAL_2020
       result.values_.emplace(j1->first, j1->second - j2->second);
+#else
+      result.values_.insert(std::make_pair(j1->first, j1->second - j2->second));
+#endif
 
     return result;
   }
@@ -340,7 +364,11 @@ namespace gtsam {
   {
     VectorValues result;
     for(const VectorValues::KeyValuePair& key_v: v)
+#ifdef TBB_GREATER_EQUAL_2020
       result.values_.emplace(key_v.first, a * key_v.second);
+#else
+      result.values_.insert(std::make_pair(key_v.first, a * key_v.second));
+#endif
     return result;
   }
 

gtsam/linear/VectorValues.h

@@ -198,7 +198,11 @@ namespace gtsam {
      *  exist, it is inserted and an iterator pointing to the new element, along with 'true', is
      *  returned. */
     std::pair<iterator, bool> tryInsert(Key j, const Vector& value) {
-      return values_.emplace(j, value); 
+#ifdef TBB_GREATER_EQUAL_2020
+      return values_.emplace(j, value);
+#else
+      return values_.insert(std::make_pair(j, value));
+#endif
     }
 
     /** Erase the vector with the given key, or throw std::out_of_range if it does not exist */

gtsam/linear/tests/testJacobianFactor.cpp

@@ -253,7 +253,7 @@ TEST(JacobianFactor, error)
 /* ************************************************************************* */
 TEST(JacobianFactor, matrices_NULL)
 {
-  // Make sure everything works with NULL noise model
+  // Make sure everything works with nullptr noise model
   JacobianFactor factor(simple::terms, simple::b);
 
   Matrix jacobianExpected(3, 9);

gtsam/linear/tests/testNoiseModel.cpp

@@ -68,10 +68,10 @@ TEST(NoiseModel, constructors)
   for(Gaussian::shared_ptr mi: m)
     EXPECT(assert_equal(unwhitened,mi->unwhiten(whitened)));
 
-  // test Mahalanobis distance
+  // test squared Mahalanobis distance
   double distance = 5*5+10*10+15*15;
   for(Gaussian::shared_ptr mi: m)
-    DOUBLES_EQUAL(distance,mi->Mahalanobis(unwhitened),1e-9);
+    DOUBLES_EQUAL(distance,mi->squaredMahalanobisDistance(unwhitened),1e-9);
 
   // test R matrix
   for(Gaussian::shared_ptr mi: m)

gtsam/linear/tests/testSampler.cpp

@@ -10,8 +10,10 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file testSampler
+ * @file testSampler.cpp
+ * @brief unit tests for Sampler class
  * @author Alex Cunningham
+ * @author Frank Dellaert
  */
 
 #include <CppUnitLite/TestHarness.h>
@@ -22,14 +24,15 @@ using namespace gtsam;
 
 const double tol = 1e-5;
 
+static const Vector3 kSigmas(1.0, 0.1, 0.0);
+
 /* ************************************************************************* */
 TEST(testSampler, basic) {
-  Vector sigmas = Vector3(1.0, 0.1, 0.0);
+  auto model = noiseModel::Diagonal::Sigmas(kSigmas);
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(sigmas);
   char seed = 'A';
   Sampler sampler1(model, seed), sampler2(model, 1), sampler3(model, 1);
-  EXPECT(assert_equal(sigmas, sampler1.sigmas()));
+  EXPECT(assert_equal(kSigmas, sampler1.sigmas()));
-  EXPECT(assert_equal(sigmas, sampler2.sigmas()));
+  EXPECT(assert_equal(kSigmas, sampler2.sigmas()));
   EXPECT_LONGS_EQUAL(3, sampler1.dim());
   EXPECT_LONGS_EQUAL(3, sampler2.dim());
   Vector actual1 = sampler1.sample();
@@ -38,5 +41,8 @@ TEST(testSampler, basic) {
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */

gtsam/navigation/AHRSFactor.cpp

@@ -114,7 +114,7 @@ void AHRSFactor::print(const string& s,
 //------------------------------------------------------------------------------
 bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const {
   const This *e = dynamic_cast<const This*>(&other);
-  return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
+  return e != nullptr && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
 }
 
 //------------------------------------------------------------------------------

gtsam/navigation/AttitudeFactor.cpp

@@ -52,7 +52,7 @@ void Rot3AttitudeFactor::print(const string& s,
 bool Rot3AttitudeFactor::equals(const NonlinearFactor& expected,
     double tol) const {
   const This* e = dynamic_cast<const This*>(&expected);
-  return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
+  return e != nullptr && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
       && this->bRef_.equals(e->bRef_, tol);
 }
 
@@ -69,7 +69,7 @@ void Pose3AttitudeFactor::print(const string& s,
 bool Pose3AttitudeFactor::equals(const NonlinearFactor& expected,
     double tol) const {
   const This* e = dynamic_cast<const This*>(&expected);
-  return e != NULL && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
+  return e != nullptr && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol)
       && this->bRef_.equals(e->bRef_, tol);
 }
 

gtsam/navigation/CombinedImuFactor.cpp

@@ -174,7 +174,7 @@ void CombinedImuFactor::print(const string& s,
 //------------------------------------------------------------------------------
 bool CombinedImuFactor::equals(const NonlinearFactor& other, double tol) const {
   const This* e = dynamic_cast<const This*>(&other);
-  return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
+  return e != nullptr && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
 }
 
 //------------------------------------------------------------------------------

gtsam/navigation/GPSFactor.cpp

@@ -32,7 +32,7 @@ void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
 //***************************************************************************
 bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const {
   const This* e = dynamic_cast<const This*>(&expected);
-  return e != NULL && Base::equals(*e, tol) && traits<Point3>::Equals(nT_, e->nT_, tol);
+  return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(nT_, e->nT_, tol);
 }
 
 //***************************************************************************
@@ -73,7 +73,7 @@ void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const
 //***************************************************************************
 bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const {
   const This* e = dynamic_cast<const This*>(&expected);
-  return e != NULL && Base::equals(*e, tol) &&
+  return e != nullptr && Base::equals(*e, tol) &&
          traits<Point3>::Equals(nT_, e->nT_, tol);
 }
 

gtsam/navigation/NavState.h

@@ -64,7 +64,7 @@ public:
       R_(pose.rotation()), t_(pose.translation()), v_(v) {
   }
   /// Construct from SO(3) and R^6
-  NavState(const Matrix3& R, const Vector9 tv) :
+  NavState(const Matrix3& R, const Vector6& tv) :
       R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
   }
   /// Named constructor with derivatives

gtsam/navigation/PreintegrationParams.h

@@ -29,6 +29,13 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
   bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
   Vector3 n_gravity; ///< Gravity vector in nav frame
 
+  /// Default constructor for serialization only
+  PreintegrationParams()
+      : accelerometerCovariance(I_3x3),
+        integrationCovariance(I_3x3),
+        use2ndOrderCoriolis(false),
+        n_gravity(0, 0, -1) {}
+
   /// The Params constructor insists on getting the navigation frame gravity vector
   /// For convenience, two commonly used conventions are provided by named constructors below
   PreintegrationParams(const Vector3& n_gravity)
@@ -60,8 +67,6 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
   bool           getUse2ndOrderCoriolis()     const { return use2ndOrderCoriolis; }
 
 protected:
-  /// Default constructor for serialization only: uninitialized!
-  PreintegrationParams() {}
 
   /** Serialization function */
   friend class boost::serialization::access;

gtsam/navigation/ScenarioRunner.h

@@ -48,7 +48,7 @@ class GTSAM_EXPORT ScenarioRunner {
   const Bias estimatedBias_;
 
   // Create two samplers for acceleration and omega noise
-  mutable Sampler gyroSampler_, accSampler_;
+  Sampler gyroSampler_, accSampler_;
 
  public:
   ScenarioRunner(const Scenario& scenario, const SharedParams& p,

gtsam/navigation/tests/testImuFactorSerialization.cpp

@@ -64,9 +64,9 @@ TEST(ImuFactor, serialization) {
 
   ImuFactor factor(1, 2, 3, 4, 5, pim);
 
-  EXPECT(equalsObj(factor));
+  EXPECT(equalsObj<ImuFactor>(factor));
-  EXPECT(equalsXML(factor));
+  EXPECT(equalsXML<ImuFactor>(factor));
-  EXPECT(equalsBinary(factor));
+  EXPECT(equalsBinary<ImuFactor>(factor));
 }
 
 /* ************************************************************************* */

gtsam/nonlinear/ISAM2Params.h

@@ -219,7 +219,7 @@ struct GTSAM_EXPORT ISAM2Params {
 
   /// When you will be removing many factors, e.g. when using ISAM2 as a
   /// fixed-lag smoother, enable this option to add factors in the first
-  /// available factor slots, to avoid accumulating NULL factor slots, at the
+  /// available factor slots, to avoid accumulating nullptr factor slots, at the
   /// cost of having to search for slots every time a factor is added.
   bool findUnusedFactorSlots;
 

gtsam/nonlinear/NonlinearEquality.h

@@ -175,6 +175,8 @@ public:
 
   /// @}
 
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
 private:
 
   /** Serialization function */
@@ -263,6 +265,8 @@ public:
     traits<X>::Print(value_, "Value");
   }
 
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
 private:
 
   /** Serialization function */
@@ -327,6 +331,8 @@ public:
     return traits<X>::Local(x1,x2);
   }
 
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
 private:
 
   /** Serialization function */

gtsam/nonlinear/NonlinearFactorGraph.cpp

@@ -54,7 +54,7 @@ void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& key
   for (size_t i = 0; i < factors_.size(); i++) {
     stringstream ss;
     ss << "Factor " << i << ": ";
-    if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter);
+    if (factors_[i] != nullptr) factors_[i]->print(ss.str(), keyFormatter);
     cout << endl;
   }
 }
@@ -67,8 +67,8 @@ void NonlinearFactorGraph::printErrors(const Values& values, const std::string&
   for (size_t i = 0; i < factors_.size(); i++) {
     stringstream ss;
     ss << "Factor " << i << ": ";
-    if (factors_[i] == NULL) {
+    if (factors_[i] == nullptr) {
-      cout << "NULL" << endl;
+      cout << "nullptr" << endl;
     } else {
       factors_[i]->print(ss.str(), keyFormatter);
       cout << "error = " << factors_[i]->error(values) << endl;

gtsam/nonlinear/tests/testCallRecord.cpp

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

gtsam/nonlinear/utilities.h

@@ -237,13 +237,16 @@ Values localToWorld(const Values& local, const Pose2& base,
       // if value is a Pose2, compose it with base pose
       Pose2 pose = local.at<Pose2>(key);
       world.insert(key, base.compose(pose));
-    } catch (std::exception e1) {
+    } catch (const std::exception& e1) {
       try {
         // if value is a Point2, transform it from base pose
         Point2 point = local.at<Point2>(key);
         world.insert(key, base.transformFrom(point));
-      } catch (std::exception e2) {
+      } catch (const std::exception& e2) {
         // if not Pose2 or Point2, do nothing
+        #ifndef NDEBUG
+          std::cerr << "Values[key] is neither Pose2 nor Point2, so skip" << std::endl;
+        #endif
       }
     }
   }

gtsam/slam/AntiFactor.h

@@ -67,7 +67,7 @@ namespace gtsam {
     /** equals */
     virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
       const This *e =  dynamic_cast<const This*> (&expected);
-      return e != NULL && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol);
+      return e != nullptr && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol);
     }
 
     /** implement functions needed to derive from Factor */

gtsam/slam/BetweenFactor.h

@@ -81,7 +81,7 @@ namespace gtsam {
     /** equals */
     virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
       const This *e =  dynamic_cast<const This*> (&expected);
-      return e != NULL && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol);
+      return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(this->measured_, e->measured_, tol);
     }
 
     /** implement functions needed to derive from Factor */

gtsam/slam/EssentialMatrixConstraint.cpp

@@ -37,7 +37,7 @@ void EssentialMatrixConstraint::print(const std::string& s,
 bool EssentialMatrixConstraint::equals(const NonlinearFactor& expected,
     double tol) const {
   const This *e = dynamic_cast<const This*>(&expected);
-  return e != NULL && Base::equals(*e, tol)
+  return e != nullptr && Base::equals(*e, tol)
       && this->measuredE_.equals(e->measuredE_, tol);
 }
 

gtsam/slam/JacobianFactorSVD.h

@@ -65,7 +65,7 @@ public:
       Base() {
     size_t numKeys = Enull.rows() / ZDim;
     size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()?
-    // PLAIN NULL SPACE TRICK
+    // PLAIN nullptr SPACE TRICK
     // Matrix Q = Enull * Enull.transpose();
     // for(const KeyMatrixZD& it: Fblocks)
     //   QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));

gtsam/slam/OrientedPlane3Factor.cpp

@@ -31,7 +31,7 @@ void OrientedPlane3DirectionPrior::print(const string& s,
 bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected,
     double tol) const {
   const This* e = dynamic_cast<const This*>(&expected);
-  return e != NULL && Base::equals(*e, tol)
+  return e != nullptr && Base::equals(*e, tol)
       && this->measured_p_.equals(e->measured_p_, tol);
 }
 

gtsam/slam/PoseRotationPrior.h

@@ -62,7 +62,7 @@ public:
   /** equals specialized to this factor */
   virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
     const This *e = dynamic_cast<const This*> (&expected);
-    return e != NULL && Base::equals(*e, tol) && measured_.equals(e->measured_, tol);
+    return e != nullptr && Base::equals(*e, tol) && measured_.equals(e->measured_, tol);
   }
 
   /** print contents */

gtsam/slam/PoseTranslationPrior.h

@@ -76,7 +76,7 @@ public:
   /** equals specialized to this factor */
   virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
     const This *e = dynamic_cast<const This*> (&expected);
-    return e != NULL && Base::equals(*e, tol) && traits<Translation>::Equals(measured_, e->measured_, tol);
+    return e != nullptr && Base::equals(*e, tol) && traits<Translation>::Equals(measured_, e->measured_, tol);
   }
 
   /** print contents */

gtsam/slam/PriorFactor.h

@@ -85,7 +85,7 @@ namespace gtsam {
     /** equals */
     virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
       const This* e = dynamic_cast<const This*> (&expected);
-      return e != NULL && Base::equals(*e, tol) && traits<T>::Equals(prior_, e->prior_, tol);
+      return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(prior_, e->prior_, tol);
     }
 
     /** implement functions needed to derive from Factor */

gtsam/slam/dataset.cpp

@@ -252,7 +252,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
   is.seekg(0, ios::beg);
 
   // If asked, create a sampler with random number generator
-  Sampler sampler;
+  std::unique_ptr<Sampler> sampler;
   if (addNoise) {
     noiseModel::Diagonal::shared_ptr noise;
     if (model)
@@ -261,7 +261,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
       throw invalid_argument(
           "gtsam::load2D: invalid noise model for adding noise"
               "(current version assumes diagonal noise model)!");
-    sampler = Sampler(noise);
+    sampler.reset(new Sampler(noise));
   }
 
   // Parse the pose constraints
@@ -289,7 +289,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
         model = modelInFile;
 
       if (addNoise)
-        l1Xl2 = l1Xl2.retract(sampler.sample());
+        l1Xl2 = l1Xl2.retract(sampler->sample());
 
       // Insert vertices if pure odometry file
       if (!initial->exists(id1))

gtsam/slam/tests/testOrientedPlane3Factor.cpp

@@ -175,7 +175,7 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
 
 /* ************************************************************************* */
 int main() {
-  srand(time(NULL));
+  srand(time(nullptr));
   TestResult tr;
   return TestRegistry::runAllTests(tr);
 }

gtsam_unstable/base/BTree.h

@@ -72,7 +72,7 @@ namespace gtsam {
     }; // Node
 
     // We store a shared pointer to the root of the functional tree
-    // composed of Node classes. If root_==NULL, the tree is empty.
+    // composed of Node classes. If root_==nullptr, the tree is empty.
     typedef boost::shared_ptr<const Node> sharedNode;
     sharedNode root_;
 
@@ -223,7 +223,7 @@ namespace gtsam {
 
     /** Return height of the tree, 0 if empty */
     size_t height() const {
-      return (root_ != NULL) ? root_->height_ : 0;
+      return (root_ != nullptr) ? root_->height_ : 0;
     }
 
     /** return size of the tree */

gtsam_unstable/linear/tests/testLinearEquality.cpp

@@ -136,7 +136,7 @@ TEST(LinearEquality, error)
 /* ************************************************************************* */
 TEST(LinearEquality, matrices_NULL)
 {
-  // Make sure everything works with NULL noise model
+  // Make sure everything works with nullptr noise model
   LinearEquality factor(simple::terms, simple::b, 0);
 
   Matrix AExpected(3, 9);

gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp

@@ -39,7 +39,7 @@ bool BatchFixedLagSmoother::equals(const FixedLagSmoother& rhs,
     double tol) const {
   const BatchFixedLagSmoother* e =
       dynamic_cast<const BatchFixedLagSmoother*>(&rhs);
-  return e != NULL && FixedLagSmoother::equals(*e, tol)
+  return e != nullptr && FixedLagSmoother::equals(*e, tol)
       && factors_.equals(e->factors_, tol) && theta_.equals(e->theta_, tol);
 }
 
@@ -145,7 +145,7 @@ void BatchFixedLagSmoother::removeFactors(
     } else {
       // TODO: Throw an error??
       cout << "Attempting to remove a factor from slot " << slot
-          << ", but it is already NULL." << endl;
+          << ", but it is already nullptr." << endl;
     }
   }
 }
@@ -370,7 +370,7 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(
       cout << " " << DefaultKeyFormatter(key);
     }
   } else {
-    cout << " NULL";
+    cout << " nullptr";
   }
   cout << " )" << endl;
 }

gtsam_unstable/nonlinear/ConcurrentBatchFilter.cpp

@@ -38,7 +38,7 @@ void ConcurrentBatchFilter::PrintNonlinearFactor(const NonlinearFactor::shared_p
     }
     std::cout << ")" << std::endl;
   } else {
-    std::cout << "{ NULL }" << std::endl;
+    std::cout << "{ nullptr }" << std::endl;
   }
 }
 
@@ -79,7 +79,7 @@ void ConcurrentBatchFilter::PrintLinearFactor(const GaussianFactor::shared_ptr&
     }
     std::cout << ")" << std::endl;
   } else {
-    std::cout << "{ NULL }" << std::endl;
+    std::cout << "{ nullptr }" << std::endl;
   }
 }
 

gtsam_unstable/nonlinear/ConcurrentBatchSmoother.cpp

@@ -394,7 +394,7 @@ void ConcurrentBatchSmoother::PrintNonlinearFactor(const NonlinearFactor::shared
     }
     std::cout << ")" << std::endl;
   } else {
-    std::cout << "{ NULL }" << std::endl;
+    std::cout << "{ nullptr }" << std::endl;
   }
 }
 
@@ -408,7 +408,7 @@ void ConcurrentBatchSmoother::PrintLinearFactor(const GaussianFactor::shared_ptr
     }
     std::cout << ")" << std::endl;
   } else {
-    std::cout << "{ NULL }" << std::endl;
+    std::cout << "{ nullptr }" << std::endl;
   }
 }
 

gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp

@@ -58,7 +58,7 @@ bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs,
     double tol) const {
   const IncrementalFixedLagSmoother* e =
       dynamic_cast<const IncrementalFixedLagSmoother*>(&rhs);
-  return e != NULL && FixedLagSmoother::equals(*e, tol)
+  return e != nullptr && FixedLagSmoother::equals(*e, tol)
       && isam_.equals(e->isam_, tol);
 }
 

gtsam_unstable/partition/FindSeparator-inl.h

@@ -83,14 +83,14 @@ namespace gtsam { namespace partition {
     graph_t *graph;
     real_t *tpwgts2;
     ctrl_t *ctrl;
-    ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, NULL, NULL);
+    ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, nullptr, nullptr);
     ctrl->iptype = METIS_IPTYPE_GROW;
-    //if () == NULL)
+    //if () == nullptr)
     //  return METIS_ERROR_INPUT;
 
     InitRandom(ctrl->seed);
 
-    graph = SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt, NULL, NULL);
+    graph = SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt, nullptr, nullptr);
 
     AllocateWorkSpace(ctrl, graph);
 

gtsam_unstable/slam/AHRS.h

@@ -77,6 +77,8 @@ public:
   void print(const std::string& s = "") const;
 
   virtual ~AHRS();
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
 } /* namespace gtsam */

gtsam_unstable/slam/BiasedGPSFactor.h

@@ -66,7 +66,7 @@ namespace gtsam {
     /** equals */
     virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
       const This *e =  dynamic_cast<const This*> (&expected);
-      return e != NULL && Base::equals(*e, tol) && traits<Point3>::Equals(this->measured_, e->measured_, tol);
+      return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(this->measured_, e->measured_, tol);
     }
 
     /** implement functions needed to derive from Factor */

gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h

@@ -155,7 +155,7 @@ public:
   /** equals */
   virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
     const This *e =  dynamic_cast<const This*> (&expected);
-    return e != NULL && Base::equals(*e, tol)
+    return e != nullptr && Base::equals(*e, tol)
     && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol
     && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol
     && (delta_angles_ - e->delta_angles_).norm() < tol

gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h

@@ -153,7 +153,7 @@ public:
   /** equals */
   virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
     const This *e =  dynamic_cast<const This*> (&expected);
-    return e != NULL && Base::equals(*e, tol)
+    return e != nullptr && Base::equals(*e, tol)
     && (delta_pos_in_t0_ - e->delta_pos_in_t0_).norm() < tol
     && (delta_vel_in_t0_ - e->delta_vel_in_t0_).norm() < tol
     && (delta_angles_ - e->delta_angles_).norm() < tol

gtsam_unstable/slam/GaussMarkov1stOrderFactor.h

@@ -81,7 +81,7 @@ public:
   /** equals */
   virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
     const This *e =  dynamic_cast<const This*> (&expected);
-    return e != NULL && Base::equals(*e, tol);
+    return e != nullptr && Base::equals(*e, tol);
   }
 
   /** implement functions needed to derive from Factor */

gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h

@@ -135,7 +135,7 @@ public:
   /** equals */
   virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
     const This *e =  dynamic_cast<const This*> (&expected);
-    return e != NULL && Base::equals(*e, tol)
+    return e != nullptr && Base::equals(*e, tol)
       && (measurement_acc_ - e->measurement_acc_).norm() < tol
       && (measurement_gyro_ - e->measurement_gyro_).norm() < tol
       && (dt_ - e->dt_) < tol

gtsam_unstable/slam/PartialPriorFactor.h

@@ -100,7 +100,7 @@ namespace gtsam {
     /** equals */
     virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
       const This *e = dynamic_cast<const This*> (&expected);
-      return e != NULL && Base::equals(*e, tol) &&
+      return e != nullptr && Base::equals(*e, tol) &&
           gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) &&
           this->mask_ == e->mask_;
     }

gtsam_unstable/slam/PoseBetweenFactor.h

@@ -79,7 +79,7 @@ namespace gtsam {
     /** equals */
     virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
       const This *e =  dynamic_cast<const This*> (&expected);
-      return e != NULL
+      return e != nullptr
           && Base::equals(*e, tol)
           && this->measured_.equals(e->measured_, tol)
           && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));

gtsam_unstable/slam/PosePriorFactor.h

@@ -74,7 +74,7 @@ namespace gtsam {
     /** equals */
     virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
       const This* e = dynamic_cast<const This*> (&expected);
-      return e != NULL
+      return e != nullptr
           && Base::equals(*e, tol)
           && this->prior_.equals(e->prior_, tol)
           && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));

gtsam_unstable/slam/RelativeElevationFactor.cpp

@@ -38,7 +38,7 @@ Vector RelativeElevationFactor::evaluateError(const Pose3& pose, const Point3& p
 /* ************************************************************************* */
 bool RelativeElevationFactor::equals(const NonlinearFactor& expected, double tol) const {
   const This *e = dynamic_cast<const This*> (&expected);
-  return e != NULL && Base::equals(*e, tol) && std::abs(this->measured_ - e->measured_) < tol;
+  return e != nullptr && Base::equals(*e, tol) && std::abs(this->measured_ - e->measured_) < tol;
 }
 
 /* ************************************************************************* */

tests/testExtendedKalmanFilter.cpp

@@ -162,7 +162,7 @@ public:
   /** Check if two factors are equal. Note type is IndexFactor and needs cast. */
   virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
     const This *e = dynamic_cast<const This*> (&f);
-    return (e != NULL) && (key1() == e->key1()) && (key2() == e->key2());
+    return (e != nullptr) && (key1() == e->key1()) && (key2() == e->key2());
   }
 
   /**
@@ -196,7 +196,7 @@ public:
     Vector b = -evaluateError(x1, x2, A1, A2);
     SharedDiagonal constrained =
         boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
-    if (constrained.get() != NULL) {
+    if (constrained.get() != nullptr) {
       return JacobianFactor::shared_ptr(new JacobianFactor(key1(), A1, key2(),
           A2, b, constrained));
     }
@@ -292,7 +292,7 @@ public:
   /** Check if two factors are equal. Note type is IndexFactor and needs cast. */
   virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
     const This *e = dynamic_cast<const This*> (&f);
-    return (e != NULL) && Base::equals(f);
+    return (e != nullptr) && Base::equals(f);
   }
 
   /**
@@ -325,7 +325,7 @@ public:
     Vector b = -evaluateError(x1, A1);
     SharedDiagonal constrained =
         boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
-    if (constrained.get() != NULL) {
+    if (constrained.get() != nullptr) {
       return JacobianFactor::shared_ptr(new JacobianFactor(key(), A1, b, constrained));
     }
     // "Whiten" the system before converting to a Gaussian Factor

tests/testNonlinearOptimizer.cpp

@@ -20,6 +20,7 @@
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <gtsam/nonlinear/DoglegOptimizer.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@@ -339,31 +340,136 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) {
 }
 
 /* ************************************************************************* */
-TEST(NonlinearOptimizer, MoreOptimizationWithHuber) {
+TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) {
 
   NonlinearFactorGraph fg;
   fg += PriorFactor<Pose2>(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1));
-  fg += BetweenFactor<Pose2>(0, 1, Pose2(1,0,M_PI/2),
+  fg += BetweenFactor<Pose2>(0, 1, Pose2(1,1.1,M_PI/4),
                               noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(2.0),
                                                          noiseModel::Isotropic::Sigma(3,1)));
-  fg += BetweenFactor<Pose2>(1, 2, Pose2(1,0,M_PI/2),
+  fg += BetweenFactor<Pose2>(0, 1, Pose2(1,0.9,M_PI/2),
                               noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(3.0),
                                                          noiseModel::Isotropic::Sigma(3,1)));
 
   Values init;
-  init.insert(0, Pose2(10,10,0));
+  init.insert(0, Pose2(0,0,0));
-  init.insert(1, Pose2(1,0,M_PI));
+  init.insert(1, Pose2(0.961187, 0.99965, 1.1781));
-  init.insert(2, Pose2(1,1,-M_PI));
 
   Values expected;
   expected.insert(0, Pose2(0,0,0));
-  expected.insert(1, Pose2(1,0,M_PI/2));
+  expected.insert(1, Pose2(0.961187, 0.99965, 1.1781));
-  expected.insert(2, Pose2(1,1,M_PI));
+
+  LevenbergMarquardtParams lm_params;
+
+  auto gn_result = GaussNewtonOptimizer(fg, init).optimize();
+  auto lm_result = LevenbergMarquardtOptimizer(fg, init, lm_params).optimize();
+  auto dl_result = DoglegOptimizer(fg, init).optimize();
+
+  EXPECT(assert_equal(expected, gn_result, 3e-2));
+  EXPECT(assert_equal(expected, lm_result, 3e-2));
+  EXPECT(assert_equal(expected, dl_result, 3e-2));
+}
+
+/* ************************************************************************* */
+TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) {
+
+  NonlinearFactorGraph fg;
+  fg += PriorFactor<Point2>(0, Point2(0,0), noiseModel::Isotropic::Sigma(2,0.01));
+  fg += BetweenFactor<Point2>(0, 1, Point2(1,1.8),
+                              noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0),
+                                                         noiseModel::Isotropic::Sigma(2,1)));
+  fg += BetweenFactor<Point2>(0, 1, Point2(1,0.9),
+                              noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0),
+                                                         noiseModel::Isotropic::Sigma(2,1)));
+  fg += BetweenFactor<Point2>(0, 1, Point2(1,90),
+                              noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0),
+                                                         noiseModel::Isotropic::Sigma(2,1)));
+
+  Values init;
+  init.insert(0, Point2(1,1));
+  init.insert(1, Point2(1,0));
+
+  Values expected;
+  expected.insert(0, Point2(0,0));
+  expected.insert(1, Point2(1,1.85));
 
   LevenbergMarquardtParams params;
-  EXPECT(assert_equal(expected, GaussNewtonOptimizer(fg, init).optimize()));
+
-  EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(fg, init, params).optimize()));
+  auto gn_result = GaussNewtonOptimizer(fg, init).optimize();
-  EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize()));
+  auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize();
+  auto dl_result = DoglegOptimizer(fg, init).optimize();
+
+  EXPECT(assert_equal(expected, gn_result, 1e-4));
+  EXPECT(assert_equal(expected, lm_result, 1e-4));
+  EXPECT(assert_equal(expected, dl_result, 1e-4));
+}
+
+/* ************************************************************************* */
+TEST(NonlinearOptimizer, Pose2OptimizationWithHuber) {
+
+  NonlinearFactorGraph fg;
+  fg += PriorFactor<Pose2>(0, Pose2(0,0, 0), noiseModel::Isotropic::Sigma(3,0.1));
+  fg += BetweenFactor<Pose2>(0, 1, Pose2(0,9, M_PI/2),
+                              noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
+                                                         noiseModel::Isotropic::Sigma(3,1)));
+  fg += BetweenFactor<Pose2>(0, 1, Pose2(0, 11, M_PI/2),
+                              noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
+                                                         noiseModel::Isotropic::Sigma(3,1)));
+  fg += BetweenFactor<Pose2>(0, 1, Pose2(0, 10, M_PI/2),
+                             noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
+                                                        noiseModel::Isotropic::Sigma(3,1)));
+  fg += BetweenFactor<Pose2>(0, 1, Pose2(0,9, 0),
+                              noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
+                                                         noiseModel::Isotropic::Sigma(3,1)));
+
+  Values init;
+  init.insert(0, Pose2(0, 0, 0));
+  init.insert(1, Pose2(0, 10, M_PI/4));
+
+  Values expected;
+  expected.insert(0, Pose2(0, 0, 0));
+  expected.insert(1, Pose2(0, 10, 1.45212));
+
+  LevenbergMarquardtParams params;
+
+  auto gn_result = GaussNewtonOptimizer(fg, init).optimize();
+  auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize();
+  auto dl_result = DoglegOptimizer(fg, init).optimize();
+
+  EXPECT(assert_equal(expected, gn_result, 1e-1));
+  EXPECT(assert_equal(expected, lm_result, 1e-1));
+  EXPECT(assert_equal(expected, dl_result, 1e-1));
+}
+
+/* ************************************************************************* */
+TEST(NonlinearOptimizer, RobustMeanCalculation) {
+
+  NonlinearFactorGraph fg;
+
+  Values init;
+
+  Values expected;
+
+  auto huber = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(20),
+                                          noiseModel::Isotropic::Sigma(1, 1));
+
+  vector<double> pts{-10,-3,-1,1,3,10,1000};
+  for(auto pt : pts) {
+    fg += PriorFactor<double>(0, pt, huber);
+  }
+
+  init.insert(0, 100.0);
+  expected.insert(0, 3.33333333);
+
+  LevenbergMarquardtParams params;
+
+  auto gn_result = GaussNewtonOptimizer(fg, init).optimize();
+  auto lm_result = LevenbergMarquardtOptimizer(fg, init, params).optimize();
+  auto dl_result = DoglegOptimizer(fg, init).optimize();
+
+  EXPECT(assert_equal(expected, gn_result, tol));
+  EXPECT(assert_equal(expected, lm_result, tol));
+  EXPECT(assert_equal(expected, dl_result, tol));
 }
 
 /* ************************************************************************* */