Merged in feature/pose3_init (pull request #400)

Pose3 Initialization in Python/MATLAB

Approved-by: Luca Carlone <luca.carlone@gatech.edu>

cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py

@@ -0,0 +1,35 @@
+"""
+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
+
+Initialize PoseSLAM with Chordal init
+Author: Luca Carlone, Frank Dellaert (python port)
+"""
+# pylint: disable=invalid-name, E1101
+
+from __future__ import print_function
+
+import numpy as np
+
+import gtsam
+
+# Read graph from file
+g2oFile = gtsam.findExampleDataFile("pose3example.txt")
+
+is3D = True
+graph, initial = gtsam.readG2o(g2oFile, is3D)
+
+# Add prior on the first key. TODO: assumes first key ios z
+priorModel = gtsam.noiseModel_Diagonal.Variances(
+    np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
+firstKey = initial.keys().at(0)
+graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel))
+
+# Initializing Pose3 - chordal relaxation"
+initialization = gtsam.InitializePose3.initialize(graph)
+
+print(initialization)

cython/gtsam/tests/test_initialize_pose3.py

@@ -0,0 +1,88 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Unit tests for 3D SLAM initialization, using rotation relaxation.
+Author: Luca Carlone and Frank Dellaert (Python)
+"""
+# pylint: disable=invalid-name, E1101, E0611
+import unittest
+
+import numpy as np
+
+import gtsam
+from gtsam import NonlinearFactorGraph, Point3, Pose3, Rot3, Values
+
+x0, x1, x2, x3 = 0, 1, 2, 3
+
+
+class TestValues(unittest.TestCase):
+
+    def setUp(self):
+
+        model = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
+
+        # We consider a small graph:
+        #                            symbolic FG
+        #               x2               0  1
+        #             / | \              1  2
+        #            /  |  \             2  3
+        #          x3   |   x1           2  0
+        #           \   |   /            0  3
+        #            \  |  /
+        #               x0
+        #
+        p0 = Point3(0, 0, 0)
+        self.R0 = Rot3.Expmap(np.array([0.0, 0.0, 0.0]))
+        p1 = Point3(1, 2, 0)
+        self.R1 = Rot3.Expmap(np.array([0.0, 0.0, 1.570796]))
+        p2 = Point3(0, 2, 0)
+        self.R2 = Rot3.Expmap(np.array([0.0, 0.0, 3.141593]))
+        p3 = Point3(-1, 1, 0)
+        self.R3 = Rot3.Expmap(np.array([0.0, 0.0, 4.712389]))
+
+        pose0 = Pose3(self.R0, p0)
+        pose1 = Pose3(self.R1, p1)
+        pose2 = Pose3(self.R2, p2)
+        pose3 = Pose3(self.R3, p3)
+
+        g = NonlinearFactorGraph()
+        g.add(gtsam.BetweenFactorPose3(x0, x1, pose0.between(pose1), model))
+        g.add(gtsam.BetweenFactorPose3(x1, x2, pose1.between(pose2), model))
+        g.add(gtsam.BetweenFactorPose3(x2, x3, pose2.between(pose3), model))
+        g.add(gtsam.BetweenFactorPose3(x2, x0, pose2.between(pose0), model))
+        g.add(gtsam.BetweenFactorPose3(x0, x3, pose0.between(pose3), model))
+        g.add(gtsam.PriorFactorPose3(x0, pose0, model))
+        self.graph = g
+
+    def test_buildPose3graph(self):
+        pose3graph = gtsam.InitializePose3.buildPose3graph(self.graph)
+
+    def test_orientations(self):
+        pose3Graph = gtsam.InitializePose3.buildPose3graph(self.graph)
+
+        initial = gtsam.InitializePose3.computeOrientationsChordal(pose3Graph)
+
+        # comparison is up to M_PI, that's why we add some multiples of 2*M_PI
+        self.assertTrue(initial.atRot3(x0).equals(self.R0, 1e-6))
+        self.assertTrue(initial.atRot3(x1).equals(self.R1, 1e-6))
+        self.assertTrue(initial.atRot3(x2).equals(self.R2, 1e-6))
+        self.assertTrue(initial.atRot3(x3).equals(self.R3, 1e-6))
+
+    def test_initializePoses(self):
+        g2oFile = gtsam.findExampleDataFile("pose3example-grid")
+        is3D = True
+        inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D)
+        priorModel = gtsam.noiseModel_Unit.Create(6)
+        inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel))
+
+        initial = gtsam.InitializePose3.initialize(inputGraph)
+        # TODO(frank): very loose !!
+        self.assertTrue(initial.equals(expectedValues, 0.1))
+
+
+if __name__ == "__main__":
+    unittest.main()

gtsam.h

@@ -2523,6 +2523,26 @@ class BetweenFactorPose3s
   gtsam::BetweenFactorPose3* at(size_t i) const;
 };
 
+#include <gtsam/slam/InitializePose3.h>
+class InitializePose3 {
+  static gtsam::Values computeOrientationsChordal(
+      const gtsam::NonlinearFactorGraph& pose3Graph);
+  static gtsam::Values computeOrientationsGradient(
+      const gtsam::NonlinearFactorGraph& pose3Graph,
+      const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame);
+  static gtsam::Values computeOrientationsGradient(
+      const gtsam::NonlinearFactorGraph& pose3Graph,
+      const gtsam::Values& givenGuess);
+  static gtsam::NonlinearFactorGraph buildPose3graph(
+      const gtsam::NonlinearFactorGraph& graph);
+  static gtsam::Values initializeOrientations(
+      const gtsam::NonlinearFactorGraph& graph);
+  static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph,
+                                  const gtsam::Values& givenGuess,
+                                  bool useGradient);
+  static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph);
+};
+
 gtsam::BetweenFactorPose3s parse3DFactors(string filename);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
 

gtsam/slam/InitializePose3.cpp

@@ -16,7 +16,7 @@
  *  @date   August, 2014
  */
 
-#include <gtsam/slam/InitializePose3.h>
+#include <gtsam/slam/InitializePose3.h> 
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
@@ -29,69 +29,65 @@
 using namespace std;
 
 namespace gtsam {
-namespace InitializePose3 {
 
-static const Matrix I9 = I_9x9;
+static const Key kAnchorKey = symbol('Z', 9999999);
-static const Vector zero9 = Vector::Zero(9);
-static const Matrix zero33 = Z_3x3;
-
-static const Key keyAnchor = symbol('Z', 9999999);
 
 /* ************************************************************************* */
-GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
+GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
 
   GaussianFactorGraph linearGraph;
-  noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9);
 
-  for(const boost::shared_ptr<NonlinearFactor>& factor: g) {
+  for(const auto& factor: g) {
     Matrix3 Rij;
+    double rotationPrecision = 1.0;
 
-    boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
+    auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
-        boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
+    if (pose3Between){
-    if (pose3Between)
       Rij = pose3Between->measured().rotation().matrix();
-    else
+      Vector precisions = Vector::Zero(6);
-      std::cout << "Error in buildLinearOrientationGraph" << std::endl;
+      precisions[0] = 1.0; // vector of all zeros except first entry equal to 1
+      pose3Between->noiseModel()->whitenInPlace(precisions); // gets marginal precision of first variable
+      rotationPrecision = precisions[0]; // rotations first
+    }else{
+      cout << "Error in buildLinearOrientationGraph" << endl;
+    }
 
-    const KeyVector& keys = factor->keys();
+    const auto& keys = factor->keys();
     Key key1 = keys[0], key2 = keys[1];
     Matrix M9 = Z_9x9;
     M9.block(0,0,3,3) = Rij;
     M9.block(3,3,3,3) = Rij;
     M9.block(6,6,3,3) = Rij;
-    linearGraph.add(key1, -I9, key2, M9, zero9, model);
+    linearGraph.add(key1, -I_9x9, key2, M9, Z_9x1, noiseModel::Isotropic::Precision(9, rotationPrecision));
   }
   // prior on the anchor orientation
-  linearGraph.add(keyAnchor, I9, (Vector(9) << 1.0, 0.0, 0.0,/*  */ 0.0, 1.0, 0.0, /*  */ 0.0, 0.0, 1.0).finished(), model);
+  linearGraph.add(
+      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));
   return linearGraph;
 }
 
 /* ************************************************************************* */
 // Transform VectorValues into valid Rot3
-Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
+Values InitializePose3::normalizeRelaxedRotations(
+    const VectorValues& relaxedRot3) {
   gttic(InitializePose3_computeOrientationsChordal);
 
   Matrix ppm = Z_3x3; // plus plus minus
   ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1;
 
   Values validRot3;
-  for(const VectorValues::value_type& it: relaxedRot3) {
+  for(const auto& it: relaxedRot3) {
     Key key = it.first;
-    if (key != keyAnchor) {
+    if (key != kAnchorKey) {
-      const Vector& rotVector = it.second;
+      Matrix3 R; R << it.second;
-      Matrix3 rotMat;
-      rotMat(0,0) = rotVector(0); rotMat(0,1) = rotVector(1); rotMat(0,2) = rotVector(2);
-      rotMat(1,0) = rotVector(3); rotMat(1,1) = rotVector(4); rotMat(1,2) = rotVector(5);
-      rotMat(2,0) = rotVector(6); rotMat(2,1) = rotVector(7); rotMat(2,2) = rotVector(8);
 
       Matrix U, V; Vector s;
-      svd(rotMat, U, s, V);
+      svd(R.transpose(), U, s, V);
       Matrix3 normalizedRotMat = U * V.transpose();
 
-      //      std::cout << "rotMat \n" << rotMat << std::endl;
-      //      std::cout << "U V' \n" << U * V.transpose() << std::endl;
-      //      std::cout << "V \n" << V << std::endl;
-
       if(normalizedRotMat.determinant() < 0)
         normalizedRotMat = U * ppm * V.transpose();
 
@@ -103,32 +99,29 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
 }
 
 /* ************************************************************************* */
-// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node
+NonlinearFactorGraph InitializePose3::buildPose3graph(const NonlinearFactorGraph& graph) {
-NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) {
   gttic(InitializePose3_buildPose3graph);
   NonlinearFactorGraph pose3Graph;
 
-  for(const boost::shared_ptr<NonlinearFactor>& factor: graph) {
+  for(const auto& factor: graph) {
 
     // recast to a between on Pose3
-    boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
+    const auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
-        boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
     if (pose3Between)
       pose3Graph.add(pose3Between);
 
     // recast PriorFactor<Pose3> to BetweenFactor<Pose3>
-    boost::shared_ptr<PriorFactor<Pose3> > pose3Prior =
+    const auto pose3Prior = boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor);
-        boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor);
     if (pose3Prior)
-      pose3Graph.emplace_shared<BetweenFactor<Pose3> >(keyAnchor, pose3Prior->keys()[0],
+      pose3Graph.emplace_shared<BetweenFactor<Pose3> >(kAnchorKey, pose3Prior->keys()[0],
               pose3Prior->prior(), pose3Prior->noiseModel());
   }
   return pose3Graph;
 }
 
 /* ************************************************************************* */
-// Return the orientations of a graph including only BetweenFactors<Pose3>
+Values InitializePose3::computeOrientationsChordal(
-Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
+    const NonlinearFactorGraph& pose3Graph) {
   gttic(InitializePose3_computeOrientationsChordal);
 
   // regularize measurements and plug everything in a factor graph
@@ -142,14 +135,15 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
 }
 
 /* ************************************************************************* */
-// Return the orientations of a graph including only BetweenFactors<Pose3>
+Values InitializePose3::computeOrientationsGradient(
-Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter, const bool setRefFrame) {
+    const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
+    const size_t maxIter, const bool setRefFrame) {
   gttic(InitializePose3_computeOrientationsGradient);
 
   // this works on the inverse rotations, according to Tron&Vidal,2011
   Values inverseRot;
-  inverseRot.insert(keyAnchor, Rot3());
+  inverseRot.insert(kAnchorKey, Rot3());
-  for(const Values::ConstKeyValuePair& key_value: givenGuess) {
+  for(const auto& key_value: givenGuess) {
     Key key = key_value.key;
     const Pose3& pose = givenGuess.at<Pose3>(key);
     inverseRot.insert(key, pose.rotation().inverse());
@@ -164,9 +158,9 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
   // calculate max node degree & allocate gradient
   size_t maxNodeDeg = 0;
   VectorValues grad;
-  for(const Values::ConstKeyValuePair& key_value: inverseRot) {
+  for(const auto& key_value: inverseRot) {
     Key key = key_value.key;
-    grad.insert(key,Vector3::Zero());
+    grad.insert(key,Z_3x1);
     size_t currNodeDeg = (adjEdgesMap.at(key)).size();
     if(currNodeDeg > maxNodeDeg)
       maxNodeDeg = currNodeDeg;
@@ -180,42 +174,37 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
   double mu_max = maxNodeDeg * rho;
   double stepsize = 2/mu_max; // = 1/(a b dG)
 
-  std::cout <<" b " << b <<" f0 " << f0 <<" a " << a <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
   double maxGrad;
   // gradient iterations
   size_t it;
-  for(it=0; it < maxIter; it++){
+  for (it = 0; it < maxIter; it++) {
     //////////////////////////////////////////////////////////////////////////
     // compute the gradient at each node
-    //std::cout << "it  " << it <<" b " << b <<" f0 " << f0 <<" a " << a
-    //   <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
     maxGrad = 0;
-    for(const Values::ConstKeyValuePair& key_value: inverseRot) {
+    for (const auto& key_value : inverseRot) {
       Key key = key_value.key;
-      //std::cout << "---------------------------key  " << DefaultKeyFormatter(key) << std::endl;
+      Vector gradKey = Z_3x1;
-      Vector gradKey = Vector3::Zero();
       // collect the gradient for each edge incident on key
-      for(const size_t& factorId: adjEdgesMap.at(key)){
+      for (const size_t& factorId : adjEdgesMap.at(key)) {
         Rot3 Rij = factorId2RotMap.at(factorId);
         Rot3 Ri = inverseRot.at<Rot3>(key);
-        if( key == (pose3Graph.at(factorId))->keys()[0] ){
+        auto factor = pose3Graph.at(factorId);
-          Key key1 = (pose3Graph.at(factorId))->keys()[1];
+        const auto& keys = factor->keys();
+        if (key == keys[0]) {
+          Key key1 = keys[1];
           Rot3 Rj = inverseRot.at<Rot3>(key1);
-          gradKey = gradKey + gradientTron(Ri, Rij  * Rj, a, b);
+          gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b);
-          //std::cout << "key1 " << DefaultKeyFormatter(key1) << " gradientTron(Ri, Rij  * Rj, a, b) \n " << gradientTron(Ri, Rij  * Rj, a, b) << std::endl;
+        } else if (key == keys[1]) {
-        }else if( key == (pose3Graph.at(factorId))->keys()[1] ){
+          Key key0 = keys[0];
-          Key key0 = (pose3Graph.at(factorId))->keys()[0];
           Rot3 Rj = inverseRot.at<Rot3>(key0);
           gradKey = gradKey + gradientTron(Ri, Rij.between(Rj), a, b);
-          //std::cout << "key0 " << DefaultKeyFormatter(key0) << " gradientTron(Ri, Rij.inverse()  * Rj, a, b) \n " << gradientTron(Ri, Rij.between(Rj), a, b) << std::endl;
+        } else {
-        }else{
+          cout << "Error in gradient computation" << endl;
-          std::cout << "Error in gradient computation" << std::endl;
         }
-      } // end of i-th gradient computation
+      }  // end of i-th gradient computation
-      grad.at(key) = stepsize *  gradKey;
+      grad.at(key) = stepsize * gradKey;
 
       double normGradKey = (gradKey).norm();
-      //std::cout << "key  " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl;
       if(normGradKey>maxGrad)
         maxGrad = normGradKey;
     } // end of loop over nodes
@@ -230,14 +219,12 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
       break;
   } // enf of gradient iterations
 
-  std::cout << "nr of gradient iterations " << it << "maxGrad " << maxGrad <<  std::endl;
-
   // Return correct rotations
-  const Rot3& Rref = inverseRot.at<Rot3>(keyAnchor); // This will be set to the identity as so far we included no prior
+  const Rot3& Rref = inverseRot.at<Rot3>(kAnchorKey); // This will be set to the identity as so far we included no prior
   Values estimateRot;
-  for(const Values::ConstKeyValuePair& key_value: inverseRot) {
+  for(const auto& key_value: inverseRot) {
     Key key = key_value.key;
-    if (key != keyAnchor) {
+    if (key != kAnchorKey) {
       const Rot3& R = inverseRot.at<Rot3>(key);
       if(setRefFrame)
         estimateRot.insert(key, Rref.compose(R.inverse()));
@@ -249,11 +236,11 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
 }
 
 /* ************************************************************************* */
-void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph){
+void InitializePose3::createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
+                         const NonlinearFactorGraph& pose3Graph) {
   size_t factorId = 0;
-  for(const boost::shared_ptr<NonlinearFactor>& factor: pose3Graph) {
+  for(const auto& factor: pose3Graph) {
-    boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
+    auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
-        boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
     if (pose3Between){
       Rot3 Rij = pose3Between->measured().rotation();
       factorId2RotMap.insert(pair<Key, Rot3 >(factorId,Rij));
@@ -275,14 +262,14 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
         adjEdgesMap.insert(pair<Key, vector<size_t> >(key2, edge_id));
       }
     }else{
-      std::cout << "Error in computeOrientationsGradient" << std::endl;
+      cout << "Error in createSymbolicGraph" << endl;
     }
     factorId++;
   }
 }
 
 /* ************************************************************************* */
-Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) {
+Vector3 InitializePose3::gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) {
   Vector3 logRot = Rot3::Logmap(R1.between(R2));
 
   double th = logRot.norm();
@@ -292,10 +279,10 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl
     th = logRot.norm();
   }
   // exclude small or invalid rotations
-  if (th > 1e-5 && th == th){ // nonzero valid rotations
+  if (th > 1e-5 && th == th) {  // nonzero valid rotations
     logRot = logRot / th;
-  }else{
+  } else {
-    logRot = Vector3::Zero();
+    logRot = Z_3x1;
     th = 0.0;
   }
 
@@ -304,8 +291,7 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl
 }
 
 /* ************************************************************************* */
-Values initializeOrientations(const NonlinearFactorGraph& graph) {
+Values InitializePose3::initializeOrientations(const NonlinearFactorGraph& graph) {
-
   // We "extract" the Pose3 subgraph of the original graph: this
   // is done to properly model priors and avoiding operating on a larger graph
   NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
@@ -315,29 +301,30 @@ Values initializeOrientations(const NonlinearFactorGraph& graph) {
 }
 
 ///* ************************************************************************* */
-Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
+Values InitializePose3::computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
   gttic(InitializePose3_computePoses);
 
   // put into Values structure
   Values initialPose;
-  for(const Values::ConstKeyValuePair& key_value: initialRot){
+  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(keyAnchor, Pose3());
+  initialPose.insert(kAnchorKey, Pose3());
-  pose3graph.emplace_shared<PriorFactor<Pose3> >(keyAnchor, Pose3(), priorModel);
+  pose3graph.emplace_shared<PriorFactor<Pose3> >(kAnchorKey, Pose3(), priorModel);
 
   // Create optimizer
   GaussNewtonParams params;
   bool singleIter = true;
-  if(singleIter){
+  if (singleIter) {
     params.maxIterations = 1;
-  }else{
+  } else {
-    std::cout << " \n\n\n\n  performing more than 1 GN iterations \n\n\n" <<std::endl;
+    cout << " \n\n\n\n  performing more than 1 GN iterations \n\n\n" << endl;
     params.setVerbosity("TERMINATION");
   }
   GaussNewtonOptimizer optimizer(pose3graph, initialPose, params);
@@ -345,9 +332,9 @@ Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
 
   // put into Values structure
   Values estimate;
-  for(const Values::ConstKeyValuePair& key_value: GNresult) {
+  for (const auto& key_value : GNresult) {
     Key key = key_value.key;
-    if (key != keyAnchor) {
+    if (key != kAnchorKey) {
       const Pose3& pose = GNresult.at<Pose3>(key);
       estimate.insert(key, pose);
     }
@@ -356,22 +343,9 @@ Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
 }
 
 /* ************************************************************************* */
-Values initialize(const NonlinearFactorGraph& graph) {
+Values InitializePose3::initialize(const NonlinearFactorGraph& graph, const Values& givenGuess,
+                  bool useGradient) {
   gttic(InitializePose3_initialize);
-
-  // We "extract" the Pose3 subgraph of the original graph: this
-  // is done to properly model priors and avoiding operating on a larger graph
-  NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
-
-  // Get orientations from relative orientation measurements
-  Values valueRot3 = computeOrientationsChordal(pose3Graph);
-
-  // Compute the full poses (1 GN iteration on full poses)
-  return computePoses(pose3Graph, valueRot3);
-}
-
-/* ************************************************************************* */
-Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient) {
   Values initialValues;
 
   // We "extract" the Pose3 subgraph of the original graph: this
@@ -380,27 +354,18 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, b
 
   // Get orientations from relative orientation measurements
   Values orientations;
-  if(useGradient)
+  if (useGradient)
     orientations = computeOrientationsGradient(pose3Graph, givenGuess);
   else
     orientations = computeOrientationsChordal(pose3Graph);
 
-//  orientations.print("orientations\n");
-
   // Compute the full poses (1 GN iteration on full poses)
   return computePoses(pose3Graph, orientations);
-
-  //  for(const Values::ConstKeyValuePair& key_value: orientations) {
-  //    Key key = key_value.key;
-  //    if (key != keyAnchor) {
-  //      const Point3& pos = givenGuess.at<Pose3>(key).translation();
-  //      const Rot3& rot = orientations.at<Rot3>(key);
-  //      Pose3 initializedPoses = Pose3(rot, pos);
-  //      initialValues.insert(key, initializedPoses);
-  //    }
-  //  }
-  //  return initialValues;
 }
 
-} // end of namespace lago
+/* ************************************************************************* */
-} // end of namespace gtsam
+Values InitializePose3::initialize(const NonlinearFactorGraph& graph) {
+  return initialize(graph, Values(), false);
+}
+
+} // namespace gtsam

gtsam/slam/InitializePose3.h

@@ -20,40 +20,68 @@
 
 #pragma once
 
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/inference/graph.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/VectorValues.h>
-#include <gtsam/inference/graph.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/geometry/Rot3.h>
 
 namespace gtsam {
 
 typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
-typedef std::map<Key, Rot3 > KeyRotMap;
+typedef std::map<Key, Rot3> KeyRotMap;
 
-namespace InitializePose3 {
+struct GTSAM_EXPORT InitializePose3 {
+  static GaussianFactorGraph buildLinearOrientationGraph(
+      const NonlinearFactorGraph& g);
 
-GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g);
+  static Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
 
-GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
+  /**
+   * Return the orientations of a graph including only BetweenFactors<Pose3>
+   */
+  static Values computeOrientationsChordal(
+      const NonlinearFactorGraph& pose3Graph);
 
-GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph);
+  /**
+   * Return the orientations of a graph including only BetweenFactors<Pose3>
+   */
+  static Values computeOrientationsGradient(
+      const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
+      size_t maxIter = 10000, const bool setRefFrame = true);
 
-GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph,
+  static void createSymbolicGraph(KeyVectorMap& adjEdgesMap,
-    const Values& givenGuess, size_t maxIter = 10000, const bool setRefFrame = true);
+                                  KeyRotMap& factorId2RotMap,
+                                  const NonlinearFactorGraph& pose3Graph);
 
-GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
+  static Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a,
-    const NonlinearFactorGraph& pose3Graph);
+                              const double b);
 
-GTSAM_EXPORT Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b);
+  /**
+   * Select the subgraph of betweenFactors and transforms priors into between
+   * wrt a fictitious node
+   */
+  static NonlinearFactorGraph buildPose3graph(
+      const NonlinearFactorGraph& graph);
 
-GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph);
+  static Values computePoses(NonlinearFactorGraph& pose3graph,
+                             Values& initialRot);
 
-GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot);
+  /**
+   * "extract" the Pose3 subgraph of the original graph, get orientations from
+   * relative orientation measurements using chordal method.
+   */
+  static Values initializeOrientations(const NonlinearFactorGraph& graph);
 
-GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph);
+  /**
+   * "extract" the Pose3 subgraph of the original graph, get orientations from
+   * relative orientation measurements (using either gradient or chordal
+   * method), and finish up with 1 GN iteration on full poses.
+   */
+  static Values initialize(const NonlinearFactorGraph& graph,
+                           const Values& givenGuess, bool useGradient = false);
 
-GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient = false);
+  /// Calls initialize above using Chordal method.
-
+  static Values initialize(const NonlinearFactorGraph& graph);
-} // end of namespace lago
+};
-} // end of namespace gtsam
+}  // end of namespace gtsam

gtsam/slam/tests/testInitializePose3.cpp

@@ -70,6 +70,17 @@ NonlinearFactorGraph graph() {
   g.add(PriorFactor<Pose3>(x0, pose0, model));
   return g;
 }
+
+NonlinearFactorGraph graph2() {
+  NonlinearFactorGraph g;
+  g.add(BetweenFactor<Pose3>(x0, x1, pose0.between(pose1), noiseModel::Isotropic::Precision(6, 1.0)));
+  g.add(BetweenFactor<Pose3>(x1, x2, pose1.between(pose2), noiseModel::Isotropic::Precision(6, 1.0)));
+  g.add(BetweenFactor<Pose3>(x2, x3, pose2.between(pose3), noiseModel::Isotropic::Precision(6, 1.0)));
+  g.add(BetweenFactor<Pose3>(x2, x0, Pose3(Rot3::Ypr(0.1,0,0.1), Point3()), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero information
+  g.add(BetweenFactor<Pose3>(x0, x3, Pose3(Rot3::Ypr(0.5,-0.2,0.2), Point3(10,20,30)), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero informatoin
+  g.add(PriorFactor<Pose3>(x0, pose0, model));
+  return g;
+}
 }
 
 /* *************************************************************************** */
@@ -91,6 +102,19 @@ TEST( InitializePose3, orientations ) {
   EXPECT(assert_equal(simple::R3, initial.at<Rot3>(x3), 1e-6));
 }
 
+/* *************************************************************************** */
+TEST( InitializePose3, orientationsPrecisions ) {
+  NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph2());
+
+  Values initial = InitializePose3::computeOrientationsChordal(pose3Graph);
+
+  // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
+  EXPECT(assert_equal(simple::R0, initial.at<Rot3>(x0), 1e-6));
+  EXPECT(assert_equal(simple::R1, initial.at<Rot3>(x1), 1e-6));
+  EXPECT(assert_equal(simple::R2, initial.at<Rot3>(x2), 1e-6));
+  EXPECT(assert_equal(simple::R3, initial.at<Rot3>(x3), 1e-6));
+}
+
 /* *************************************************************************** */
 TEST( InitializePose3, orientationsGradientSymbolicGraph ) {
   NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());