Merge pull request #483 from borglab/feature/ShonanCLI

Shonan averaging cli in C++ and python

examples/ShonanAveragingCLI.cpp

@@ -0,0 +1,125 @@
+/* ----------------------------------------------------------------------------
+
+* 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    ShonanAveragingCLI.cpp
+ * @brief   Run Shonan Rotation Averaging Algorithm on a file or example dataset
+ * @author  Frank Dellaert
+ * @date    August, 2020
+ *
+ * Example usage:
+ *
+ * Running without arguments will run on tiny 3D example pose3example-grid
+ * ./ShonanAveragingCLI
+ *
+ * Read 2D dataset w10000 (in examples/data) and output to w10000-rotations.g2o
+ * ./ShonanAveragingCLI -d 2 -n w10000 -o w10000-rotations.g2o
+ *
+ * Read 3D dataset sphere25000.txt and output to shonan.g2o (default)
+ * ./ShonanAveragingCLI -i spere2500.txt
+ *
+ */
+
+#include <gtsam/base/timing.h>
+#include <gtsam/sfm/ShonanAveraging.h>
+#include <gtsam/slam/InitializePose.h>
+#include <gtsam/slam/dataset.h>
+
+#include <boost/program_options.hpp>
+
+using namespace std;
+using namespace gtsam;
+namespace po = boost::program_options;
+
+/* ************************************************************************* */
+int main(int argc, char* argv[]) {
+  string datasetName;
+  string inputFile;
+  string outputFile;
+  int d, seed;
+  po::options_description desc(
+      "Shonan Rotation Averaging CLI reads a *pose* graph, extracts the "
+      "rotation constraints, and runs the Shonan algorithm.");
+  desc.add_options()("help", "Print help message")(
+      "named_dataset,n",
+      po::value<string>(&datasetName)->default_value("pose3example-grid"),
+      "Find and read frome example dataset file")(
+      "input_file,i", po::value<string>(&inputFile)->default_value(""),
+      "Read pose constraints graph from the specified file")(
+      "output_file,o",
+      po::value<string>(&outputFile)->default_value("shonan.g2o"),
+      "Write solution to the specified file")(
+      "dimension,d", po::value<int>(&d)->default_value(3),
+      "Optimize over 2D or 3D rotations")(
+      "seed,s", po::value<int>(&seed)->default_value(42),
+      "Random seed for initial estimate");
+  po::variables_map vm;
+  po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
+  po::notify(vm);
+
+  if (vm.count("help")) {
+    cout << desc << "\n";
+    return 1;
+  }
+
+  // Get input file
+  if (inputFile.empty()) {
+    if (datasetName.empty()) {
+      cout << "You must either specify a named dataset or an input file\n"
+           << desc << endl;
+      return 1;
+    }
+    inputFile = findExampleDataFile(datasetName);
+  }
+
+  // Seed random number generator
+  static std::mt19937 rng(seed);
+
+  NonlinearFactorGraph::shared_ptr inputGraph;
+  Values::shared_ptr posesInFile;
+  Values poses;
+  if (d == 2) {
+    cout << "Running Shonan averaging for SO(2) on " << inputFile << endl;
+    ShonanAveraging2 shonan(inputFile);
+    auto initial = shonan.initializeRandomly(rng);
+    auto result = shonan.run(initial);
+
+    // Parse file again to set up translation problem, adding a prior
+    boost::tie(inputGraph, posesInFile) = load2D(inputFile);
+    auto priorModel = noiseModel::Unit::Create(3);
+    inputGraph->addPrior(0, posesInFile->at<Pose2>(0), priorModel);
+
+    cout << "recovering 2D translations" << endl;
+    auto poseGraph = initialize::buildPoseGraph<Pose2>(*inputGraph);
+    poses = initialize::computePoses<Pose2>(result.first, &poseGraph);
+  } else if (d == 3) {
+    cout << "Running Shonan averaging for SO(3) on " << inputFile << endl;
+    ShonanAveraging3 shonan(inputFile);
+    auto initial = shonan.initializeRandomly(rng);
+    auto result = shonan.run(initial);
+
+    // Parse file again to set up translation problem, adding a prior
+    boost::tie(inputGraph, posesInFile) = load3D(inputFile);
+    auto priorModel = noiseModel::Unit::Create(6);
+    inputGraph->addPrior(0, posesInFile->at<Pose3>(0), priorModel);
+
+    cout << "recovering 3D translations" << endl;
+    auto poseGraph = initialize::buildPoseGraph<Pose3>(*inputGraph);
+    poses = initialize::computePoses<Pose3>(result.first, &poseGraph);
+  } else {
+    cout << "Can only run SO(2) or SO(3) averaging\n" << desc << endl;
+    return 1;
+  }
+  cout << "Writing result to " << outputFile << endl;
+  writeG2o(NonlinearFactorGraph(), poses, outputFile);
+  return 0;
+}
+
+/* ************************************************************************* */

gtsam/geometry/Pose3.cpp

@@ -430,7 +430,7 @@ boost::optional<Pose3> align(const vector<Point3Pair>& baPointPairs) {
 std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
   // Both Rot3 and Point3 have ostream definitions so we use them.
   os << "R: " << pose.rotation() << "\n";
-  os << "t: " << pose.translation();
+  os << "t: " << pose.translation().transpose();
   return os;
 }
 

gtsam/geometry/tests/testPose3.cpp

@@ -857,15 +857,11 @@ TEST( Pose3, adjointTranspose) {
 }
 
 /* ************************************************************************* */
-TEST( Pose3, stream)
+TEST( Pose3, stream) {
-{
-  Pose3 T;
   std::ostringstream os;
-  os << T;
+  os << Pose3();
-
-  string expected;
-  expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 0\n0\n0";;
 
+  string expected = "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\nt: 0 0 0";
   EXPECT(os.str() == expected);
 }
 
@@ -1038,10 +1034,7 @@ TEST(Pose3, print) {
 
   // Add expected rotation
   expected << "R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t0, 0, 1\n]\n";
-
+  expected << "t: 1 2 3\n";
-  expected << "t: 1\n"
-              "2\n"
-              "3\n";
 
   // reset cout to the original stream
   std::cout.rdbuf(oldbuf);

gtsam/gtsam.i

@@ -2816,7 +2816,19 @@ void save2D(const gtsam::NonlinearFactorGraph& graph,
     const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
     string filename);
 
+// std::vector<gtsam::BetweenFactor<Pose2>::shared_ptr>
+// Ignored by pybind -> will be List[BetweenFactorPose2]
+class BetweenFactorPose2s
+{
+  BetweenFactorPose2s();
+  size_t size() const;
+  gtsam::BetweenFactor<gtsam::Pose2>* at(size_t i) const;
+  void push_back(const gtsam::BetweenFactor<gtsam::Pose2>* factor);
+};
+gtsam::BetweenFactorPose2s parse2DFactors(string filename);
+
 // std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
+// Ignored by pybind -> will be List[BetweenFactorPose3]
 class BetweenFactorPose3s
 {
   BetweenFactorPose3s();
@@ -2824,6 +2836,14 @@ class BetweenFactorPose3s
   gtsam::BetweenFactor<gtsam::Pose3>* at(size_t i) const;
   void push_back(const gtsam::BetweenFactor<gtsam::Pose3>* factor);
 };
+gtsam::BetweenFactorPose3s parse3DFactors(string filename);
+
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
+
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename, bool is3D);
+void writeG2o(const gtsam::NonlinearFactorGraph& graph,
+    const gtsam::Values& estimate, string filename);
 
 #include <gtsam/slam/InitializePose3.h>
 class InitializePose3 {
@@ -2845,14 +2865,6 @@ class InitializePose3 {
   static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph);
 };
 
-gtsam::BetweenFactorPose3s parse3DFactors(string filename);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
-
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
-pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename, bool is3D);
-void writeG2o(const gtsam::NonlinearFactorGraph& graph,
-    const gtsam::Values& estimate, string filename);
-
 #include <gtsam/slam/KarcherMeanFactor-inl.h>
 template<T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose3}>
 virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
@@ -3376,6 +3388,7 @@ namespace utilities {
   gtsam::KeySet createKeySet(string s, Vector I);
   Matrix extractPoint2(const gtsam::Values& values);
   Matrix extractPoint3(const gtsam::Values& values);
+  gtsam::Values allPose2s(gtsam::Values& values);
   Matrix extractPose2(const gtsam::Values& values);
   gtsam::Values allPose3s(gtsam::Values& values);
   Matrix extractPose3(const gtsam::Values& values);

gtsam/nonlinear/utilities.h

@@ -107,6 +107,11 @@ Matrix extractPoint3(const Values& values) {
   return result;
 }
 
+/// Extract all Pose3 values
+Values allPose2s(const Values& values) {
+  return values.filter<Pose2>();
+}
+
 /// Extract all Pose2 values into a single matrix [x y theta]
 Matrix extractPose2(const Values& values) {
   Values::ConstFiltered<Pose2> poses = values.filter<Pose2>();

gtsam/sfm/tests/testShonanAveraging.cpp

@@ -302,7 +302,7 @@ TEST(ShonanAveraging3, runKlausKarcher) {
 }
 
 /* ************************************************************************* */
-TEST(ShonanAveraging2, runKlausKarcher) {
+TEST(ShonanAveraging2, noisyToyGraph) {
   // Load 2D toy example
   auto lmParams = LevenbergMarquardtParams::CeresDefaults();
   // lmParams.setVerbosityLM("SUMMARY");

gtsam/slam/InitializePose.h

@@ -0,0 +1,99 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  InitializePose.h
+ *  @author Frank Dellaert
+ *  @date   August, 2020
+ *  @brief common code between lago.* (2D) and InitializePose3.* (3D)
+ */
+
+#pragma once
+
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+
+namespace gtsam {
+namespace initialize {
+
+static constexpr Key kAnchorKey = 99999999;
+
+/**
+ * Select the subgraph of betweenFactors and transforms priors into between
+ * wrt a fictitious node
+ */
+template <class Pose>
+static NonlinearFactorGraph buildPoseGraph(const NonlinearFactorGraph& graph) {
+  NonlinearFactorGraph poseGraph;
+
+  for (const auto& factor : graph) {
+    // recast to a between on Pose
+    if (auto between =
+            boost::dynamic_pointer_cast<BetweenFactor<Pose> >(factor))
+      poseGraph.add(between);
+
+    // recast PriorFactor<Pose> to BetweenFactor<Pose>
+    if (auto prior = boost::dynamic_pointer_cast<PriorFactor<Pose> >(factor))
+      poseGraph.emplace_shared<BetweenFactor<Pose> >(
+          kAnchorKey, prior->keys()[0], prior->prior(), prior->noiseModel());
+  }
+  return poseGraph;
+}
+
+/**
+ * Use Gauss-Newton optimizer to optimize for poses given rotation estimates
+ */
+template <class Pose>
+static Values computePoses(const Values& initialRot,
+                           NonlinearFactorGraph* posegraph,
+                           bool singleIter = true) {
+  const auto origin = Pose().translation();
+
+  // Upgrade rotations to full poses
+  Values initialPose;
+  for (const auto& key_value : initialRot) {
+    Key key = key_value.key;
+    const auto& rot = initialRot.at<typename Pose::Rotation>(key);
+    Pose initializedPose = Pose(rot, origin);
+    initialPose.insert(key, initializedPose);
+  }
+
+  // add prior on dummy node
+  auto priorModel = noiseModel::Unit::Create(Pose::dimension);
+  initialPose.insert(kAnchorKey, Pose());
+  posegraph->emplace_shared<PriorFactor<Pose> >(kAnchorKey, Pose(), priorModel);
+
+  // Create optimizer
+  GaussNewtonParams params;
+  if (singleIter) {
+    params.maxIterations = 1;
+  } else {
+    params.setVerbosity("TERMINATION");
+  }
+  GaussNewtonOptimizer optimizer(*posegraph, initialPose, params);
+  Values GNresult = optimizer.optimize();
+
+  // put into Values structure
+  Values estimate;
+  for (const auto& key_value : GNresult) {
+    Key key = key_value.key;
+    if (key != kAnchorKey) {
+      const Pose& pose = GNresult.at<Pose>(key);
+      estimate.insert(key, pose);
+    }
+  }
+  return estimate;
+}
+}  // namespace initialize
+}  // namespace gtsam

gtsam/slam/InitializePose3.cpp

@@ -10,28 +10,31 @@
  * -------------------------------------------------------------------------- */
 
 /**
- *  @file  InitializePose3.h
+ *  @file  InitializePose3.cpp
  *  @author Luca Carlone
  *  @author Frank Dellaert
  *  @date   August, 2014
  */
 
 #include <gtsam/slam/InitializePose3.h> 
+
+#include <gtsam/slam/InitializePose.h> 
 #include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/timing.h>
 
 #include <boost/math/special_functions.hpp>
 
+#include <utility>
+
 using namespace std;
 
 namespace gtsam {
 
-static const Key kAnchorKey = symbol('Z', 9999999);
-
 /* ************************************************************************* */
 GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
 
@@ -62,7 +65,7 @@ GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const Nonlinear
   }
   // prior on the anchor orientation
   linearGraph.add(
-      kAnchorKey, I_9x9,
+      initialize::kAnchorKey, I_9x9,
       (Vector(9) << 1.0, 0.0, 0.0, /*  */ 0.0, 1.0, 0.0, /*  */ 0.0, 0.0, 1.0)
           .finished(),
           noiseModel::Isotropic::Precision(9, 1));
@@ -78,7 +81,7 @@ Values InitializePose3::normalizeRelaxedRotations(
   Values validRot3;
   for(const auto& it: relaxedRot3) {
     Key key = it.first;
-    if (key != kAnchorKey) {
+    if (key != initialize::kAnchorKey) {
       Matrix3 M;
       M << Eigen::Map<const Matrix3>(it.second.data()); // Recover M from vectorized
 
@@ -91,24 +94,10 @@ Values InitializePose3::normalizeRelaxedRotations(
 }
 
 /* ************************************************************************* */
-NonlinearFactorGraph InitializePose3::buildPose3graph(const NonlinearFactorGraph& graph) {
+NonlinearFactorGraph InitializePose3::buildPose3graph(
+    const NonlinearFactorGraph& graph) {
   gttic(InitializePose3_buildPose3graph);
-  NonlinearFactorGraph pose3Graph;
+  return initialize::buildPoseGraph<Pose3>(graph);
-
-  for(const auto& factor: graph) {
-
-    // recast to a between on Pose3
-    const auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
-    if (pose3Between)
-      pose3Graph.add(pose3Between);
-
-    // recast PriorFactor<Pose3> to BetweenFactor<Pose3>
-    const auto pose3Prior = boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor);
-    if (pose3Prior)
-      pose3Graph.emplace_shared<BetweenFactor<Pose3> >(kAnchorKey, pose3Prior->keys()[0],
-              pose3Prior->prior(), pose3Prior->noiseModel());
-  }
-  return pose3Graph;
 }
 
 /* ************************************************************************* */
@@ -134,7 +123,7 @@ Values InitializePose3::computeOrientationsGradient(
 
   // this works on the inverse rotations, according to Tron&Vidal,2011
   Values inverseRot;
-  inverseRot.insert(kAnchorKey, Rot3());
+  inverseRot.insert(initialize::kAnchorKey, Rot3());
   for(const auto& key_value: givenGuess) {
     Key key = key_value.key;
     const Pose3& pose = givenGuess.at<Pose3>(key);
@@ -145,7 +134,7 @@ Values InitializePose3::computeOrientationsGradient(
   KeyVectorMap adjEdgesMap;
   KeyRotMap factorId2RotMap;
 
-  createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);
+  createSymbolicGraph(pose3Graph, &adjEdgesMap, &factorId2RotMap);
 
   // calculate max node degree & allocate gradient
   size_t maxNodeDeg = 0;
@@ -212,11 +201,11 @@ Values InitializePose3::computeOrientationsGradient(
   } // enf of gradient iterations
 
   // Return correct rotations
-  const Rot3& Rref = inverseRot.at<Rot3>(kAnchorKey); // This will be set to the identity as so far we included no prior
+  const Rot3& Rref = inverseRot.at<Rot3>(initialize::kAnchorKey); // This will be set to the identity as so far we included no prior
   Values estimateRot;
   for(const auto& key_value: inverseRot) {
     Key key = key_value.key;
-    if (key != kAnchorKey) {
+    if (key != initialize::kAnchorKey) {
       const Rot3& R = inverseRot.at<Rot3>(key);
       if(setRefFrame)
         estimateRot.insert(key, Rref.compose(R.inverse()));
@@ -228,30 +217,32 @@ Values InitializePose3::computeOrientationsGradient(
 }
 
 /* ************************************************************************* */
-void InitializePose3::createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
+void InitializePose3::createSymbolicGraph(
-                         const NonlinearFactorGraph& pose3Graph) {
+    const NonlinearFactorGraph& pose3Graph, KeyVectorMap* adjEdgesMap,
+    KeyRotMap* factorId2RotMap) {
   size_t factorId = 0;
   for (const auto& factor : pose3Graph) {
-    auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
+    auto pose3Between =
+        boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
     if (pose3Between) {
       Rot3 Rij = pose3Between->measured().rotation();
-      factorId2RotMap.insert(pair<Key, Rot3 >(factorId,Rij));
+      factorId2RotMap->emplace(factorId, Rij);
 
       Key key1 = pose3Between->key1();
-      if (adjEdgesMap.find(key1) != adjEdgesMap.end()){  // key is already in
+      if (adjEdgesMap->find(key1) != adjEdgesMap->end()) {  // key is already in
-        adjEdgesMap.at(key1).push_back(factorId);
+        adjEdgesMap->at(key1).push_back(factorId);
       } else {
         vector<size_t> edge_id;
         edge_id.push_back(factorId);
-        adjEdgesMap.insert(pair<Key, vector<size_t> >(key1, edge_id));
+        adjEdgesMap->emplace(key1, edge_id);
       }
       Key key2 = pose3Between->key2();
-      if (adjEdgesMap.find(key2) != adjEdgesMap.end()){  // key is already in
+      if (adjEdgesMap->find(key2) != adjEdgesMap->end()) {  // key is already in
-        adjEdgesMap.at(key2).push_back(factorId);
+        adjEdgesMap->at(key2).push_back(factorId);
       } else {
         vector<size_t> edge_id;
         edge_id.push_back(factorId);
-        adjEdgesMap.insert(pair<Key, vector<size_t> >(key2, edge_id));
+        adjEdgesMap->emplace(key2, edge_id);
       }
     } else {
       cout << "Error in createSymbolicGraph" << endl;
@@ -293,50 +284,16 @@ Values InitializePose3::initializeOrientations(const NonlinearFactorGraph& graph
 }
 
 ///* ************************************************************************* */
-Values InitializePose3::computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
+Values InitializePose3::computePoses(const Values& initialRot,
+                                      NonlinearFactorGraph* posegraph,
+                                      bool singleIter) {
   gttic(InitializePose3_computePoses);
-
+  return initialize::computePoses<Pose3>(initialRot, posegraph, singleIter);
-  // put into Values structure
-  Values initialPose;
-  for (const auto& key_value : initialRot) {
-    Key key = key_value.key;
-    const Rot3& rot = initialRot.at<Rot3>(key);
-    Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0));
-    initialPose.insert(key, initializedPose);
-  }
-
-  // add prior
-  noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
-  initialPose.insert(kAnchorKey, Pose3());
-  pose3graph.emplace_shared<PriorFactor<Pose3> >(kAnchorKey, Pose3(), priorModel);
-
-  // Create optimizer
-  GaussNewtonParams params;
-  bool singleIter = true;
-  if (singleIter) {
-    params.maxIterations = 1;
-  } else {
-    cout << " \n\n\n\n  performing more than 1 GN iterations \n\n\n" << endl;
-    params.setVerbosity("TERMINATION");
-  }
-  GaussNewtonOptimizer optimizer(pose3graph, initialPose, params);
-  Values GNresult = optimizer.optimize();
-
-  // put into Values structure
-  Values estimate;
-  for (const auto& key_value : GNresult) {
-    Key key = key_value.key;
-    if (key != kAnchorKey) {
-      const Pose3& pose = GNresult.at<Pose3>(key);
-      estimate.insert(key, pose);
-    }
-  }
-  return estimate;
 }
 
 /* ************************************************************************* */
-Values InitializePose3::initialize(const NonlinearFactorGraph& graph, const Values& givenGuess,
+Values InitializePose3::initialize(const NonlinearFactorGraph& graph,
-                  bool useGradient) {
+                                   const Values& givenGuess, bool useGradient) {
   gttic(InitializePose3_initialize);
   Values initialValues;
 
@@ -352,7 +309,7 @@ Values InitializePose3::initialize(const NonlinearFactorGraph& graph, const Valu
     orientations = computeOrientationsChordal(pose3Graph);
 
   // Compute the full poses (1 GN iteration on full poses)
-  return computePoses(pose3Graph, orientations);
+  return computePoses(orientations, &pose3Graph);
 }
 
 /* ************************************************************************* */

gtsam/slam/InitializePose3.h

@@ -26,6 +26,9 @@
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 
+#include <map>
+#include <vector>
+
 namespace gtsam {
 
 typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
@@ -50,9 +53,9 @@ struct GTSAM_EXPORT InitializePose3 {
       const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
       size_t maxIter = 10000, const bool setRefFrame = true);
 
-  static void createSymbolicGraph(KeyVectorMap& adjEdgesMap,
+  static void createSymbolicGraph(const NonlinearFactorGraph& pose3Graph,
-                                  KeyRotMap& factorId2RotMap,
+                                  KeyVectorMap* adjEdgesMap,
-                                  const NonlinearFactorGraph& pose3Graph);
+                                  KeyRotMap* factorId2RotMap);
 
   static Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a,
                               const double b);
@@ -64,8 +67,12 @@ struct GTSAM_EXPORT InitializePose3 {
   static NonlinearFactorGraph buildPose3graph(
       const NonlinearFactorGraph& graph);
 
-  static Values computePoses(NonlinearFactorGraph& pose3graph,
+  /**
-                             Values& initialRot);
+   * Use Gauss-Newton optimizer to optimize for poses given rotation estimates
+   */
+  static Values computePoses(const Values& initialRot,
+                             NonlinearFactorGraph* poseGraph,
+                             bool singleIter = true);
 
   /**
    * "extract" the Pose3 subgraph of the original graph, get orientations from

gtsam/slam/dataset.cpp

@@ -77,6 +77,7 @@ string findExampleDataFile(const string &name) {
   namesToSearch.push_back(name + ".txt");
   namesToSearch.push_back(name + ".out");
   namesToSearch.push_back(name + ".xml");
+  namesToSearch.push_back(name + ".g2o");
 
   // Find first name that exists
   for (const fs::path root : rootsToSearch) {
@@ -87,10 +88,12 @@ string findExampleDataFile(const string &name) {
   }
 
   // If we did not return already, then we did not find the file
-  throw invalid_argument("gtsam::findExampleDataFile could not find a matching "
+  throw invalid_argument(
+      "gtsam::findExampleDataFile could not find a matching "
       "file in\n" GTSAM_SOURCE_TREE_DATASET_DIR
       " or\n" GTSAM_INSTALLED_DATASET_DIR " named\n" +
-                         name + ", " + name + ".graph, or " + name + ".txt");
+      name + ", " + name + ".g2o, " + ", " + name + ".graph, or " + name +
+      ".txt");
 }
 
 /* ************************************************************************* */
@@ -1281,7 +1284,13 @@ Values initialCamerasAndPointsEstimate(const SfmData &db) {
   return initial;
 }
 
-// To be deprecated when pybind wrapper is merged:
+// Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
+BetweenFactorPose2s
+parse2DFactors(const std::string &filename,
+               const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex) {
+  return parseFactors<Pose2>(filename, model, maxIndex);
+}
+
 BetweenFactorPose3s
 parse3DFactors(const std::string &filename,
                const noiseModel::Diagonal::shared_ptr &model, size_t maxIndex) {

gtsam/slam/dataset.h

@@ -341,7 +341,13 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
  */
 GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
 
-// To be deprecated when pybind wrapper is merged:
+// Wrapper-friendly versions of parseFactors<Pose2> and parseFactors<Pose2>
+using BetweenFactorPose2s = std::vector<BetweenFactor<Pose2>::shared_ptr>;
+GTSAM_EXPORT BetweenFactorPose2s
+parse2DFactors(const std::string &filename,
+               const noiseModel::Diagonal::shared_ptr &model = nullptr,
+               size_t maxIndex = 0);
+
 using BetweenFactorPose3s = std::vector<BetweenFactor<Pose3>::shared_ptr>;
 GTSAM_EXPORT BetweenFactorPose3s
 parse3DFactors(const std::string &filename,

gtsam/slam/lago.cpp

@@ -17,6 +17,8 @@
  */
 
 #include <gtsam/slam/lago.h>
+
+#include <gtsam/slam/InitializePose.h>
 #include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/inference/Symbol.h>
@@ -33,7 +35,6 @@ namespace lago {
 static const Matrix I = I_1x1;
 static const Matrix I3 = I_3x3;
 
-static const Key keyAnchor = symbol('Z', 9999999);
 static const noiseModel::Diagonal::shared_ptr priorOrientationNoise =
     noiseModel::Diagonal::Sigmas((Vector(1) << 0).finished());
 static const noiseModel::Diagonal::shared_ptr priorPose2Noise =
@@ -79,7 +80,7 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
   key2doubleMap thetaToRootMap;
 
   // Orientation of the roo
-  thetaToRootMap.insert(pair<Key, double>(keyAnchor, 0.0));
+  thetaToRootMap.insert(pair<Key, double>(initialize::kAnchorKey, 0.0));
 
   // for all nodes in the tree
   for(const key2doubleMap::value_type& it: deltaThetaMap) {
@@ -189,41 +190,16 @@ GaussianFactorGraph buildLinearOrientationGraph(
     lagoGraph.add(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta);
   }
   // prior on the anchor orientation
-  lagoGraph.add(keyAnchor, I, (Vector(1) << 0.0).finished(), priorOrientationNoise);
+  lagoGraph.add(initialize::kAnchorKey, I, (Vector(1) << 0.0).finished(), priorOrientationNoise);
   return lagoGraph;
 }
 
-/* ************************************************************************* */
-// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node
-static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) {
-  gttic(lago_buildPose2graph);
-  NonlinearFactorGraph pose2Graph;
-
-  for(const boost::shared_ptr<NonlinearFactor>& factor: graph) {
-
-    // recast to a between on Pose2
-    boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
-        boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
-    if (pose2Between)
-      pose2Graph.add(pose2Between);
-
-    // recast PriorFactor<Pose2> to BetweenFactor<Pose2>
-    boost::shared_ptr<PriorFactor<Pose2> > pose2Prior =
-        boost::dynamic_pointer_cast<PriorFactor<Pose2> >(factor);
-    if (pose2Prior)
-      pose2Graph.add(
-          BetweenFactor<Pose2>(keyAnchor, pose2Prior->keys()[0],
-              pose2Prior->prior(), pose2Prior->noiseModel()));
-  }
-  return pose2Graph;
-}
-
 /* ************************************************************************* */
 static PredecessorMap<Key> findOdometricPath(
     const NonlinearFactorGraph& pose2Graph) {
 
   PredecessorMap<Key> tree;
-  Key minKey = keyAnchor; // this initialization does not matter
+  Key minKey = initialize::kAnchorKey; // this initialization does not matter
   bool minUnassigned = true;
 
   for(const boost::shared_ptr<NonlinearFactor>& factor: pose2Graph) {
@@ -240,8 +216,8 @@ static PredecessorMap<Key> findOdometricPath(
         minKey = key1;
     }
   }
-  tree.insert(minKey, keyAnchor);
+  tree.insert(minKey, initialize::kAnchorKey);
-  tree.insert(keyAnchor, keyAnchor); // root
+  tree.insert(initialize::kAnchorKey, initialize::kAnchorKey); // root
   return tree;
 }
 
@@ -284,7 +260,7 @@ VectorValues initializeOrientations(const NonlinearFactorGraph& graph,
 
   // We "extract" the Pose2 subgraph of the original graph: this
   // is done to properly model priors and avoiding operating on a larger graph
-  NonlinearFactorGraph pose2Graph = buildPose2graph(graph);
+  NonlinearFactorGraph pose2Graph = initialize::buildPoseGraph<Pose2>(graph);
 
   // Get orientations from relative orientation measurements
   return computeOrientations(pose2Graph, useOdometricPath);
@@ -338,7 +314,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph,
     }
   }
   // add prior
-  linearPose2graph.add(keyAnchor, I3, Vector3(0.0, 0.0, 0.0),
+  linearPose2graph.add(initialize::kAnchorKey, I3, Vector3(0.0, 0.0, 0.0),
       priorPose2Noise);
 
   // optimize
@@ -348,7 +324,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph,
   Values initialGuessLago;
   for(const VectorValues::value_type& it: posesLago) {
     Key key = it.first;
-    if (key != keyAnchor) {
+    if (key != initialize::kAnchorKey) {
       const Vector& poseVector = it.second;
       Pose2 poseLago = Pose2(poseVector(0), poseVector(1),
           orientationsLago.at(key)(0) + poseVector(2));
@@ -364,7 +340,7 @@ Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) {
 
   // We "extract" the Pose2 subgraph of the original graph: this
   // is done to properly model priors and avoiding operating on a larger graph
-  NonlinearFactorGraph pose2Graph = buildPose2graph(graph);
+  NonlinearFactorGraph pose2Graph = initialize::buildPoseGraph<Pose2>(graph);
 
   // Get orientations from relative orientation measurements
   VectorValues orientationsLago = computeOrientations(pose2Graph,
@@ -385,7 +361,7 @@ Values initialize(const NonlinearFactorGraph& graph,
   // for all nodes in the tree
   for(const VectorValues::value_type& it: orientations) {
     Key key = it.first;
-    if (key != keyAnchor) {
+    if (key != initialize::kAnchorKey) {
       const Pose2& pose = initialGuess.at<Pose2>(key);
       const Vector& orientation = it.second;
       Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0));

gtsam/slam/tests/testInitializePose.cpp

@@ -0,0 +1,82 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  testInitializePose3.cpp
+ *  @brief Unit tests for 3D SLAM initialization, using rotation relaxation
+ *
+ *  @author Luca Carlone
+ *  @author Frank Dellaert
+ *  @date   August, 2014
+ */
+
+#include <gtsam/slam/InitializePose.h>
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/slam/dataset.h>
+
+#include <cmath>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+TEST(InitializePose3, computePoses2D) {
+  const string g2oFile = findExampleDataFile("noisyToyGraph.txt");
+  NonlinearFactorGraph::shared_ptr inputGraph;
+  Values::shared_ptr posesInFile;
+  bool is3D = false;
+  boost::tie(inputGraph, posesInFile) = readG2o(g2oFile, is3D);
+
+  auto priorModel = noiseModel::Unit::Create(3);
+  inputGraph->addPrior(0, posesInFile->at<Pose2>(0), priorModel);
+
+  auto poseGraph = initialize::buildPoseGraph<Pose2>(*inputGraph);
+
+  auto I = genericValue(Rot3());
+  Values orientations;
+  for (size_t i : {0, 1, 2, 3})
+    orientations.insert(i, posesInFile->at<Pose2>(i).rotation());
+  const Values poses = initialize::computePoses<Pose2>(orientations, &poseGraph);
+
+  // posesInFile is seriously noisy, so we check error of recovered poses
+  EXPECT_DOUBLES_EQUAL(0.0810283, inputGraph->error(poses), 1e-6);
+}
+
+/* ************************************************************************* */
+TEST(InitializePose3, computePoses3D) {
+  const string g2oFile = findExampleDataFile("Klaus3");
+  NonlinearFactorGraph::shared_ptr inputGraph;
+  Values::shared_ptr posesInFile;
+  bool is3D = true;
+  boost::tie(inputGraph, posesInFile) = readG2o(g2oFile, is3D);
+
+  auto priorModel = noiseModel::Unit::Create(6);
+  inputGraph->addPrior(0, posesInFile->at<Pose3>(0), priorModel);
+
+  auto poseGraph = initialize::buildPoseGraph<Pose3>(*inputGraph);
+
+  auto I = genericValue(Rot3());
+  Values orientations;
+  for (size_t i : {0, 1, 2})
+    orientations.insert(i, posesInFile->at<Pose3>(i).rotation());
+  Values poses = initialize::computePoses<Pose3>(orientations, &poseGraph);
+  EXPECT(assert_equal(*posesInFile, poses, 0.1));  // TODO(frank): very loose !!
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

gtsam/slam/tests/testInitializePose3.cpp

@@ -121,7 +121,7 @@ TEST( InitializePose3, orientationsGradientSymbolicGraph ) {
   KeyVectorMap adjEdgesMap;
   KeyRotMap factorId2RotMap;
 
-  InitializePose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);
+  InitializePose3::createSymbolicGraph(pose3Graph, &adjEdgesMap, &factorId2RotMap);
 
   EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[0], 0, 1e-9);
   EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[1], 3, 1e-9);
@@ -258,21 +258,21 @@ TEST( InitializePose3, posesWithGivenGuess ) {
 }
 
 /* ************************************************************************* */
-TEST( InitializePose3, initializePoses )
+TEST(InitializePose3, initializePoses) {
-{
   const string g2oFile = findExampleDataFile("pose3example-grid");
   NonlinearFactorGraph::shared_ptr inputGraph;
-  Values::shared_ptr expectedValues;
+  Values::shared_ptr posesInFile;
   bool is3D = true;
-  boost::tie(inputGraph, expectedValues) = readG2o(g2oFile, is3D);
+  boost::tie(inputGraph, posesInFile) = readG2o(g2oFile, is3D);
-  noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
+
+  auto priorModel = noiseModel::Unit::Create(6);
   inputGraph->addPrior(0, Pose3(), priorModel);
 
   Values initial = InitializePose3::initialize(*inputGraph);
-  EXPECT(assert_equal(*expectedValues,initial,0.1));  // TODO(frank): very loose !!
+  EXPECT(assert_equal(*posesInFile, initial,
+                      0.1));  // TODO(frank): very loose !!
 }
 
-
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam_unstable/timing/timeShonanAveraging.cpp

@@ -34,13 +34,11 @@
 using namespace std;
 using namespace gtsam;
 
-// string g2oFile = findExampleDataFile("toyExample.g2o");
-
 // save a single line of timing info to an output stream
 void saveData(size_t p, double time1, double costP, double cost3, double time2,
               double min_eigenvalue, double suBound, std::ostream* os) {
-    *os << (int)p << "\t" << time1 << "\t" << costP << "\t" << cost3 << "\t"
+  *os << static_cast<int>(p) << "\t" << time1 << "\t" << costP << "\t" << cost3
-        << time2 << "\t" << min_eigenvalue << "\t" << suBound << endl;
+      << "\t" << time2 << "\t" << min_eigenvalue << "\t" << suBound << endl;
 }
 
 void checkR(const Matrix& R) {
@@ -82,7 +80,8 @@ void saveG2oResult(string name, const Values& values, std::map<Key, Pose3> poses
         myfile << i << " ";
         myfile << poses[i].x() << " " << poses[i].y() << " " << poses[i].z() << " ";
         Vector quaternion = Rot3(R).quaternion();
-        myfile << quaternion(3) << " " << quaternion(2) << " " << quaternion(1) << " " << quaternion(0);
+        myfile << quaternion(3) << " " << quaternion(2) << " " << quaternion(1)
+               << " " << quaternion(0);
         myfile << "\n";
     }
     myfile.close();
@@ -117,8 +116,7 @@ int main(int argc, char* argv[]) {
       if (argc > 1)
         g2oFile = argv[argc - 1];
       else
-            g2oFile = string(
+        g2oFile = findExampleDataFile("sphere2500");
-                "/home/jingwu/git/SESync/data/sphere2500.g2o");
 
     } catch (const exception& e) {
       cerr << e.what() << '\n';
@@ -147,14 +145,14 @@ int main(int argc, char* argv[]) {
     cout << "(int)p" << "\t" << "time1" << "\t" << "costP" << "\t" << "cost3" << "\t"
         << "time2" << "\t" << "MinEigenvalue" << "\t" << "SuBound" << endl;
 
-    const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
+    const Values randomRotations = kShonan.initializeRandomly();
 
-    for (size_t p = pMin; p < 6; p++) {
+    for (size_t p = pMin; p <= 7; p++) {
         // Randomly initialize at lowest level, initialize by line search after that
         const Values initial =
             (p > pMin) ? kShonan.initializeWithDescent(p, Qstar, minEigenVector,
                                                        lambdaMin)
-                       : ShonanAveraging::LiftTo<Rot3>(pMin, randomRotations);
+                       : ShonanAveraging3::LiftTo<Rot3>(pMin, randomRotations);
         chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
         // optimizing
         const Values result = kShonan.tryOptimizingAt(p, initial);

python/CMakeLists.txt

@@ -26,6 +26,7 @@ set(ignore
     gtsam::ISAM2ThresholdMapValue
     gtsam::FactorIndices
     gtsam::FactorIndexSet
+    gtsam::BetweenFactorPose2s
     gtsam::BetweenFactorPose3s
     gtsam::Point2Vector
     gtsam::Pose3Vector

python/gtsam/__init__.py

@@ -8,14 +8,20 @@ def _init():
 
     global Point2  # export function
 
-    def Point2(x=0, y=0):
+    def Point2(x=np.nan, y=np.nan):
         """Shim for the deleted Point2 type."""
+        if isinstance(x, np.ndarray):
+            assert x.shape == (2,), "Point2 takes 2-vector"
+            return x  # "copy constructor"
         return np.array([x, y], dtype=float)
 
     global Point3  # export function
 
-    def Point3(x=0, y=0, z=0):
+    def Point3(x=np.nan, y=np.nan, z=np.nan):
         """Shim for the deleted Point3 type."""
+        if isinstance(x, np.ndarray):
+            assert x.shape == (3,), "Point3 takes 3-vector"
+            return x  # "copy constructor"
         return np.array([x, y, z], dtype=float)
 
     # for interactive debugging

python/gtsam/examples/ShonanAveragingCLI.py

@@ -0,0 +1,125 @@
+"""
+GTSAM Copyright 2010-2018, 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
+
+Shonan Rotation Averaging CLI reads a *pose* graph, extracts the
+rotation constraints, and runs the Shonan algorithm.
+
+Author: Frank Dellaert
+Date: August 2020
+"""
+# pylint: disable=invalid-name, E1101
+
+import argparse
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+import gtsam
+from gtsam.utils import plot
+
+def estimate_poses_given_rot(factors: gtsam.BetweenFactorPose3s,
+                             rotations: gtsam.Values,
+                             d: int = 3):
+    """ Estimate Poses from measurements, given rotations. From SfmProblem in shonan.
+
+    Arguments:
+        factors -- data structure with many BetweenFactorPose3 factors
+        rotations {Values} -- Estimated rotations
+
+    Returns:
+        Values -- Estimated Poses
+    """
+
+    I_d = np.eye(d)
+
+    def R(j):
+        return rotations.atRot3(j) if d == 3 else rotations.atRot2(j)
+
+    def pose(R, t):
+        return gtsam.Pose3(R, t) if d == 3 else gtsam.Pose2(R, t)
+
+    graph = gtsam.GaussianFactorGraph()
+    model = gtsam.noiseModel.Unit.Create(d)
+
+    # Add a factor anchoring t_0
+    graph.add(0, I_d, np.zeros((d,)), model)
+
+    # Add a factor saying t_j - t_i = Ri*t_ij for all edges (i,j)
+    for factor in factors:
+        keys = factor.keys()
+        i, j, Tij = keys[0], keys[1], factor.measured()
+        measured = R(i).rotate(Tij.translation())
+        graph.add(j, I_d, i, -I_d, measured, model)
+
+    # Solve linear system
+    translations = graph.optimize()
+
+    # Convert to Values.
+    result = gtsam.Values()
+    for j in range(rotations.size()):
+        tj = translations.at(j)
+        result.insert(j, pose(R(j), tj))
+
+    return result
+
+
+def run(args):
+    """Run Shonan averaging and then recover translations linearly before saving result."""
+
+    # Get input file
+    if args.input_file:
+        input_file = args.input_file
+    else:
+        if args.named_dataset == "":
+            raise ValueError(
+                "You must either specify a named dataset or an input file")
+        input_file = gtsam.findExampleDataFile(args.named_dataset)
+
+    if args.dimension == 2:
+        print("Running Shonan averaging for SO(2) on ", input_file)
+        shonan = gtsam.ShonanAveraging2(input_file)
+        if shonan.nrUnknowns() == 0:
+            raise ValueError("No 2D pose constraints found, try -d 3.")
+        initial = shonan.initializeRandomly()
+        rotations, _ = shonan.run(initial, 2, 10)
+        factors = gtsam.parse2DFactors(input_file)
+    elif args.dimension == 3:
+        print("Running Shonan averaging for SO(3) on ", input_file)
+        shonan = gtsam.ShonanAveraging3(input_file)
+        if shonan.nrUnknowns() == 0:
+            raise ValueError("No 3D pose constraints found, try -d 2.")
+        initial = shonan.initializeRandomly()
+        rotations, _ = shonan.run(initial, 3, 10)
+        factors = gtsam.parse3DFactors(input_file)
+    else:
+        raise ValueError("Can only run SO(2) or SO(3) averaging")
+
+    print("Recovering translations")
+    poses = estimate_poses_given_rot(factors, rotations, args.dimension)
+
+    print("Writing result to ", args.output_file)
+    gtsam.writeG2o(gtsam.NonlinearFactorGraph(), poses, args.output_file)
+
+    plot.plot_trajectory(1, poses, scale=0.2)
+    plot.set_axes_equal(1)
+    plt.show()
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument('-n', '--named_dataset', type=str, default="pose3example-grid",
+                        help='Find and read frome example dataset file')
+    parser.add_argument('-i', '--input_file', type=str, default="",
+                        help='Read pose constraints graph from the specified file')
+    parser.add_argument('-o', '--output_file', type=str, default="shonan.g2o",
+                        help='Write solution to the specified file')
+    parser.add_argument('-d', '--dimension', type=int, default=3,
+                        help='Optimize over 2D or 3D rotations')
+    parser.add_argument("-p", "--plot", action="store_true", default=True,
+                        help="Plot result")
+    run(parser.parse_args())

python/gtsam/specializations.h

@@ -2,12 +2,11 @@
 // These are required to save one copy operation on Python calls
 #ifdef GTSAM_ALLOCATOR_TBB
 py::bind_vector<std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key> > >(m_, "KeyVector");
-py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> > >(m_, "Point2Vector");
-py::bind_vector<std::vector<gtsam::Pose3> >(m_, "Pose3Vector");
-py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(m_, "BetweenFactorPose3s");
 #else
 py::bind_vector<std::vector<gtsam::Key> >(m_, "KeyVector");
+#endif
 py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2> > >(m_, "Point2Vector");
 py::bind_vector<std::vector<gtsam::Pose3> >(m_, "Pose3Vector");
 py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(m_, "BetweenFactorPose3s");
-#endif
+py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > > >(m_, "BetweenFactorPose2s");
+

python/gtsam/tests/test_Point2.py

@@ -0,0 +1,29 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Point2 unit tests.
+Author: Frank Dellaert & Fan Jiang
+"""
+import unittest
+
+import gtsam
+import numpy as np
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestPoint2(GtsamTestCase):
+    """Test selected Point2 methods."""
+
+    def test_constructors(self):
+        """Test constructors from doubles and vectors."""
+        expected = gtsam.Point2(1, 2)
+        actual = gtsam.Point2(np.array([1, 2]))
+        np.testing.assert_array_equal(actual, expected)
+
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam/tests/test_Point3.py

@@ -0,0 +1,29 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Point3 unit tests.
+Author: Frank Dellaert & Fan Jiang
+"""
+import unittest
+
+import gtsam
+import numpy as np
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestPoint3(GtsamTestCase):
+    """Test selected Point3 methods."""
+
+    def test_constructors(self):
+        """Test constructors from doubles and vectors."""
+        expected = gtsam.Point3(1, 2, 3)
+        actual = gtsam.Point3(np.array([1, 2, 3]))
+        np.testing.assert_array_equal(actual, expected)
+
+
+if __name__ == "__main__":
+    unittest.main()

python/gtsam/utils/plot.py

@@ -1,9 +1,11 @@
 """Various plotting utlities."""
 
-import numpy as np
+# pylint: disable=no-member, invalid-name
+
 import matplotlib.pyplot as plt
+import numpy as np
 from matplotlib import patches
-from mpl_toolkits.mplot3d import Axes3D
+from mpl_toolkits.mplot3d import Axes3D  # pylint: disable=unused-import
 
 import gtsam
 
@@ -307,58 +309,47 @@ def plot_pose3(fignum, pose, axis_length=0.1, P=None,
 def plot_trajectory(fignum, values, scale=1, marginals=None,
                     title="Plot Trajectory", axis_labels=('X axis', 'Y axis', 'Z axis')):
     """
-    Plot a complete 3D trajectory using poses in `values`.
+    Plot a complete 2D/3D trajectory using poses in `values`.
 
     Args:
         fignum (int): Integer representing the figure number to use for plotting.
-        values (gtsam.Values): Values dict containing the poses.
+        values (gtsam.Values): Values containing some Pose2 and/or Pose3 values.
         scale (float): Value to scale the poses by.
         marginals (gtsam.Marginals): Marginalized probability values of the estimation.
             Used to plot uncertainty bounds.
         title (string): The title of the plot.
         axis_labels (iterable[string]): List of axis labels to set.
     """
-    pose3Values = gtsam.utilities.allPose3s(values)
+    fig = plt.figure(fignum)
-    keys = gtsam.KeyVector(pose3Values.keys())
+    axes = fig.gca(projection='3d')
-    lastKey = None
 
-    for key in keys:
+    axes.set_xlabel(axis_labels[0])
-        try:
+    axes.set_ylabel(axis_labels[1])
-            pose = pose3Values.atPose3(key)
+    axes.set_zlabel(axis_labels[2])
-        except:
-            print("Warning: no Pose3 at key: {0}".format(key))
-
-        if lastKey is not None:
-            try:
-                lastPose = pose3Values.atPose3(lastKey)
-            except:
-                print("Warning: no Pose3 at key: {0}".format(lastKey))
-                pass
 
+    # Plot 2D poses, if any
+    poses = gtsam.utilities.allPose2s(values)
+    for key in poses.keys():
+        pose = poses.atPose2(key)
         if marginals:
-                covariance = marginals.marginalCovariance(lastKey)
+            covariance = marginals.marginalCovariance(key)
         else:
             covariance = None
 
-            fig = plot_pose3(fignum, lastPose,  P=covariance,
+        plot_pose2_on_axes(axes, pose, covariance=covariance,
-                             axis_length=scale, axis_labels=axis_labels)
+                           axis_length=scale)
 
-        lastKey = key
+    # Then 3D poses, if any
-
+    poses = gtsam.utilities.allPose3s(values)
-    # Draw final pose
+    for key in poses.keys():
-    if lastKey is not None:
+        pose = poses.atPose3(key)
-        try:
-            lastPose = pose3Values.atPose3(lastKey)
         if marginals:
-                covariance = marginals.marginalCovariance(lastKey)
+            covariance = marginals.marginalCovariance(key)
         else:
             covariance = None
 
-            fig = plot_pose3(fignum, lastPose, P=covariance,
+        plot_pose3_on_axes(axes, pose, P=covariance,
-                             axis_length=scale, axis_labels=axis_labels)
+                           axis_length=scale)
-
-        except:
-            pass
 
     fig.suptitle(title)
     fig.canvas.set_window_title(title.lower())
@@ -383,8 +374,8 @@ def plot_incremental_trajectory(fignum, values, start=0,
     fig = plt.figure(fignum)
     axes = fig.gca(projection='3d')
 
-    pose3Values = gtsam.utilities.allPose3s(values)
+    poses = gtsam.utilities.allPose3s(values)
-    keys = gtsam.KeyVector(pose3Values.keys())
+    keys = gtsam.KeyVector(poses.keys())
 
     for key in keys[start:]:
         if values.exists(key):