Merge pull request #206 from ptrmu/develop

make code match comments when creating Pose3 noiseModel instances

cython/gtsam/examples/ImuFactorExample2.py

@@ -106,7 +106,7 @@ def IMU_example():
     # Add a prior on pose x0. This indirectly specifies where the origin is.
     # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
     noise = gtsam.noiseModel_Diagonal.Sigmas(
-        np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
+        np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3]))
     newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise))
 
     # Add imu priors

cython/gtsam/examples/VisualISAM2Example.py

@@ -120,7 +120,7 @@ def visual_ISAM2_example():
         if i == 0:
             # Add a prior on pose x0
             pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array(
-                [0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))  # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+                [0.1, 0.1, 0.1, 0.3, 0.3, 0.3]))  # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
             graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], pose_noise))
 
             # Add a prior on landmark l0

examples/ISAM2Example_SmartFactor.cpp

@@ -27,7 +27,7 @@ int main(int argc, char* argv[]) {
       noiseModel::Isotropic::Sigma(2, 1.0);  // one pixel in u and v
 
   Vector6 sigmas;
-  sigmas << Vector3::Constant(0.3), Vector3::Constant(0.1);
+  sigmas << Vector3::Constant(0.1), Vector3::Constant(0.3);
   auto noise = noiseModel::Diagonal::Sigmas(sigmas);
 
   ISAM2Params parameters;

examples/SFMExample.cpp

@@ -74,7 +74,7 @@ int main(int argc, char* argv[]) {
   NonlinearFactorGraph graph;
 
   // Add a prior on pose x1. This indirectly specifies where the origin is.
-  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
   graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise); // add directly to graph
 
   // Simulated measurements from each camera pose, adding them to the factor graph

examples/SFMExample_SmartFactor.cpp

@@ -81,7 +81,7 @@ int main(int argc, char* argv[]) {
   // Add a prior on pose x0. This indirectly specifies where the origin is.
   // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
   noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
-      (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
+      (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
   graph.emplace_shared<PriorFactor<Pose3> >(0, poses[0], noise);
 
   // Because the structure-from-motion problem has a scale ambiguity, the problem is

examples/SFMExample_SmartFactorPCG.cpp

@@ -73,7 +73,7 @@ int main(int argc, char* argv[]) {
   // Add a prior on pose x0. This indirectly specifies where the origin is.
   // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
   noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
-      (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
+      (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
   graph.emplace_shared<PriorFactor<Pose3> >(0, poses[0], noise);
 
   // Fix the scale ambiguity by adding a prior

examples/SelfCalibrationExample.cpp

@@ -53,7 +53,7 @@ int main(int argc, char* argv[]) {
   NonlinearFactorGraph graph;
 
   // Add a prior on pose x1.
-  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
   graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise);
 
   // Simulated measurements from each camera pose, adding them to the factor graph

examples/VisualISAM2Example.cpp

@@ -108,7 +108,7 @@ int main(int argc, char* argv[]) {
     if (i == 0) {
       // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw
       static auto kPosePrior = noiseModel::Diagonal::Sigmas(
-          (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))
+          (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
               .finished());
       graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0],
                                                 kPosePrior);

examples/VisualISAMExample.cpp

@@ -107,7 +107,7 @@ int main(int argc, char* argv[]) {
     if (i == 0) {
       // Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
       noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
-          (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
+          (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
       graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise);
 
       // Add a prior on landmark l0

matlab/unstable_examples/+imuSimulator/OdometryExample3D.m

@@ -25,12 +25,12 @@ graph = NonlinearFactorGraph;
 
 %% Add a Gaussian prior on pose x_1
 priorMean = Pose3();%Pose3.Expmap([0.0; 0.0; 0.0; 0.0; 0.0; 0.0]); % prior mean is at origin
-priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.3; 0.1; 0.1; 0.1]); % 30cm std on x,y, 0.1 rad on theta
+priorNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.1; 0.3; 0.3; 0.3]); % 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
 graph.add(PriorFactorPose3(1, priorMean, priorNoise)); % add directly to graph
 
 %% Add two odometry factors
 odometry = Pose3.Expmap([0.0; 0.0; 0.0; 2.0; 0.0; 0.0]); % create a measurement for both factors (the same in this case)
-odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.2; 0.1; 0.1; 0.1]); % 20cm std on x,y, 0.1 rad on theta
+odometryNoise = noiseModel.Diagonal.Sigmas([0.1; 0.1; 0.1; 0.2; 0.2; 0.2]); % 20cm std on x,y,z 0.1 rad on roll,pitch,yaw
 graph.add(BetweenFactorPose3(1, 2, odometry, odometryNoise));
 graph.add(BetweenFactorPose3(2, 3, odometry, odometryNoise));
 

tests/testVisualISAM2.cpp

@@ -76,7 +76,7 @@ TEST(testVisualISAM2, all)
         {
             // Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw
             static auto kPosePrior = noiseModel::Diagonal::Sigmas(
-                (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))
+                (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
                     .finished());
             graph.emplace_shared<PriorFactor<Pose3>>(Symbol('x', 0), poses[0],
                                                      kPosePrior);