diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 667ccef0d..2f247811d 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/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 >(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; } diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index e6f08286a..6fe8b3d7c 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -70,6 +70,17 @@ NonlinearFactorGraph graph() { g.add(PriorFactor(x0, pose0, model)); return g; } + +NonlinearFactorGraph graph2() { + NonlinearFactorGraph g; + g.add(BetweenFactor(x0, x1, pose0.between(pose1), noiseModel::Isotropic::Precision(6, 1.0))); + g.add(BetweenFactor(x1, x2, pose1.between(pose2), noiseModel::Isotropic::Precision(6, 1.0))); + g.add(BetweenFactor(x2, x3, pose2.between(pose3), noiseModel::Isotropic::Precision(6, 1.0))); + g.add(BetweenFactor(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(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(x0, pose0, model)); + return g; +} } /* *************************************************************************** */ @@ -91,6 +102,19 @@ TEST( InitializePose3, orientations ) { EXPECT(assert_equal(simple::R3, initial.at(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(x0), 1e-6)); + EXPECT(assert_equal(simple::R1, initial.at(x1), 1e-6)); + EXPECT(assert_equal(simple::R2, initial.at(x2), 1e-6)); + EXPECT(assert_equal(simple::R3, initial.at(x3), 1e-6)); +} + /* *************************************************************************** */ TEST( InitializePose3, orientationsGradientSymbolicGraph ) { NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());