now initialization is aware of rotation noise model

gtsam/slam/InitializePose3.cpp

@@ -36,16 +36,21 @@ static const Key kAnchorKey = symbol('Z', 9999999);
 GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
 
   GaussianFactorGraph linearGraph;
-  noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9);
 
   for(const auto& factor: g) {
     Matrix3 Rij;
+    double rotationPrecision = 1.0;
 
     auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
-    if (pose3Between)
+    if (pose3Between){
       Rij = pose3Between->measured().rotation().matrix();
-    else
+      Vector precisions = Vector::Zero(6);
+      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 auto& keys = factor->keys();
     Key key1 = keys[0], key2 = keys[1];
@@ -53,14 +58,14 @@ GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const Nonlinear
     M9.block(0,0,3,3) = Rij;
     M9.block(3,3,3,3) = Rij;
     M9.block(6,6,3,3) = Rij;
-    linearGraph.add(key1, -I_9x9, key2, M9, Z_9x1, model);
+    linearGraph.add(key1, -I_9x9, key2, M9, Z_9x1, noiseModel::Isotropic::Precision(9, rotationPrecision));
   }
   // prior on the anchor orientation
   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(),
-      model);
+          noiseModel::Isotropic::Precision(9, 1));
   return linearGraph;
 }
 

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());