Merge branch 'develop' into feature/wrap_translation_averaging

CMakeLists.txt

@@ -166,7 +166,7 @@ if(MSVC AND BUILD_SHARED_LIBS)
 endif()
 
 # Store these in variables so they are automatically replicated in GTSAMConfig.cmake and such.
-set(BOOST_FIND_MINIMUM_VERSION 1.43)
+set(BOOST_FIND_MINIMUM_VERSION 1.58)
 set(BOOST_FIND_MINIMUM_COMPONENTS serialization system filesystem thread program_options date_time timer chrono regex)
 
 find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM_COMPONENTS})
@@ -174,7 +174,7 @@ find_package(Boost ${BOOST_FIND_MINIMUM_VERSION} COMPONENTS ${BOOST_FIND_MINIMUM
 # Required components
 if(NOT Boost_SERIALIZATION_LIBRARY OR NOT Boost_SYSTEM_LIBRARY OR NOT Boost_FILESYSTEM_LIBRARY OR
     NOT Boost_THREAD_LIBRARY OR NOT Boost_DATE_TIME_LIBRARY)
-  message(FATAL_ERROR "Missing required Boost components >= v1.43, please install/upgrade Boost or configure your search paths.")
+  message(FATAL_ERROR "Missing required Boost components >= v1.58, please install/upgrade Boost or configure your search paths.")
 endif()
 
 option(GTSAM_DISABLE_NEW_TIMERS "Disables using Boost.chrono for timing" OFF)
@@ -490,7 +490,7 @@ set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION
 
 # Deb-package specific cpack
 set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev")
-set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)")
+set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.58)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)")
 
 
 ###############################################################################

INSTALL.md

@@ -13,7 +13,7 @@ $ make install
 ## Important Installation Notes
 
 1. GTSAM requires the following libraries to be installed on your system:
-    - BOOST version 1.43 or greater (install through Linux repositories or MacPorts)
+    - BOOST version 1.58 or greater (install through Linux repositories or MacPorts)
     - Cmake version 3.0 or higher
     - Support for XCode 4.3 command line tools on Mac requires CMake 2.8.8 or higher
 

README.md

@@ -13,6 +13,8 @@ mapping (SAM) in robotics and vision, using Factor Graphs and Bayes
 Networks as the underlying computing paradigm rather than sparse
 matrices.
 
+The current support matrix is:
+
 | Platform     | Compiler  | Build Status  |
 |:------------:|:---------:|:-------------:|
 | Ubuntu 18.04 | gcc/clang | ![Linux CI](https://github.com/borglab/gtsam/workflows/Linux%20CI/badge.svg) |
@@ -38,7 +40,7 @@ $ make install
 
 Prerequisites:
 
-- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
+- [Boost](http://www.boost.org/users/download/) >= 1.58 (Ubuntu: `sudo apt-get install libboost-all-dev`)
 - [CMake](http://www.cmake.org/cmake/resources/software.html) >= 3.0 (Ubuntu: `sudo apt-get install cmake`)
 - A modern compiler, i.e., at least gcc 4.7.3 on Linux.
 

examples/CombinedImuFactorsExample.cpp

@@ -0,0 +1,303 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 CombinedImuFactorsExample
+ * @brief Test example for using GTSAM ImuCombinedFactor
+ * navigation code.
+ * @author Varun Agrawal
+ */
+
+/**
+ * Example of use of the CombinedImuFactor in
+ * conjunction with GPS
+ *  - we read IMU and GPS data from a CSV file, with the following format:
+ *  A row starting with "i" is the first initial position formatted with
+ *  N, E, D, qx, qY, qZ, qW, velN, velE, velD
+ *  A row starting with "0" is an imu measurement
+ *  linAccN, linAccE, linAccD, angVelN, angVelE, angVelD
+ *  A row starting with "1" is a gps correction formatted with
+ *  N, E, D, qX, qY, qZ, qW
+ * Note that for GPS correction, we're only using the position not the
+ * rotation. The rotation is provided in the file for ground truth comparison.
+ *
+ *  See usage: ./CombinedImuFactorsExample --help
+ */
+
+#include <boost/program_options.hpp>
+
+// GTSAM related includes.
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/navigation/CombinedImuFactor.h>
+#include <gtsam/navigation/GPSFactor.h>
+#include <gtsam/navigation/ImuFactor.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/dataset.h>
+
+#include <cstring>
+#include <fstream>
+#include <iostream>
+
+using namespace gtsam;
+using namespace std;
+
+using symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
+using symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
+using symbol_shorthand::X;  // Pose3 (x,y,z,r,p,y)
+
+namespace po = boost::program_options;
+
+po::variables_map parseOptions(int argc, char* argv[]) {
+  po::options_description desc;
+  desc.add_options()("help,h", "produce help message")(
+      "data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
+      "path to the CSV file with the IMU data")(
+      "output_filename",
+      po::value<string>()->default_value("imuFactorExampleResults.csv"),
+      "path to the result file to use")("use_isam", po::bool_switch(),
+                                        "use ISAM as the optimizer");
+
+  po::variables_map vm;
+  po::store(po::parse_command_line(argc, argv, desc), vm);
+
+  if (vm.count("help")) {
+    cout << desc << "\n";
+    exit(1);
+  }
+
+  return vm;
+}
+
+Vector10 readInitialState(ifstream& file) {
+  string value;
+  // Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD)
+  Vector10 initial_state;
+  getline(file, value, ',');  // i
+  for (int i = 0; i < 9; i++) {
+    getline(file, value, ',');
+    initial_state(i) = stof(value.c_str());
+  }
+  getline(file, value, '\n');
+  initial_state(9) = stof(value.c_str());
+
+  return initial_state;
+}
+
+boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
+  // We use the sensor specs to build the noise model for the IMU factor.
+  double accel_noise_sigma = 0.0003924;
+  double gyro_noise_sigma = 0.000205689024915;
+  double accel_bias_rw_sigma = 0.004905;
+  double gyro_bias_rw_sigma = 0.000001454441043;
+  Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2);
+  Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2);
+  Matrix33 integration_error_cov =
+      I_3x3 * 1e-8;  // error committed in integrating position from velocities
+  Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
+  Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
+  Matrix66 bias_acc_omega_int =
+      I_6x6 * 1e-5;  // error in the bias used for preintegration
+
+  auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
+  // PreintegrationBase params:
+  p->accelerometerCovariance =
+      measured_acc_cov;  // acc white noise in continuous
+  p->integrationCovariance =
+      integration_error_cov;  // integration uncertainty continuous
+  // should be using 2nd order integration
+  // PreintegratedRotation params:
+  p->gyroscopeCovariance =
+      measured_omega_cov;  // gyro white noise in continuous
+  // PreintegrationCombinedMeasurements params:
+  p->biasAccCovariance = bias_acc_cov;      // acc bias in continuous
+  p->biasOmegaCovariance = bias_omega_cov;  // gyro bias in continuous
+  p->biasAccOmegaInt = bias_acc_omega_int;
+
+  return p;
+}
+
+int main(int argc, char* argv[]) {
+  string data_filename, output_filename;
+  po::variables_map var_map = parseOptions(argc, argv);
+
+  data_filename = findExampleDataFile(var_map["data_csv_path"].as<string>());
+  output_filename = var_map["output_filename"].as<string>();
+
+  // Set up output file for plotting errors
+  FILE* fp_out = fopen(output_filename.c_str(), "w+");
+  fprintf(fp_out,
+          "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,"
+          "gt_qy,gt_qz,gt_qw\n");
+
+  // Begin parsing the CSV file.  Input the first line for initialization.
+  // From there, we'll iterate through the file and we'll preintegrate the IMU
+  // or add in the GPS given the input.
+  ifstream file(data_filename.c_str());
+
+  Vector10 initial_state = readInitialState(file);
+  cout << "initial state:\n" << initial_state.transpose() << "\n\n";
+
+  // Assemble initial quaternion through GTSAM constructor
+  // ::Quaternion(w,x,y,z);
+  Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3),
+                                         initial_state(4), initial_state(5));
+  Point3 prior_point(initial_state.head<3>());
+  Pose3 prior_pose(prior_rotation, prior_point);
+  Vector3 prior_velocity(initial_state.tail<3>());
+
+  imuBias::ConstantBias prior_imu_bias;  // assume zero initial bias
+
+  int index = 0;
+
+  Values initial_values;
+
+  // insert pose at initialization
+  initial_values.insert(X(index), prior_pose);
+  initial_values.insert(V(index), prior_velocity);
+  initial_values.insert(B(index), prior_imu_bias);
+
+  // Assemble prior noise model and add it the graph.`
+  auto pose_noise_model = noiseModel::Diagonal::Sigmas(
+      (Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5)
+          .finished());  // rad,rad,rad,m, m, m
+  auto velocity_noise_model = noiseModel::Isotropic::Sigma(3, 0.1);  // m/s
+  auto bias_noise_model = noiseModel::Isotropic::Sigma(6, 1e-3);
+
+  // Add all prior factors (pose, velocity, bias) to the graph.
+  NonlinearFactorGraph graph;
+  graph.addPrior<Pose3>(X(index), prior_pose, pose_noise_model);
+  graph.addPrior<Vector3>(V(index), prior_velocity, velocity_noise_model);
+  graph.addPrior<imuBias::ConstantBias>(B(index), prior_imu_bias,
+                                        bias_noise_model);
+
+  auto p = imuParams();
+
+  std::shared_ptr<PreintegrationType> preintegrated =
+      std::make_shared<PreintegratedCombinedMeasurements>(p, prior_imu_bias);
+
+  assert(preintegrated);
+
+  // Store previous state for imu integration and latest predicted outcome.
+  NavState prev_state(prior_pose, prior_velocity);
+  NavState prop_state = prev_state;
+  imuBias::ConstantBias prev_bias = prior_imu_bias;
+
+  // Keep track of total error over the entire run as simple performance metric.
+  double current_position_error = 0.0, current_orientation_error = 0.0;
+
+  double output_time = 0.0;
+  double dt = 0.005;  // The real system has noise, but here, results are nearly
+                      // exactly the same, so keeping this for simplicity.
+
+  // All priors have been set up, now iterate through the data file.
+  while (file.good()) {
+    // Parse out first value
+    string value;
+    getline(file, value, ',');
+    int type = stoi(value.c_str());
+
+    if (type == 0) {  // IMU measurement
+      Vector6 imu;
+      for (int i = 0; i < 5; ++i) {
+        getline(file, value, ',');
+        imu(i) = stof(value.c_str());
+      }
+      getline(file, value, '\n');
+      imu(5) = stof(value.c_str());
+
+      // Adding the IMU preintegration.
+      preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);
+
+    } else if (type == 1) {  // GPS measurement
+      Vector7 gps;
+      for (int i = 0; i < 6; ++i) {
+        getline(file, value, ',');
+        gps(i) = stof(value.c_str());
+      }
+      getline(file, value, '\n');
+      gps(6) = stof(value.c_str());
+
+      index++;
+
+      // Adding IMU factor and GPS factor and optimizing.
+      auto preint_imu_combined =
+          dynamic_cast<const PreintegratedCombinedMeasurements&>(
+              *preintegrated);
+      CombinedImuFactor imu_factor(X(index - 1), V(index - 1), X(index),
+                                   V(index), B(index - 1), B(index),
+                                   preint_imu_combined);
+      graph.add(imu_factor);
+
+      auto correction_noise = noiseModel::Isotropic::Sigma(3, 1.0);
+      GPSFactor gps_factor(X(index),
+                           Point3(gps(0),   // N,
+                                  gps(1),   // E,
+                                  gps(2)),  // D,
+                           correction_noise);
+      graph.add(gps_factor);
+
+      // Now optimize and compare results.
+      prop_state = preintegrated->predict(prev_state, prev_bias);
+      initial_values.insert(X(index), prop_state.pose());
+      initial_values.insert(V(index), prop_state.v());
+      initial_values.insert(B(index), prev_bias);
+
+      LevenbergMarquardtParams params;
+      params.setVerbosityLM("SUMMARY");
+      LevenbergMarquardtOptimizer optimizer(graph, initial_values, params);
+      Values result = optimizer.optimize();
+
+      // Overwrite the beginning of the preintegration for the next step.
+      prev_state =
+          NavState(result.at<Pose3>(X(index)), result.at<Vector3>(V(index)));
+      prev_bias = result.at<imuBias::ConstantBias>(B(index));
+
+      // Reset the preintegration object.
+      preintegrated->resetIntegrationAndSetBias(prev_bias);
+
+      // Print out the position and orientation error for comparison.
+      Vector3 result_position = prev_state.pose().translation();
+      Vector3 position_error = result_position - gps.head<3>();
+      current_position_error = position_error.norm();
+
+      Quaternion result_quat = prev_state.pose().rotation().toQuaternion();
+      Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5));
+      Quaternion quat_error = result_quat * gps_quat.inverse();
+      quat_error.normalize();
+      Vector3 euler_angle_error(quat_error.x() * 2, quat_error.y() * 2,
+                                quat_error.z() * 2);
+      current_orientation_error = euler_angle_error.norm();
+
+      // display statistics
+      cout << "Position error:" << current_position_error << "\t "
+           << "Angular error:" << current_orientation_error << "\n"
+           << endl;
+
+      fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
+              output_time, result_position(0), result_position(1),
+              result_position(2), result_quat.x(), result_quat.y(),
+              result_quat.z(), result_quat.w(), gps(0), gps(1), gps(2),
+              gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w());
+
+      output_time += 1.0;
+
+    } else {
+      cerr << "ERROR parsing file\n";
+      return 1;
+    }
+  }
+  fclose(fp_out);
+  cout << "Complete, results written to " << output_filename << "\n\n";
+
+  return 0;
+}

examples/ImuFactorsExample.cpp

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file imuFactorsExample
+ * @file ImuFactorsExample
  * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor
  * navigation code.
  * @author Garrett (ghemann@gmail.com), Luca Carlone
@@ -31,32 +31,26 @@
  * Note that for GPS correction, we're only using the position not the
  * rotation. The rotation is provided in the file for ground truth comparison.
  *
- *  Usage: ./ImuFactorsExample [data_csv_path] [-c]
+ *  See usage: ./ImuFactorsExample --help
- *  optional arguments:
- *    data_csv_path           path to the CSV file with the IMU data.
- *    -c                      use CombinedImuFactor
- *  Note: Define USE_LM to use Levenberg Marquardt Optimizer
- *        By default ISAM2 is used
  */
 
+#include <boost/program_options.hpp>
+
 // GTSAM related includes.
+#include <gtsam/inference/Symbol.h>
 #include <gtsam/navigation/CombinedImuFactor.h>
 #include <gtsam/navigation/GPSFactor.h>
 #include <gtsam/navigation/ImuFactor.h>
-#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/nonlinear/ISAM2.h>
-#include <gtsam/slam/dataset.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/ISAM2.h>
+#include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/inference/Symbol.h>
+#include <gtsam/slam/dataset.h>
 
 #include <cstring>
 #include <fstream>
 #include <iostream>
 
-// Uncomment the following to use Levenberg Marquardt Optimizer
-// #define USE_LM
-
 using namespace gtsam;
 using namespace std;
 
@@ -64,45 +58,87 @@ using symbol_shorthand::B;  // Bias  (ax,ay,az,gx,gy,gz)
 using symbol_shorthand::V;  // Vel   (xdot,ydot,zdot)
 using symbol_shorthand::X;  // Pose3 (x,y,z,r,p,y)
 
-static const char output_filename[] = "imuFactorExampleResults.csv";
+namespace po = boost::program_options;
-static const char use_combined_imu_flag[3] = "-c";
+
+po::variables_map parseOptions(int argc, char* argv[]) {
+  po::options_description desc;
+  desc.add_options()("help,h", "produce help message")(
+      "data_csv_path", po::value<string>()->default_value("imuAndGPSdata.csv"),
+      "path to the CSV file with the IMU data")(
+      "output_filename",
+      po::value<string>()->default_value("imuFactorExampleResults.csv"),
+      "path to the result file to use")("use_isam", po::bool_switch(),
+                                        "use ISAM as the optimizer");
+
+  po::variables_map vm;
+  po::store(po::parse_command_line(argc, argv, desc), vm);
+
+  if (vm.count("help")) {
+    cout << desc << "\n";
+    exit(1);
+  }
+
+  return vm;
+}
+
+boost::shared_ptr<PreintegratedCombinedMeasurements::Params> imuParams() {
+  // We use the sensor specs to build the noise model for the IMU factor.
+  double accel_noise_sigma = 0.0003924;
+  double gyro_noise_sigma = 0.000205689024915;
+  double accel_bias_rw_sigma = 0.004905;
+  double gyro_bias_rw_sigma = 0.000001454441043;
+  Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2);
+  Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2);
+  Matrix33 integration_error_cov =
+      I_3x3 * 1e-8;  // error committed in integrating position from velocities
+  Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
+  Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
+  Matrix66 bias_acc_omega_int =
+      I_6x6 * 1e-5;  // error in the bias used for preintegration
+
+  auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
+  // PreintegrationBase params:
+  p->accelerometerCovariance =
+      measured_acc_cov;  // acc white noise in continuous
+  p->integrationCovariance =
+      integration_error_cov;  // integration uncertainty continuous
+  // should be using 2nd order integration
+  // PreintegratedRotation params:
+  p->gyroscopeCovariance =
+      measured_omega_cov;  // gyro white noise in continuous
+  // PreintegrationCombinedMeasurements params:
+  p->biasAccCovariance = bias_acc_cov;      // acc bias in continuous
+  p->biasOmegaCovariance = bias_omega_cov;  // gyro bias in continuous
+  p->biasAccOmegaInt = bias_acc_omega_int;
+
+  return p;
+}
 
 int main(int argc, char* argv[]) {
-  string data_filename;
+  string data_filename, output_filename;
-  bool use_combined_imu = false;
 
-#ifndef USE_LM
+  bool use_isam = false;
-  printf("Using ISAM2\n");
+
-  ISAM2Params parameters;
+  po::variables_map var_map = parseOptions(argc, argv);
-  parameters.relinearizeThreshold = 0.01;
+
-  parameters.relinearizeSkip = 1;
+  data_filename = findExampleDataFile(var_map["data_csv_path"].as<string>());
-  ISAM2 isam2(parameters);
+  output_filename = var_map["output_filename"].as<string>();
-#else
+  use_isam = var_map["use_isam"].as<bool>();
-  printf("Using Levenberg Marquardt Optimizer\n");
+
-#endif
+  ISAM2* isam2;
+  if (use_isam) {
+    printf("Using ISAM2\n");
+    ISAM2Params parameters;
+    parameters.relinearizeThreshold = 0.01;
+    parameters.relinearizeSkip = 1;
+    isam2 = new ISAM2(parameters);
 
-  if (argc < 2) {
-    printf("using default CSV file\n");
-    data_filename = findExampleDataFile("imuAndGPSdata.csv");
-  } else if (argc < 3) {
-    if (strcmp(argv[1], use_combined_imu_flag) == 0) {
-      printf("using CombinedImuFactor\n");
-      use_combined_imu = true;
-      printf("using default CSV file\n");
-      data_filename = findExampleDataFile("imuAndGPSdata.csv");
-    } else {
-      data_filename = argv[1];
-    }
   } else {
-    data_filename = argv[1];
+    printf("Using Levenberg Marquardt Optimizer\n");
-    if (strcmp(argv[2], use_combined_imu_flag) == 0) {
-      printf("using CombinedImuFactor\n");
-      use_combined_imu = true;
-    }
   }
 
   // Set up output file for plotting errors
-  FILE* fp_out = fopen(output_filename, "w+");
+  FILE* fp_out = fopen(output_filename.c_str(), "w+");
   fprintf(fp_out,
           "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,"
           "gt_qy,gt_qz,gt_qw\n");
@@ -118,10 +154,10 @@ int main(int argc, char* argv[]) {
   getline(file, value, ',');  // i
   for (int i = 0; i < 9; i++) {
     getline(file, value, ',');
-    initial_state(i) = atof(value.c_str());
+    initial_state(i) = stof(value.c_str());
   }
   getline(file, value, '\n');
-  initial_state(9) = atof(value.c_str());
+  initial_state(9) = stof(value.c_str());
   cout << "initial state:\n" << initial_state.transpose() << "\n\n";
 
   // Assemble initial quaternion through GTSAM constructor
@@ -152,43 +188,11 @@ int main(int argc, char* argv[]) {
   graph->addPrior(V(correction_count), prior_velocity, velocity_noise_model);
   graph->addPrior(B(correction_count), prior_imu_bias, bias_noise_model);
 
-  // We use the sensor specs to build the noise model for the IMU factor.
+  auto p = imuParams();
-  double accel_noise_sigma = 0.0003924;
-  double gyro_noise_sigma = 0.000205689024915;
-  double accel_bias_rw_sigma = 0.004905;
-  double gyro_bias_rw_sigma = 0.000001454441043;
-  Matrix33 measured_acc_cov = I_3x3 * pow(accel_noise_sigma, 2);
-  Matrix33 measured_omega_cov = I_3x3 * pow(gyro_noise_sigma, 2);
-  Matrix33 integration_error_cov =
-      I_3x3 * 1e-8;  // error committed in integrating position from velocities
-  Matrix33 bias_acc_cov = I_3x3 * pow(accel_bias_rw_sigma, 2);
-  Matrix33 bias_omega_cov = I_3x3 * pow(gyro_bias_rw_sigma, 2);
-  Matrix66 bias_acc_omega_int =
-      I_6x6 * 1e-5;  // error in the bias used for preintegration
 
-  auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
+  std::shared_ptr<PreintegrationType> preintegrated =
-  // PreintegrationBase params:
+      std::make_shared<PreintegratedImuMeasurements>(p, prior_imu_bias);
-  p->accelerometerCovariance =
-      measured_acc_cov;  // acc white noise in continuous
-  p->integrationCovariance =
-      integration_error_cov;  // integration uncertainty continuous
-  // should be using 2nd order integration
-  // PreintegratedRotation params:
-  p->gyroscopeCovariance =
-      measured_omega_cov;  // gyro white noise in continuous
-  // PreintegrationCombinedMeasurements params:
-  p->biasAccCovariance = bias_acc_cov;      // acc bias in continuous
-  p->biasOmegaCovariance = bias_omega_cov;  // gyro bias in continuous
-  p->biasAccOmegaInt = bias_acc_omega_int;
 
-  std::shared_ptr<PreintegrationType> preintegrated = nullptr;
-  if (use_combined_imu) {
-    preintegrated =
-        std::make_shared<PreintegratedCombinedMeasurements>(p, prior_imu_bias);
-  } else {
-    preintegrated =
-        std::make_shared<PreintegratedImuMeasurements>(p, prior_imu_bias);
-  }
   assert(preintegrated);
 
   // Store previous state for imu integration and latest predicted outcome.
@@ -207,16 +211,16 @@ int main(int argc, char* argv[]) {
   while (file.good()) {
     // Parse out first value
     getline(file, value, ',');
-    int type = atoi(value.c_str());
+    int type = stoi(value.c_str());
 
     if (type == 0) {  // IMU measurement
       Vector6 imu;
       for (int i = 0; i < 5; ++i) {
         getline(file, value, ',');
-        imu(i) = atof(value.c_str());
+        imu(i) = stof(value.c_str());
       }
       getline(file, value, '\n');
-      imu(5) = atof(value.c_str());
+      imu(5) = stof(value.c_str());
 
       // Adding the IMU preintegration.
       preintegrated->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);
@@ -225,35 +229,24 @@ int main(int argc, char* argv[]) {
       Vector7 gps;
       for (int i = 0; i < 6; ++i) {
         getline(file, value, ',');
-        gps(i) = atof(value.c_str());
+        gps(i) = stof(value.c_str());
       }
       getline(file, value, '\n');
-      gps(6) = atof(value.c_str());
+      gps(6) = stof(value.c_str());
 
       correction_count++;
 
       // Adding IMU factor and GPS factor and optimizing.
-      if (use_combined_imu) {
+      auto preint_imu =
-        auto preint_imu_combined =
+          dynamic_cast<const PreintegratedImuMeasurements&>(*preintegrated);
-            dynamic_cast<const PreintegratedCombinedMeasurements&>(
+      ImuFactor imu_factor(X(correction_count - 1), V(correction_count - 1),
-                *preintegrated);
+                           X(correction_count), V(correction_count),
-        CombinedImuFactor imu_factor(
+                           B(correction_count - 1), preint_imu);
-            X(correction_count - 1), V(correction_count - 1),
+      graph->add(imu_factor);
-            X(correction_count), V(correction_count), B(correction_count - 1),
+      imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
-            B(correction_count), preint_imu_combined);
+      graph->add(BetweenFactor<imuBias::ConstantBias>(
-        graph->add(imu_factor);
+          B(correction_count - 1), B(correction_count), zero_bias,
-      } else {
+          bias_noise_model));
-        auto preint_imu =
-            dynamic_cast<const PreintegratedImuMeasurements&>(*preintegrated);
-        ImuFactor imu_factor(X(correction_count - 1), V(correction_count - 1),
-                             X(correction_count), V(correction_count),
-                             B(correction_count - 1), preint_imu);
-        graph->add(imu_factor);
-        imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
-        graph->add(BetweenFactor<imuBias::ConstantBias>(
-            B(correction_count - 1), B(correction_count), zero_bias,
-            bias_noise_model));
-      }
 
       auto correction_noise = noiseModel::Isotropic::Sigma(3, 1.0);
       GPSFactor gps_factor(X(correction_count),
@@ -270,18 +263,21 @@ int main(int argc, char* argv[]) {
       initial_values.insert(B(correction_count), prev_bias);
 
       Values result;
-#ifdef USE_LM
-      LevenbergMarquardtOptimizer optimizer(*graph, initial_values);
-      result = optimizer.optimize();
-#else
-      isam2.update(*graph, initial_values);
-      isam2.update();
-      result = isam2.calculateEstimate();
 
-      // reset the graph
+      if (use_isam) {
-      graph->resize(0);
+        isam2->update(*graph, initial_values);
-      initial_values.clear();
+        isam2->update();
-#endif
+        result = isam2->calculateEstimate();
+
+        // reset the graph
+        graph->resize(0);
+        initial_values.clear();
+
+      } else {
+        LevenbergMarquardtOptimizer optimizer(*graph, initial_values);
+        result = optimizer.optimize();
+      }
+
       // Overwrite the beginning of the preintegration for the next step.
       prev_state = NavState(result.at<Pose3>(X(correction_count)),
                             result.at<Vector3>(V(correction_count)));

examples/ImuFactorsExample2.cpp

@@ -1,3 +1,19 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 ImuFactorExample2
+ * @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor with ISAM2.
+ * @author Robert Truax
+ */
 
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Cal3_S2.h>

gtsam/base/DSFMap.h

@@ -21,6 +21,7 @@
 #include <cstdlib>  // Provides size_t
 #include <map>
 #include <set>
+#include <vector>
 
 namespace gtsam {
 
@@ -120,4 +121,12 @@ class IndexPair : public std::pair<size_t,size_t> {
   inline size_t i() const { return first; };
   inline size_t j() const { return second; };
 };
+
+typedef std::vector<IndexPair> IndexPairVector;
+typedef std::set<IndexPair> IndexPairSet;
+
+inline IndexPairVector IndexPairSetAsArray(IndexPairSet& set) { return IndexPairVector(set.begin(), set.end()); }
+
+typedef std::map<IndexPair, IndexPairSet> IndexPairSetMap;
+typedef DSFMap<IndexPair> DSFMapIndexPair;
 }  // namespace gtsam

gtsam/geometry/Rot3.h

@@ -154,12 +154,23 @@ namespace gtsam {
     static Rot3 Rz(double t);
 
     /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
-    static Rot3 RzRyRx(double x, double y, double z);
+    static Rot3 RzRyRx(double x, double y, double z,
+                       OptionalJacobian<3, 1> Hx = boost::none,
+                       OptionalJacobian<3, 1> Hy = boost::none,
+                       OptionalJacobian<3, 1> Hz = boost::none);
 
     /// Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
-    inline static Rot3 RzRyRx(const Vector& xyz) {
+    inline static Rot3 RzRyRx(const Vector& xyz,
+                              OptionalJacobian<3, 3> H = boost::none) {
       assert(xyz.size() == 3);
-      return RzRyRx(xyz(0), xyz(1), xyz(2));
+      Rot3 out;
+      if (H) {
+        Vector3 Hx, Hy, Hz;
+        out = RzRyRx(xyz(0), xyz(1), xyz(2), Hx, Hy, Hz);
+        (*H) << Hx, Hy, Hz;
+      } else
+        out = RzRyRx(xyz(0), xyz(1), xyz(2));
+      return out;
     }
 
     /// Positive yaw is to right (as in aircraft heading). See ypr
@@ -185,7 +196,12 @@ namespace gtsam {
      * Positive pitch is down (decreasing aircraft altitude).
      * Positive roll is to right (decreasing yaw in aircraft).
      */
-    static Rot3 Ypr(double y, double p, double r) { return RzRyRx(r,p,y);}
+    static Rot3 Ypr(double y, double p, double r,
+                    OptionalJacobian<3, 1> Hy = boost::none,
+                    OptionalJacobian<3, 1> Hp = boost::none,
+                    OptionalJacobian<3, 1> Hr = boost::none) {
+      return RzRyRx(r, p, y, Hr, Hp, Hy);
+    }
 
     /** Create from Quaternion coefficients */
     static Rot3 Quaternion(double w, double x, double y, double z) {

gtsam/geometry/Rot3M.cpp

@@ -82,7 +82,8 @@ Rot3 Rot3::Rz(double t) {
 
 /* ************************************************************************* */
 // Considerably faster than composing matrices above !
-Rot3 Rot3::RzRyRx(double x, double y, double z) {
+Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx,
+                  OptionalJacobian<3, 1> Hy, OptionalJacobian<3, 1> Hz) {
   double cx=cos(x),sx=sin(x);
   double cy=cos(y),sy=sin(y);
   double cz=cos(z),sz=sin(z);
@@ -97,6 +98,9 @@ Rot3 Rot3::RzRyRx(double x, double y, double z) {
   double s_c = sx * cz;
   double c_c = cx * cz;
   double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz;
+  if (Hx) (*Hx) << 1, 0, 0;
+  if (Hy) (*Hy) << 0, cx, -sx;
+  if (Hz) (*Hz) << -sy, sc_, cc_;
   return Rot3(
       _cc,- c_s + ssc,  s_s + csc,
       _cs,  c_c + sss, -s_c + css,

gtsam/geometry/Rot3Q.cpp

@@ -67,10 +67,23 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3(
+  Rot3 Rot3::RzRyRx(double x, double y, double z, OptionalJacobian<3, 1> Hx,
-      gtsam::Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) *
+                    OptionalJacobian<3, 1> Hy, OptionalJacobian<3, 1> Hz) {
-      gtsam::Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) *
+    if (Hx) (*Hx) << 1, 0, 0;
-      gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX())));
+
+    if (Hy or Hz) {
+      const auto cx = cos(x), sx = sin(x);
+      if (Hy) (*Hy) << 0, cx, -sx;
+      if (Hz) {
+        const auto cy = cos(y), sy = sin(y);
+        (*Hz) << -sy, sx * cy, cx * cy;
+      }
+    }
+
+    return Rot3(
+        gtsam::Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) *
+        gtsam::Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) *
+        gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX())));
   }
 
   /* ************************************************************************* */

gtsam/geometry/tests/testRot3.cpp

@@ -743,6 +743,57 @@ TEST(Rot3, axisAngle) {
   CHECK_AXIS_ANGLE(_axis, theta165, Rot3::AxisAngle(axis, theta195))
 }
 
+/* ************************************************************************* */
+Rot3 RzRyRx_proxy(double const& a, double const& b, double const& c) {
+  return Rot3::RzRyRx(a, b, c);
+}
+
+TEST(Rot3, RzRyRx_scalars_derivative) {
+  const auto x = 0.1, y = 0.4, z = 0.2;
+  const auto num_x = numericalDerivative31(RzRyRx_proxy, x, y, z);
+  const auto num_y = numericalDerivative32(RzRyRx_proxy, x, y, z);
+  const auto num_z = numericalDerivative33(RzRyRx_proxy, x, y, z);
+
+  Vector3 act_x, act_y, act_z;
+  Rot3::RzRyRx(x, y, z, act_x, act_y, act_z);
+
+  CHECK(assert_equal(num_x, act_x));
+  CHECK(assert_equal(num_y, act_y));
+  CHECK(assert_equal(num_z, act_z));
+}
+
+/* ************************************************************************* */
+Rot3 RzRyRx_proxy(Vector3 const& xyz) { return Rot3::RzRyRx(xyz); }
+
+TEST(Rot3, RzRyRx_vector_derivative) {
+  const auto xyz = Vector3{-0.3, 0.1, 0.7};
+  const auto num = numericalDerivative11(RzRyRx_proxy, xyz);
+
+  Matrix3 act;
+  Rot3::RzRyRx(xyz, act);
+
+  CHECK(assert_equal(num, act));
+}
+
+/* ************************************************************************* */
+Rot3 Ypr_proxy(double const& y, double const& p, double const& r) {
+  return Rot3::Ypr(y, p, r);
+}
+
+TEST(Rot3, Ypr_derivative) {
+  const auto y = 0.7, p = -0.3, r = 0.1;
+  const auto num_y = numericalDerivative31(Ypr_proxy, y, p, r);
+  const auto num_p = numericalDerivative32(Ypr_proxy, y, p, r);
+  const auto num_r = numericalDerivative33(Ypr_proxy, y, p, r);
+
+  Vector3 act_y, act_p, act_r;
+  Rot3::Ypr(y, p, r, act_y, act_p, act_r);
+
+  CHECK(assert_equal(num_y, act_y));
+  CHECK(assert_equal(num_p, act_p));
+  CHECK(assert_equal(num_r, act_r));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/gtsam.i

@@ -259,11 +259,59 @@ class IndexPair {
   size_t j() const;
 };
 
-template<KEY = {gtsam::IndexPair}>
+// template<KEY = {gtsam::IndexPair}>
-class DSFMap {
+// class DSFMap {
-  DSFMap();
+//   DSFMap();
-  KEY find(const KEY& key) const;
+//   KEY find(const KEY& key) const;
-  void merge(const KEY& x, const KEY& y);
+//   void merge(const KEY& x, const KEY& y);
+//   std::map<KEY, Set> sets();
+// };
+
+class IndexPairSet {
+  IndexPairSet();
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  void insert(gtsam::IndexPair key);
+  bool erase(gtsam::IndexPair key); // returns true if value was removed
+  bool count(gtsam::IndexPair key) const; // returns true if value exists
+};
+
+class IndexPairVector {
+  IndexPairVector();
+  IndexPairVector(const gtsam::IndexPairVector& other);
+
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  gtsam::IndexPair at(size_t i) const;
+  void push_back(gtsam::IndexPair key) const;
+};
+
+gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set);
+
+class IndexPairSetMap {
+  IndexPairSetMap();
+  // common STL methods
+  size_t size() const;
+  bool empty() const;
+  void clear();
+
+  // structure specific methods
+  gtsam::IndexPairSet at(gtsam::IndexPair& key);
+};
+
+class DSFMapIndexPair {
+  DSFMapIndexPair();
+  gtsam::IndexPair find(const gtsam::IndexPair& key) const;
+  void merge(const gtsam::IndexPair& x, const gtsam::IndexPair& y);
+  gtsam::IndexPairSetMap sets();
 };
 
 #include <gtsam/base/Matrix.h>
@@ -2800,7 +2848,6 @@ class SfmData {
   gtsam::SfmTrack track(size_t idx) const;
 };
 
-string findExampleDataFile(string name);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
     gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise, bool smart);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
@@ -2925,6 +2972,7 @@ class ShonanAveragingParameters2 {
   void setOptimalityThreshold(double value);
   double getOptimalityThreshold() const;
   void setAnchor(size_t index, const gtsam::Rot2& value);
+  pair<size_t, gtsam::Rot2> getAnchor();
   void setAnchorWeight(double value);
   double getAnchorWeight() const;
   void setKarcherWeight(double value);
@@ -2940,6 +2988,7 @@ class ShonanAveragingParameters3 {
   void setOptimalityThreshold(double value);
   double getOptimalityThreshold() const;
   void setAnchor(size_t index, const gtsam::Rot3& value);
+  pair<size_t, gtsam::Rot3> getAnchor();
   void setAnchorWeight(double value);
   double getAnchorWeight() const;
   void setKarcherWeight(double value);

gtsam/navigation/NavState.cpp

@@ -89,8 +89,8 @@ Matrix7 NavState::matrix() const {
 //------------------------------------------------------------------------------
 ostream& operator<<(ostream& os, const NavState& state) {
   os << "R: " << state.attitude() << "\n";
-  os << "p: " << state.position() << "\n";
+  os << "p: " << state.position().transpose() << "\n";
-  os << "v: " << Point3(state.velocity());
+  os << "v: " << state.velocity().transpose();
   return os;
 }
 
@@ -218,28 +218,37 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
   const double dt2 = dt * dt;
   const Vector3 omega_cross_vel = omega.cross(n_v);
 
-  Vector9 xi;
+  // Get perturbations in nav frame
-  Matrix3 D_dP_R;
+  Vector9 n_xi, xi;
-  dR(xi) << nRb.unrotate((-dt) * omega, H ? &D_dP_R : 0);
+  Matrix3 D_dR_R, D_dP_R, D_dV_R, D_body_nav;
-  dP(xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper
+  dR(n_xi) << ((-dt) * omega);
-  dV(xi) << ((-2.0 * dt) * omega_cross_vel);
+  dP(n_xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper
+  dV(n_xi) << ((-2.0 * dt) * omega_cross_vel);
   if (secondOrder) {
     const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t));
-    dP(xi) -= (0.5 * dt2) * omega_cross2_t;
+    dP(n_xi) -= (0.5 * dt2) * omega_cross2_t;
-    dV(xi) -= dt * omega_cross2_t;
+    dV(n_xi) -= dt * omega_cross2_t;
   }
+
+  // Transform n_xi into the body frame
+  xi << nRb.unrotate(dR(n_xi), H ? &D_dR_R : 0, H ? &D_body_nav : 0), 
+        nRb.unrotate(dP(n_xi), H ? &D_dP_R : 0),
+        nRb.unrotate(dV(n_xi), H ? &D_dV_R : 0);
+
   if (H) {
     H->setZero();
     const Matrix3 Omega = skewSymmetric(omega);
     const Matrix3 D_cross_state = Omega * R();
     H->setZero();
-    D_R_R(H) << D_dP_R;
+    D_R_R(H) << D_dR_R;
-    D_t_v(H) << (-dt2) * D_cross_state;
+    D_t_v(H) << D_body_nav * (-dt2) * D_cross_state;
-    D_v_v(H) << (-2.0 * dt) * D_cross_state;
+    D_t_R(H) << D_dP_R;
+    D_v_v(H) << D_body_nav * (-2.0 * dt) * D_cross_state;
+    D_v_R(H) << D_dV_R;
     if (secondOrder) {
       const Matrix3 D_cross2_state = Omega * D_cross_state;
-      D_t_t(H) -= (0.5 * dt2) * D_cross2_state;
+      D_t_t(H) -= D_body_nav * (0.5 * dt2) * D_cross2_state;
-      D_v_t(H) -= dt * D_cross2_state;
+      D_v_t(H) -= D_body_nav * dt * D_cross2_state;
     }
   }
   return xi;

gtsam/navigation/tests/testNavState.cpp

@@ -192,6 +192,49 @@ TEST(NavState, Coriolis2) {
   EXPECT(assert_equal(numericalDerivative21(coriolis, state2, true), aH));
 }
 
+TEST(NavState, Coriolis3) {
+  /** Consider a massless planet with an attached nav frame at 
+   *  n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with 
+   *  velocity n_v = [0 1 0]'. Orient the body so that it is not instantaneously
+   *  aligned with the nav frame (i.e., nRb != I_3x3). Test that first and 
+   *  second order Coriolis corrections are as expected.
+   */
+
+  // Get true first and second order coriolis accelerations
+  double dt = 2.0, dt2 = dt * dt;
+  Vector3 n_omega(0.0, 0.0, 1.0), n_t(1.0, 0.0, 0.0), n_v(0.0, 1.0, 0.0);
+  Vector3 n_aCorr1 = -2.0 * n_omega.cross(n_v),
+          n_aCorr2 = -n_omega.cross(n_omega.cross(n_t));
+  Rot3 nRb = Rot3(-1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0),
+       bRn = nRb.inverse();
+
+  // Get expected first and second order corrections in the nav frame
+  Vector3 n_dP1e = 0.5 * dt2 * n_aCorr1, 
+          n_dP2e = 0.5 * dt2 * (n_aCorr1 + n_aCorr2),
+          n_dV1e = dt * n_aCorr1, 
+          n_dV2e = dt * (n_aCorr1 + n_aCorr2);
+
+  // Get expected first and second order corrections in the body frame
+  Vector3 dRe = -dt * (bRn * n_omega),
+          b_dP1e = bRn * n_dP1e, b_dP2e = bRn * n_dP2e,
+          b_dV1e = bRn * n_dV1e, b_dV2e = bRn * n_dV2e;
+
+  // Get actual first and scond order corrections in body frame
+  NavState kState2(nRb, n_t, n_v);
+  Vector9 dXi1a = kState2.coriolis(dt, n_omega, false),
+          dXi2a = kState2.coriolis(dt, n_omega, true);
+  Vector3 dRa = NavState::dR(dXi1a),
+          b_dP1a = NavState::dP(dXi1a), b_dV1a = NavState::dV(dXi1a),
+          b_dP2a = NavState::dP(dXi2a), b_dV2a = NavState::dV(dXi2a);
+
+  EXPECT(assert_equal(dRe, dRa));
+  EXPECT(assert_equal(b_dP1e, b_dP1a));
+  EXPECT(assert_equal(b_dV1e, b_dV1a));
+  EXPECT(assert_equal(b_dP2e, b_dP2a));
+  EXPECT(assert_equal(b_dV2e, b_dV2a));
+
+}
+
 /* ************************************************************************* */
 TEST(NavState, CorrectPIM) {
   Vector9 xi;
@@ -215,7 +258,7 @@ TEST(NavState, Stream)
   os << state;
 
   string expected;
-  expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\np: 0\n0\n0\nv: 0\n0\n0";
+  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);
 }

gtsam/nonlinear/NonlinearFactorGraph.cpp

@@ -205,42 +205,49 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
     // Create factors and variable connections
     for(size_t i = 0; i < size(); ++i) {
       const NonlinearFactor::shared_ptr& factor = at(i);
-      if(formatting.plotFactorPoints) {
+      // If null pointer, move on to the next
+      if (!factor) {
+        continue;
+      }
+
+      if (formatting.plotFactorPoints) {
         const KeyVector& keys = factor->keys();
-        if (formatting.binaryEdges && keys.size()==2) {
+        if (formatting.binaryEdges && keys.size() == 2) {
-          stm << "  var" << keys[0] << "--" << "var" << keys[1] << ";\n";
+          stm << "  var" << keys[0] << "--"
+              << "var" << keys[1] << ";\n";
         } else {
           // Make each factor a dot
           stm << "  factor" << i << "[label=\"\", shape=point";
           {
-            map<size_t, Point2>::const_iterator pos = formatting.factorPositions.find(i);
+            map<size_t, Point2>::const_iterator pos =
-            if(pos != formatting.factorPositions.end())
+                formatting.factorPositions.find(i);
-              stm << ", pos=\"" << formatting.scale*(pos->second.x() - minX) << ","
+            if (pos != formatting.factorPositions.end())
-                  << formatting.scale*(pos->second.y() - minY) << "!\"";
+              stm << ", pos=\"" << formatting.scale * (pos->second.x() - minX)
+                  << "," << formatting.scale * (pos->second.y() - minY)
+                  << "!\"";
           }
           stm << "];\n";
 
           // Make factor-variable connections
-          if(formatting.connectKeysToFactor && factor) {
+          if (formatting.connectKeysToFactor && factor) {
-            for(Key key: *factor) {
+            for (Key key : *factor) {
-              stm << "  var" << key << "--" << "factor" << i << ";\n";
+              stm << "  var" << key << "--"
+                  << "factor" << i << ";\n";
             }
           }
         }
-      }
+      } else {
-      else {
+        Key k;
-        if(factor) {
+        bool firstTime = true;
-          Key k;
+        for (Key key : *this->at(i)) {
-          bool firstTime = true;
+          if (firstTime) {
-          for(Key key: *this->at(i)) {
-            if(firstTime) {
-              k = key;
-              firstTime = false;
-              continue;
-            }
-            stm << "  var" << key << "--" << "var" << k << ";\n";
             k = key;
+            firstTime = false;
+            continue;
           }
+          stm << "  var" << key << "--"
+              << "var" << k << ";\n";
+          k = key;
         }
       }
     }

gtsam/sfm/ShonanAveraging.cpp

@@ -16,26 +16,25 @@
  * @brief  Shonan Averaging algorithm
  */
 
-#include <gtsam/sfm/ShonanAveraging.h>
+#include <SymEigsSolver.h>
-
 #include <gtsam/linear/PCGSolver.h>
 #include <gtsam/linear/SubgraphPreconditioner.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/sfm/ShonanAveraging.h>
+#include <gtsam/sfm/ShonanFactor.h>
 #include <gtsam/sfm/ShonanGaugeFactor.h>
 #include <gtsam/slam/FrobeniusFactor.h>
 #include <gtsam/slam/KarcherMeanFactor-inl.h>
-#include <gtsam/sfm/ShonanFactor.h>
 
 #include <Eigen/Eigenvalues>
-#include <SymEigsSolver.h>
-
 #include <algorithm>
 #include <complex>
 #include <iostream>
 #include <map>
 #include <random>
+#include <set>
 #include <vector>
 
 namespace gtsam {
@@ -50,8 +49,11 @@ template <size_t d>
 ShonanAveragingParameters<d>::ShonanAveragingParameters(
     const LevenbergMarquardtParams &_lm, const std::string &method,
     double optimalityThreshold, double alpha, double beta, double gamma)
-    : lm(_lm), optimalityThreshold(optimalityThreshold), alpha(alpha),
+    : lm(_lm),
-      beta(beta), gamma(gamma) {
+      optimalityThreshold(optimalityThreshold),
+      alpha(alpha),
+      beta(beta),
+      gamma(gamma) {
   // By default, we will do conjugate gradient
   lm.linearSolverType = LevenbergMarquardtParams::Iterative;
 
@@ -92,29 +94,40 @@ template struct ShonanAveragingParameters<3>;
 
 /* ************************************************************************* */
 // Calculate number of unknown rotations referenced by measurements
+// Throws exception of keys are not numbered as expected (may change in future).
 template <size_t d>
-static size_t
+static size_t NrUnknowns(
-NrUnknowns(const typename ShonanAveraging<d>::Measurements &measurements) {
+    const typename ShonanAveraging<d>::Measurements &measurements) {
+  Key maxKey = 0;
   std::set<Key> keys;
   for (const auto &measurement : measurements) {
-    keys.insert(measurement.key1());
+    for (const Key &key : measurement.keys()) {
-    keys.insert(measurement.key2());
+      maxKey = std::max(key, maxKey);
+      keys.insert(key);
+    }
   }
-  return keys.size();
+  size_t N = keys.size();
+  if (maxKey != N - 1) {
+    throw std::invalid_argument(
+        "As of now, ShonanAveraging expects keys numbered 0..N-1");
+  }
+  return N;
 }
 
 /* ************************************************************************* */
 template <size_t d>
 ShonanAveraging<d>::ShonanAveraging(const Measurements &measurements,
                                     const Parameters &parameters)
-    : parameters_(parameters), measurements_(measurements),
+    : parameters_(parameters),
+      measurements_(measurements),
       nrUnknowns_(NrUnknowns<d>(measurements)) {
   for (const auto &measurement : measurements_) {
     const auto &model = measurement.noiseModel();
     if (model && model->dim() != SO<d>::dimension) {
       measurement.print("Factor with incorrect noise model:\n");
-      throw std::invalid_argument("ShonanAveraging: measurements passed to "
+      throw std::invalid_argument(
-                                  "constructor have incorrect dimension.");
+          "ShonanAveraging: measurements passed to "
+          "constructor have incorrect dimension.");
     }
   }
   Q_ = buildQ();
@@ -196,7 +209,7 @@ Matrix ShonanAveraging<d>::StiefelElementMatrix(const Values &values) {
   Matrix S(p, N * d);
   for (const auto it : values.filter<SOn>()) {
     S.middleCols<d>(it.key * d) =
-        it.value.matrix().leftCols<d>(); // project Qj to Stiefel
+        it.value.matrix().leftCols<d>();  // project Qj to Stiefel
   }
   return S;
 }
@@ -227,7 +240,8 @@ Values ShonanAveraging<3>::projectFrom(size_t p, const Values &values) const {
 }
 
 /* ************************************************************************* */
-template <size_t d> static Matrix RoundSolutionS(const Matrix &S) {
+template <size_t d>
+static Matrix RoundSolutionS(const Matrix &S) {
   const size_t N = S.cols() / d;
   // First, compute a thin SVD of S
   Eigen::JacobiSVD<Matrix> svd(S, Eigen::ComputeThinV);
@@ -246,8 +260,7 @@ template <size_t d> static Matrix RoundSolutionS(const Matrix &S) {
   for (size_t i = 0; i < N; ++i) {
     // Compute the determinant of the ith dxd block of R
     double determinant = R.middleCols<d>(d * i).determinant();
-    if (determinant > 0)
+    if (determinant > 0) ++numPositiveBlocks;
-      ++numPositiveBlocks;
   }
 
   if (numPositiveBlocks < N / 2) {
@@ -263,7 +276,8 @@ template <size_t d> static Matrix RoundSolutionS(const Matrix &S) {
 }
 
 /* ************************************************************************* */
-template <> Values ShonanAveraging<2>::roundSolutionS(const Matrix &S) const {
+template <>
+Values ShonanAveraging<2>::roundSolutionS(const Matrix &S) const {
   // Round to a 2*2N matrix
   Matrix R = RoundSolutionS<2>(S);
 
@@ -276,7 +290,8 @@ template <> Values ShonanAveraging<2>::roundSolutionS(const Matrix &S) const {
   return values;
 }
 
-template <> Values ShonanAveraging<3>::roundSolutionS(const Matrix &S) const {
+template <>
+Values ShonanAveraging<3>::roundSolutionS(const Matrix &S) const {
   // Round to a 3*3N matrix
   Matrix R = RoundSolutionS<3>(S);
 
@@ -332,7 +347,8 @@ static double Kappa(const BinaryMeasurement<T> &measurement) {
 }
 
 /* ************************************************************************* */
-template <size_t d> Sparse ShonanAveraging<d>::buildD() const {
+template <size_t d>
+Sparse ShonanAveraging<d>::buildD() const {
   // Each measurement contributes 2*d elements along the diagonal of the
   // degree matrix.
   static constexpr size_t stride = 2 * d;
@@ -366,7 +382,8 @@ template <size_t d> Sparse ShonanAveraging<d>::buildD() const {
 }
 
 /* ************************************************************************* */
-template <size_t d> Sparse ShonanAveraging<d>::buildQ() const {
+template <size_t d>
+Sparse ShonanAveraging<d>::buildQ() const {
   // Each measurement contributes 2*d^2 elements on a pair of symmetric
   // off-diagonal blocks
   static constexpr size_t stride = 2 * d * d;
@@ -513,12 +530,12 @@ struct MatrixProdFunctor {
 //   - We've been using 10^-4 for the nonnegativity tolerance
 //   - for numLanczosVectors, 20 is a good default value
 
-static bool
+static bool SparseMinimumEigenValue(
-SparseMinimumEigenValue(const Sparse &A, const Matrix &S, double *minEigenValue,
+    const Sparse &A, const Matrix &S, double *minEigenValue,
-                        Vector *minEigenVector = 0, size_t *numIterations = 0,
+    Vector *minEigenVector = 0, size_t *numIterations = 0,
-                        size_t maxIterations = 1000,
+    size_t maxIterations = 1000,
-                        double minEigenvalueNonnegativityTolerance = 10e-4,
+    double minEigenvalueNonnegativityTolerance = 10e-4,
-                        Eigen::Index numLanczosVectors = 20) {
+    Eigen::Index numLanczosVectors = 20) {
   // a. Estimate the largest-magnitude eigenvalue of this matrix using Lanczos
   MatrixProdFunctor lmOperator(A);
   Spectra::SymEigsSolver<double, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN,
@@ -530,8 +547,7 @@ SparseMinimumEigenValue(const Sparse &A, const Matrix &S, double *minEigenValue,
       maxIterations, 1e-4, Spectra::SELECT_EIGENVALUE::LARGEST_MAGN);
 
   // Check convergence and bail out if necessary
-  if (lmConverged != 1)
+  if (lmConverged != 1) return false;
-    return false;
 
   const double lmEigenValue = lmEigenValueSolver.eigenvalues()(0);
 
@@ -541,7 +557,7 @@ SparseMinimumEigenValue(const Sparse &A, const Matrix &S, double *minEigenValue,
     *minEigenValue = lmEigenValue;
     if (minEigenVector) {
       *minEigenVector = lmEigenValueSolver.eigenvectors(1).col(0);
-      minEigenVector->normalize(); // Ensure that this is a unit vector
+      minEigenVector->normalize();  // Ensure that this is a unit vector
     }
     return true;
   }
@@ -578,7 +594,7 @@ SparseMinimumEigenValue(const Sparse &A, const Matrix &S, double *minEigenValue,
   Vector perturbation(v0.size());
   perturbation.setRandom();
   perturbation.normalize();
-  Vector xinit = v0 + (.03 * v0.norm()) * perturbation; // Perturb v0 by ~3%
+  Vector xinit = v0 + (.03 * v0.norm()) * perturbation;  // Perturb v0 by ~3%
 
   // Use this to initialize the eigensolver
   minEigenValueSolver.init(xinit.data());
@@ -590,21 +606,20 @@ SparseMinimumEigenValue(const Sparse &A, const Matrix &S, double *minEigenValue,
       maxIterations, minEigenvalueNonnegativityTolerance / lmEigenValue,
       Spectra::SELECT_EIGENVALUE::LARGEST_MAGN);
 
-  if (minConverged != 1)
+  if (minConverged != 1) return false;
-    return false;
 
   *minEigenValue = minEigenValueSolver.eigenvalues()(0) + 2 * lmEigenValue;
   if (minEigenVector) {
     *minEigenVector = minEigenValueSolver.eigenvectors(1).col(0);
-    minEigenVector->normalize(); // Ensure that this is a unit vector
+    minEigenVector->normalize();  // Ensure that this is a unit vector
   }
-  if (numIterations)
+  if (numIterations) *numIterations = minEigenValueSolver.num_iterations();
-    *numIterations = minEigenValueSolver.num_iterations();
   return true;
 }
 
 /* ************************************************************************* */
-template <size_t d> Sparse ShonanAveraging<d>::computeA(const Matrix &S) const {
+template <size_t d>
+Sparse ShonanAveraging<d>::computeA(const Matrix &S) const {
   auto Lambda = computeLambda(S);
   return Lambda - Q_;
 }
@@ -628,8 +643,8 @@ double ShonanAveraging<d>::computeMinEigenValue(const Values &values,
 
 /* ************************************************************************* */
 template <size_t d>
-std::pair<double, Vector>
+std::pair<double, Vector> ShonanAveraging<d>::computeMinEigenVector(
-ShonanAveraging<d>::computeMinEigenVector(const Values &values) const {
+    const Values &values) const {
   Vector minEigenVector;
   double minEigenValue = computeMinEigenValue(values, &minEigenVector);
   return std::make_pair(minEigenValue, minEigenVector);
@@ -643,20 +658,25 @@ bool ShonanAveraging<d>::checkOptimality(const Values &values) const {
 }
 
 /* ************************************************************************* */
-/// Create a tangent direction xi with eigenvector segment v_i
 template <size_t d>
-Vector ShonanAveraging<d>::MakeATangentVector(size_t p, const Vector &v,
+VectorValues ShonanAveraging<d>::TangentVectorValues(size_t p,
-                                              size_t i) {
+                                                     const Vector &v) {
+  VectorValues delta;
   // Create a tangent direction xi with eigenvector segment v_i
   const size_t dimension = SOn::Dimension(p);
-  const auto v_i = v.segment<d>(d * i);
+  double sign0 = pow(-1.0, round((p + 1) / 2) + 1);
-  Vector xi = Vector::Zero(dimension);
+  for (size_t i = 0; i < v.size() / d; i++) {
-  double sign = pow(-1.0, round((p + 1) / 2) + 1);
+    // Assumes key is 0-based integer
-  for (size_t j = 0; j < d; j++) {
+    const auto v_i = v.segment<d>(d * i);
-    xi(j + p - d - 1) = sign * v_i(d - j - 1);
+    Vector xi = Vector::Zero(dimension);
-    sign = -sign;
+    double sign = sign0;
+    for (size_t j = 0; j < d; j++) {
+      xi(j + p - d - 1) = sign * v_i(d - j - 1);
+      sign = -sign;
+    }
+    delta.insert(i, xi);
   }
-  return xi;
+  return delta;
 }
 
 /* ************************************************************************* */
@@ -690,14 +710,8 @@ template <size_t d>
 Values ShonanAveraging<d>::LiftwithDescent(size_t p, const Values &values,
                                            const Vector &minEigenVector) {
   Values lifted = LiftTo<SOn>(p, values);
-  for (auto it : lifted.filter<SOn>()) {
+  VectorValues delta = TangentVectorValues(p, minEigenVector);
-    // Create a tangent direction xi with eigenvector segment v_i
+  return lifted.retract(delta);
-    // Assumes key is 0-based integer
-    const Vector xi = MakeATangentVector(p, minEigenVector, it.key);
-    // Move the old value in the descent direction
-    it.value = it.value.retract(xi);
-  }
-  return lifted;
 }
 
 /* ************************************************************************* */
@@ -750,7 +764,8 @@ Values ShonanAveraging<d>::initializeRandomly(std::mt19937 &rng) const {
 }
 
 /* ************************************************************************* */
-template <size_t d> Values ShonanAveraging<d>::initializeRandomly() const {
+template <size_t d>
+Values ShonanAveraging<d>::initializeRandomly() const {
   return initializeRandomly(kRandomNumberGenerator);
 }
 
@@ -759,7 +774,7 @@ template <size_t d>
 Values ShonanAveraging<d>::initializeRandomlyAt(size_t p,
                                                 std::mt19937 &rng) const {
   const Values randomRotations = initializeRandomly(rng);
-  return LiftTo<Rot3>(p, randomRotations); // lift to p!
+  return LiftTo<Rot3>(p, randomRotations);  // lift to p!
 }
 
 /* ************************************************************************* */
@@ -823,8 +838,8 @@ ShonanAveraging3::ShonanAveraging3(string g2oFile, const Parameters &parameters)
 
 // Extract Rot3 measurement from Pose3 betweenfactors
 // Modeled after similar function in dataset.cpp
-static BinaryMeasurement<Rot3>
+static BinaryMeasurement<Rot3> convert(
-convert(const BetweenFactor<Pose3>::shared_ptr &f) {
+    const BetweenFactor<Pose3>::shared_ptr &f) {
   auto gaussian =
       boost::dynamic_pointer_cast<noiseModel::Gaussian>(f->noiseModel());
   if (!gaussian)
@@ -837,12 +852,11 @@ convert(const BetweenFactor<Pose3>::shared_ptr &f) {
       noiseModel::Gaussian::Covariance(M.block<3, 3>(3, 3), true));
 }
 
-static ShonanAveraging3::Measurements
+static ShonanAveraging3::Measurements extractRot3Measurements(
-extractRot3Measurements(const BetweenFactorPose3s &factors) {
+    const BetweenFactorPose3s &factors) {
   ShonanAveraging3::Measurements result;
   result.reserve(factors.size());
-  for (auto f : factors)
+  for (auto f : factors) result.push_back(convert(f));
-    result.push_back(convert(f));
   return result;
 }
 
@@ -851,4 +865,4 @@ ShonanAveraging3::ShonanAveraging3(const BetweenFactorPose3s &factors,
     : ShonanAveraging<3>(extractRot3Measurements(factors), parameters) {}
 
 /* ************************************************************************* */
-} // namespace gtsam
+}  // namespace gtsam

gtsam/sfm/ShonanAveraging.h

@@ -20,36 +20,39 @@
 
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Vector.h>
+#include <gtsam/dllexport.h>
 #include <gtsam/geometry/Rot2.h>
 #include <gtsam/geometry/Rot3.h>
+#include <gtsam/linear/VectorValues.h>
 #include <gtsam/nonlinear/LevenbergMarquardtParams.h>
 #include <gtsam/sfm/BinaryMeasurement.h>
 #include <gtsam/slam/dataset.h>
-#include <gtsam/dllexport.h>
 
 #include <Eigen/Sparse>
 #include <map>
 #include <string>
 #include <type_traits>
 #include <utility>
+#include <vector>
 
 namespace gtsam {
 class NonlinearFactorGraph;
 class LevenbergMarquardtOptimizer;
 
 /// Parameters governing optimization etc.
-template <size_t d> struct GTSAM_EXPORT ShonanAveragingParameters {
+template <size_t d>
+struct GTSAM_EXPORT ShonanAveragingParameters {
   // Select Rot2 or Rot3 interface based template parameter d
   using Rot = typename std::conditional<d == 2, Rot2, Rot3>::type;
   using Anchor = std::pair<size_t, Rot>;
 
   // Paremeters themselves:
-  LevenbergMarquardtParams lm; // LM parameters
+  LevenbergMarquardtParams lm;  // LM parameters
-  double optimalityThreshold;  // threshold used in checkOptimality
+  double optimalityThreshold;   // threshold used in checkOptimality
-  Anchor anchor;               // pose to use as anchor if not Karcher
+  Anchor anchor;                // pose to use as anchor if not Karcher
-  double alpha;                // weight of anchor-based prior (default 0)
+  double alpha;                 // weight of anchor-based prior (default 0)
-  double beta;                 // weight of Karcher-based prior (default 1)
+  double beta;                  // weight of Karcher-based prior (default 1)
-  double gamma;                // weight of gauge-fixing factors (default 0)
+  double gamma;                 // weight of gauge-fixing factors (default 0)
 
   ShonanAveragingParameters(const LevenbergMarquardtParams &lm =
                                 LevenbergMarquardtParams::CeresDefaults(),
@@ -64,6 +67,7 @@ template <size_t d> struct GTSAM_EXPORT ShonanAveragingParameters {
   double getOptimalityThreshold() const { return optimalityThreshold; }
 
   void setAnchor(size_t index, const Rot &value) { anchor = {index, value}; }
+  std::pair<size_t, Rot> getAnchor() { return anchor; }
 
   void setAnchorWeight(double value) { alpha = value; }
   double getAnchorWeight() { return alpha; }
@@ -93,8 +97,9 @@ using ShonanAveragingParameters3 = ShonanAveragingParameters<3>;
  *    European Computer Vision Conference, 2020.
  * You can view our ECCV spotlight video at https://youtu.be/5ppaqMyHtE0
  */
-template <size_t d> class GTSAM_EXPORT ShonanAveraging {
+template <size_t d>
-public:
+class GTSAM_EXPORT ShonanAveraging {
+ public:
   using Sparse = Eigen::SparseMatrix<double>;
 
   // Define the Parameters type and use its typedef of the rotation type:
@@ -105,13 +110,13 @@ public:
   // TODO(frank): use BinaryMeasurement?
   using Measurements = std::vector<BinaryMeasurement<Rot>>;
 
-private:
+ private:
   Parameters parameters_;
   Measurements measurements_;
   size_t nrUnknowns_;
-  Sparse D_; // Sparse (diagonal) degree matrix
+  Sparse D_;  // Sparse (diagonal) degree matrix
-  Sparse Q_; // Sparse measurement matrix, == \tilde{R} in Eriksson18cvpr
+  Sparse Q_;  // Sparse measurement matrix, == \tilde{R} in Eriksson18cvpr
-  Sparse L_; // connection Laplacian L = D - Q, needed for optimality check
+  Sparse L_;  // connection Laplacian L = D - Q, needed for optimality check
 
   /**
    * Build 3Nx3N sparse matrix consisting of rotation measurements, arranged as
@@ -122,7 +127,7 @@ private:
   /// Build 3Nx3N sparse degree matrix D
   Sparse buildD() const;
 
-public:
+ public:
   /// @name Standard Constructors
   /// @{
 
@@ -156,12 +161,12 @@ public:
   /// @name Matrix API (advanced use, debugging)
   /// @{
 
-  Sparse D() const { return D_; }              ///< Sparse version of D
+  Sparse D() const { return D_; }               ///< Sparse version of D
-  Matrix denseD() const { return Matrix(D_); } ///< Dense version of D
+  Matrix denseD() const { return Matrix(D_); }  ///< Dense version of D
-  Sparse Q() const { return Q_; }              ///< Sparse version of Q
+  Sparse Q() const { return Q_; }               ///< Sparse version of Q
-  Matrix denseQ() const { return Matrix(Q_); } ///< Dense version of Q
+  Matrix denseQ() const { return Matrix(Q_); }  ///< Dense version of Q
-  Sparse L() const { return L_; }              ///< Sparse version of L
+  Sparse L() const { return L_; }               ///< Sparse version of L
-  Matrix denseL() const { return Matrix(L_); } ///< Dense version of L
+  Matrix denseL() const { return Matrix(L_); }  ///< Dense version of L
 
   /// Version that takes pxdN Stiefel manifold elements
   Sparse computeLambda(const Matrix &S) const;
@@ -200,8 +205,8 @@ public:
   /// Project pxdN Stiefel manifold matrix S to Rot3^N
   Values roundSolutionS(const Matrix &S) const;
 
-  /// Create a tangent direction xi with eigenvector segment v_i
+  /// Create a VectorValues with eigenvector v_i
-  static Vector MakeATangentVector(size_t p, const Vector &v, size_t i);
+  static VectorValues TangentVectorValues(size_t p, const Vector &v);
 
   /// Calculate the riemannian gradient of F(values) at values
   Matrix riemannianGradient(size_t p, const Values &values) const;
@@ -220,11 +225,10 @@ public:
    * @param minEigenVector corresponding to minEigenValue at level p-1
    * @return values of type SO(p)
    */
-  Values
+  Values initializeWithDescent(
-  initializeWithDescent(size_t p, const Values &values,
+      size_t p, const Values &values, const Vector &minEigenVector,
-                        const Vector &minEigenVector, double minEigenValue,
+      double minEigenValue, double gradienTolerance = 1e-2,
-                        double gradienTolerance = 1e-2,
+      double preconditionedGradNormTolerance = 1e-4) const;
-                        double preconditionedGradNormTolerance = 1e-4) const;
   /// @}
   /// @name Advanced API
   /// @{
@@ -237,11 +241,11 @@ public:
 
   /**
    * Create initial Values of type SO(p)
-   * @param p the dimensionality of the rotation manifold 
+   * @param p the dimensionality of the rotation manifold
    */
   Values initializeRandomlyAt(size_t p, std::mt19937 &rng) const;
 
-  /// Version of initializeRandomlyAt with fixed random seed. 
+  /// Version of initializeRandomlyAt with fixed random seed.
   Values initializeRandomlyAt(size_t p) const;
 
   /**
@@ -300,7 +304,8 @@ public:
   Values roundSolution(const Values &values) const;
 
   /// Lift Values of type T to SO(p)
-  template <class T> static Values LiftTo(size_t p, const Values &values) {
+  template <class T>
+  static Values LiftTo(size_t p, const Values &values) {
     Values result;
     for (const auto it : values.filter<T>()) {
       result.insert(it.key, SOn::Lift(p, it.value.matrix()));
@@ -327,7 +332,7 @@ public:
    */
   Values initializeRandomly(std::mt19937 &rng) const;
 
-  /// Random initialization for wrapper, fixed random seed. 
+  /// Random initialization for wrapper, fixed random seed.
   Values initializeRandomly() const;
 
   /**
@@ -346,20 +351,22 @@ public:
 // convenience interface with file access.
 
 class ShonanAveraging2 : public ShonanAveraging<2> {
-public:
+ public:
   ShonanAveraging2(const Measurements &measurements,
                    const Parameters &parameters = Parameters());
-  ShonanAveraging2(string g2oFile, const Parameters &parameters = Parameters());
+  explicit ShonanAveraging2(string g2oFile,
+                            const Parameters &parameters = Parameters());
 };
 
 class ShonanAveraging3 : public ShonanAveraging<3> {
-public:
+ public:
   ShonanAveraging3(const Measurements &measurements,
                    const Parameters &parameters = Parameters());
-  ShonanAveraging3(string g2oFile, const Parameters &parameters = Parameters());
+  explicit ShonanAveraging3(string g2oFile,
+                            const Parameters &parameters = Parameters());
 
   // TODO(frank): Deprecate after we land pybind wrapper
   ShonanAveraging3(const BetweenFactorPose3s &factors,
                    const Parameters &parameters = Parameters());
 };
-} // namespace gtsam
+}  // namespace gtsam

gtsam/sfm/tests/testShonanAveraging.cpp

@@ -121,18 +121,17 @@ TEST(ShonanAveraging3, tryOptimizingAt4) {
 }
 
 /* ************************************************************************* */
-TEST(ShonanAveraging3, MakeATangentVector) {
+TEST(ShonanAveraging3, TangentVectorValues) {
   Vector9 v;
   v << 1, 2, 3, 4, 5, 6, 7, 8, 9;
-  Matrix expected(5, 5);
+  Vector expected0(10), expected1(10), expected2(10);
-  expected << 0, 0, 0, 0, -4, //
+  expected0 << 0, 3, -2, 1, 0, 0, 0, 0, 0, 0;
-      0, 0, 0, 0, -5,         //
+  expected1 << 0, 6, -5, 4, 0, 0, 0, 0, 0, 0;
-      0, 0, 0, 0, -6,         //
+  expected2 << 0, 9, -8, 7, 0, 0, 0, 0, 0, 0;
-      0, 0, 0, 0, 0,          //
+  const VectorValues xi = ShonanAveraging3::TangentVectorValues(5, v);
-      4, 5, 6, 0, 0;
+  EXPECT(assert_equal(expected0, xi[0]));
-  const Vector xi_1 = ShonanAveraging3::MakeATangentVector(5, v, 1);
+  EXPECT(assert_equal(expected1, xi[1]));
-  const auto actual = SOn::Hat(xi_1);
+  EXPECT(assert_equal(expected2, xi[2]));
-  CHECK(assert_equal(expected, actual));
 }
 
 /* ************************************************************************* */
@@ -168,7 +167,8 @@ TEST(ShonanAveraging3, CheckWithEigen) {
       minEigenValue = min(lambdas(i), minEigenValue);
 
   // Actual check
-  EXPECT_DOUBLES_EQUAL(minEigenValue, lambda, 1e-12);
+  EXPECT_DOUBLES_EQUAL(0, lambda, 1e-11);
+  EXPECT_DOUBLES_EQUAL(0, minEigenValue, 1e-11);
 
   // Construct test descent direction (as minEigenVector is not predictable
   // across platforms, being one from a basically flat 3d- subspace)

gtsam/slam/dataset.cpp

@@ -1129,6 +1129,13 @@ bool readBAL(const string &filename, SfmData &data) {
   return true;
 }
 
+/* ************************************************************************* */
+SfmData readBal(const string &filename) {
+  SfmData data;
+  readBAL(filename, data);
+  return data;
+}
+
 /* ************************************************************************* */
 bool writeBAL(const string &filename, SfmData &data) {
   // Open the output file

gtsam/slam/dataset.h

@@ -277,6 +277,14 @@ GTSAM_EXPORT bool readBundler(const std::string& filename, SfmData &data);
  */
 GTSAM_EXPORT bool readBAL(const std::string& filename, SfmData &data);
 
+/**
+ * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and returns the data
+ * as a SfmData structure. Mainly used by wrapped code.
+ * @param filename The name of the BAL file.
+ * @return SfM structure where the data is stored.
+ */
+GTSAM_EXPORT SfmData readBal(const std::string& filename);
+
 /**
  * @brief This function writes a "Bundle Adjustment in the Large" (BAL) file from a
  * SfmData structure

python/CMakeLists.txt

@@ -4,6 +4,10 @@ if (NOT GTSAM_BUILD_PYTHON)
     return()
 endif()
 
+# Common directory for storing data/datasets stored with the package.
+# This will store the data in the Python site package directly.
+set(GTSAM_PYTHON_DATASET_DIR "./gtsam/Data")
+
 # Generate setup.py.
 file(READ "${PROJECT_SOURCE_DIR}/README.md" README_CONTENTS)
 configure_file(${PROJECT_SOURCE_DIR}/python/setup.py.in
@@ -26,6 +30,8 @@ set(ignore
     gtsam::ISAM2ThresholdMapValue
     gtsam::FactorIndices
     gtsam::FactorIndexSet
+    gtsam::IndexPairSetMap
+    gtsam::IndexPairVector
     gtsam::BetweenFactorPose2s
     gtsam::BetweenFactorPose3s
     gtsam::Point2Vector

python/gtsam/__init__.py

@@ -1,4 +1,6 @@
+from . import utils
 from .gtsam import *
+from .utils import findExampleDataFile
 
 
 def _init():

python/gtsam/examples/ImuFactorISAM2Example.py

@@ -1,6 +1,6 @@
 """
-iSAM2 example with ImuFactor.
+ImuFactor example with iSAM2.
-Author: Robert Truax (C++), Frank Dellaert, Varun Agrawal
+Authors: Robert Truax (C++), Frank Dellaert, Varun Agrawal (Python)
 """
 # pylint: disable=invalid-name, E1101
 
@@ -8,9 +8,11 @@ from __future__ import print_function
 
 import math
 
-import gtsam
 import matplotlib.pyplot as plt
 import numpy as np
+from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
+
+import gtsam
 from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
                    ConstantTwistScenario, ImuFactor, NonlinearFactorGraph,
                    PinholeCameraCal3_S2, Point3, Pose3,
@@ -18,7 +20,6 @@ from gtsam import (ISAM2, BetweenFactorConstantBias, Cal3_S2,
                    PriorFactorVector, Rot3, Values)
 from gtsam.symbol_shorthand import B, V, X
 from gtsam.utils import plot
-from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=W0611
 
 
 def vector3(x, y, z):
@@ -58,7 +59,6 @@ def get_camera(radius):
 
 def get_scenario(radius, pose_0, angular_velocity, delta_t):
     """Create the set of ground-truth landmarks and poses"""
-
     angular_velocity_vector = vector3(0, -angular_velocity, 0)
     linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
     scenario = ConstantTwistScenario(

python/gtsam/examples/PreintegrationExample.py

@@ -111,7 +111,7 @@ class PreintegrationExample(object):
     def plotGroundTruthPose(self, t, scale=0.3, time_interval=0.01):
         # plot ground truth pose, as well as prediction from integrated IMU measurements
         actualPose = self.scenario.pose(t)
-        plot_pose3(POSES_FIG, actualPose, 0.3)
+        plot_pose3(POSES_FIG, actualPose, scale)
         t = actualPose.translation()
         self.maxDim = max([max(np.abs(t)), self.maxDim])
         ax = plt.gca()

python/gtsam/preamble.h

@@ -9,3 +9,4 @@ PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::
 PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
 PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > >);
 PYBIND11_MAKE_OPAQUE(std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > >);
+PYBIND11_MAKE_OPAQUE(std::vector<gtsam::IndexPair>);

python/gtsam/specializations.h

@@ -10,3 +10,5 @@ py::bind_vector<std::vector<gtsam::Pose3> >(m_, "Pose3Vector");
 py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(m_, "BetweenFactorPose3s");
 py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > > >(m_, "BetweenFactorPose2s");
 py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(m_, "BinaryMeasurementsUnit3");
+py::bind_map<gtsam::IndexPairSetMap>(m_, "IndexPairSetMap");
+py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");

python/gtsam/tests/test_dsf_map.py

@@ -35,6 +35,20 @@ class TestDSFMap(GtsamTestCase):
         dsf.merge(pair1, pair2)
         self.assertEqual(key(dsf.find(pair1)), key(dsf.find(pair2)))
 
+    def test_sets(self):
+        from gtsam import IndexPair
+        dsf = gtsam.DSFMapIndexPair()
+        dsf.merge(IndexPair(0, 1), IndexPair(1,2))
+        dsf.merge(IndexPair(0, 1), IndexPair(3,4))
+        dsf.merge(IndexPair(4,5), IndexPair(6,8))
+        sets = dsf.sets()
+
+        for i in sets:
+            s = sets[i]
+            for val in gtsam.IndexPairSetAsArray(s):
+                val.i()
+                val.j()
+
 
 if __name__ == '__main__':
     unittest.main()

python/gtsam/utils/__init__.py

@@ -0,0 +1,22 @@
+import glob
+import os.path as osp
+
+
+def findExampleDataFile(name):
+    """
+    Find the example data file specified by `name`.
+    """
+    # This is okay since gtsam will already be loaded
+    # before this function is accessed. Thus no effect.
+    import gtsam
+
+    site_package_path = gtsam.__path__[0]
+    # add the * at the end to glob the entire directory
+    data_path = osp.join(site_package_path, "Data", "*")
+
+    extensions = ("", ".graph", ".txt", ".out", ".xml", ".g2o")
+
+    for data_file_path in glob.glob(data_path, recursive=True):
+        for ext in extensions:
+            if (name + ext) == osp.basename(data_file_path):
+                return data_file_path

python/setup.py.in

@@ -1,3 +1,4 @@
+import glob
 import os
 import sys
 
@@ -8,6 +9,11 @@ except ImportError:
 
 packages = find_packages(where=".")
 print("PACKAGES: ", packages)
+
+data_path = '${GTSAM_SOURCE_DIR}/examples/Data/'
+data_files_and_directories = glob.glob(data_path + '**', recursive=True)
+data_files = [x for x in data_files_and_directories if not os.path.isdir(x)]
+
 package_data = {
     '': [
         './*.so',
@@ -27,6 +33,7 @@ setup(
     author_email='frank.dellaert@gtsam.org',
     license='Simplified BSD license',
     keywords='slam sam robotics localization mapping optimization',
+    long_description_content_type='text/markdown',
     long_description=readme_contents,
     # https://pypi.org/pypi?%3Aaction=list_classifiers
     classifiers=[
@@ -43,6 +50,7 @@ setup(
     ],
     packages=packages,
     package_data=package_data,
+    data_files=[('${GTSAM_PYTHON_DATASET_DIR}', data_files),],
     test_suite="gtsam.tests",
     install_requires=["numpy"],
     zip_safe=False,