diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 578b78dd5..38bfcf82f 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -72,7 +72,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { /* ************************************************************************* */ // Transform VectorValues into valid Rot3 Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { - gttic(InitializePose3_computeOrientations); + gttic(InitializePose3_computeOrientationsChordal); Matrix ppm = Matrix::Zero(3,3); // plus plus minus ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1; @@ -131,9 +131,26 @@ NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { } /* ************************************************************************* */ -// Return the orientations of a graph including only BetweenFactors -Values computeOrientations(const NonlinearFactorGraph& pose3Graph) { - gttic(InitializePose3_computeOrientations); +// Return the orientations of a graph including only BetweenFactors +Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { + gttic(InitializePose3_computeOrientationsChordal); + + // regularize measurements and plug everything in a factor graph + GaussianFactorGraph relaxedGraph = buildLinearOrientationGraph(pose3Graph); + + // Solve the LFG + VectorValues relaxedRot3 = relaxedGraph.optimize(); + + // normalize and compute Rot3 + return normalizeRelaxedRotations(relaxedRot3); +} + +/* ************************************************************************* */ +// Return the orientations of a graph including only BetweenFactors +Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess) { + gttic(InitializePose3_computeOrientationsGradient); + + // regularize measurements and plug everything in a factor graph GaussianFactorGraph relaxedGraph = buildLinearOrientationGraph(pose3Graph); @@ -153,7 +170,7 @@ Values initializeOrientations(const NonlinearFactorGraph& graph) { NonlinearFactorGraph pose3Graph = buildPose3graph(graph); // Get orientations from relative orientation measurements - return computeOrientations(pose3Graph); + return computeOrientationsChordal(pose3Graph); } ///* ************************************************************************* */ @@ -200,18 +217,26 @@ Values initialize(const NonlinearFactorGraph& graph) { NonlinearFactorGraph pose3Graph = buildPose3graph(graph); // Get orientations from relative orientation measurements - Values valueRot3 = computeOrientations(pose3Graph); + 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) { +Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient) { Values initialValues; - // get the orientation estimates from LAGO - Values orientations = initializeOrientations(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); + + // Get orientations from relative orientation measurements + Values orientations; + if(useGradient) + orientations = computeOrientationsGradient(pose3Graph, givenGuess); + else + orientations = computeOrientationsChordal(pose3Graph); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, orientations) { Key key = key_value.key; diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h index 4611cf7ee..7b5489353 100644 --- a/gtsam/slam/InitializePose3.h +++ b/gtsam/slam/InitializePose3.h @@ -32,9 +32,7 @@ GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFact GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); -GTSAM_EXPORT Values computeOrientations(const NonlinearFactorGraph& pose3Graph); - -GTSAM_EXPORT Values initializeOrientations(const NonlinearFactorGraph& graph); +GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph); GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph); @@ -42,7 +40,7 @@ GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph, Values& init GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph); -GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess); +GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient = false); } // end of namespace lago } // end of namespace gtsam diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index 355fb1238..5387af7ef 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -80,7 +80,9 @@ TEST( InitializePose3, buildPose3graph ) { /* *************************************************************************** */ TEST( InitializePose3, orientations ) { - Values initial = InitializePose3::initializeOrientations(simple::graph()); + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); + + 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));