diff --git a/cython/gtsam/examples/ImuFactorExample2.py b/cython/gtsam/examples/ImuFactorExample2.py index 10bd3de11..1f6732f49 100644 --- a/cython/gtsam/examples/ImuFactorExample2.py +++ b/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 diff --git a/cython/gtsam/examples/VisualISAM2Example.py b/cython/gtsam/examples/VisualISAM2Example.py index e2caee6d4..c0f4fbd94 100644 --- a/cython/gtsam/examples/VisualISAM2Example.py +++ b/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 diff --git a/examples/ISAM2Example_SmartFactor.cpp b/examples/ISAM2Example_SmartFactor.cpp index 8331ade04..d8fee1516 100644 --- a/examples/ISAM2Example_SmartFactor.cpp +++ b/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; diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 0e48bb892..8c87fa315 100644 --- a/examples/SFMExample.cpp +++ b/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 >(Symbol('x', 0), poses[0], poseNoise); // add directly to graph // Simulated measurements from each camera pose, adding them to the factor graph diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index e9d02b15c..c5545fc0c 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/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 >(0, poses[0], noise); // Because the structure-from-motion problem has a scale ambiguity, the problem is diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp index da7c5ba9b..075e2a653 100644 --- a/examples/SFMExample_SmartFactorPCG.cpp +++ b/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 >(0, poses[0], noise); // Fix the scale ambiguity by adding a prior diff --git a/examples/SelfCalibrationExample.cpp b/examples/SelfCalibrationExample.cpp index 93e010543..ba097c074 100644 --- a/examples/SelfCalibrationExample.cpp +++ b/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 >(Symbol('x', 0), poses[0], poseNoise); // Simulated measurements from each camera pose, adding them to the factor graph diff --git a/examples/VisualISAM2Example.cpp b/examples/VisualISAM2Example.cpp index 751b776f6..35bc9bcf6 100644 --- a/examples/VisualISAM2Example.cpp +++ b/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 >(Symbol('x', 0), poses[0], kPosePrior); diff --git a/examples/VisualISAMExample.cpp b/examples/VisualISAMExample.cpp index 8ca1be596..7a58deeb7 100644 --- a/examples/VisualISAMExample.cpp +++ b/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 >(Symbol('x', 0), poses[0], poseNoise); // Add a prior on landmark l0 diff --git a/matlab/unstable_examples/+imuSimulator/OdometryExample3D.m b/matlab/unstable_examples/+imuSimulator/OdometryExample3D.m index d28d3c2cb..71ff20ec2 100644 --- a/matlab/unstable_examples/+imuSimulator/OdometryExample3D.m +++ b/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)); diff --git a/tests/testVisualISAM2.cpp b/tests/testVisualISAM2.cpp index 1ced2af23..9c63e1aa8 100644 --- a/tests/testVisualISAM2.cpp +++ b/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>(Symbol('x', 0), poses[0], kPosePrior);