diff --git a/gtsam.h b/gtsam.h index 0aca81d02..014907037 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2524,21 +2524,24 @@ class BetweenFactorPose3s }; #include -namespace initialize_pose3 { -gtsam::Values computeOrientationsChordal( - const gtsam::NonlinearFactorGraph& pose3Graph); -gtsam::Values computeOrientationsGradient( - const gtsam::NonlinearFactorGraph& pose3Graph, - const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame); -gtsam::Values computeOrientationsGradient( - const gtsam::NonlinearFactorGraph& pose3Graph, - const gtsam::Values& givenGuess); -gtsam::NonlinearFactorGraph buildPose3graph( - const gtsam::NonlinearFactorGraph& graph); -gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& givenGuess, bool useGradient); -gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); -} // namespace initialize_pose3 +class InitializePose3 { + static gtsam::Values computeOrientationsChordal( + const gtsam::NonlinearFactorGraph& pose3Graph); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess); + static gtsam::NonlinearFactorGraph buildPose3graph( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initializeOrientations( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& givenGuess, + bool useGradient); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); +}; gtsam::BetweenFactorPose3s parse3DFactors(string filename); pair load3D(string filename); diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index b781f79f0..667ccef0d 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -29,12 +29,11 @@ using namespace std; namespace gtsam { -namespace initialize_pose3 { static const Key kAnchorKey = symbol('Z', 9999999); /* ************************************************************************* */ -GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { +GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const NonlinearFactorGraph& g) { GaussianFactorGraph linearGraph; noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9); @@ -67,7 +66,8 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { /* ************************************************************************* */ // Transform VectorValues into valid Rot3 -Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { +Values InitializePose3::normalizeRelaxedRotations( + const VectorValues& relaxedRot3) { gttic(InitializePose3_computeOrientationsChordal); Matrix ppm = Z_3x3; // plus plus minus @@ -94,7 +94,7 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { } /* ************************************************************************* */ -NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { +NonlinearFactorGraph InitializePose3::buildPose3graph(const NonlinearFactorGraph& graph) { gttic(InitializePose3_buildPose3graph); NonlinearFactorGraph pose3Graph; @@ -115,7 +115,8 @@ NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { } /* ************************************************************************* */ -Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { +Values InitializePose3::computeOrientationsChordal( + const NonlinearFactorGraph& pose3Graph) { gttic(InitializePose3_computeOrientationsChordal); // regularize measurements and plug everything in a factor graph @@ -129,10 +130,9 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { } /* ************************************************************************* */ -Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, - const Values& givenGuess, - const size_t maxIter, - const bool setRefFrame) { +Values InitializePose3::computeOrientationsGradient( + const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, + const size_t maxIter, const bool setRefFrame) { gttic(InitializePose3_computeOrientationsGradient); // this works on the inverse rotations, according to Tron&Vidal,2011 @@ -231,7 +231,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, } /* ************************************************************************* */ -void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, +void InitializePose3::createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph) { size_t factorId = 0; for(const auto& factor: pose3Graph) { @@ -264,7 +264,7 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, } /* ************************************************************************* */ -Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) { +Vector3 InitializePose3::gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) { Vector3 logRot = Rot3::Logmap(R1.between(R2)); double th = logRot.norm(); @@ -286,8 +286,7 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl } /* ************************************************************************* */ -Values initializeOrientations(const NonlinearFactorGraph& graph) { - +Values InitializePose3::initializeOrientations(const NonlinearFactorGraph& 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); @@ -297,7 +296,7 @@ Values initializeOrientations(const NonlinearFactorGraph& graph) { } ///* ************************************************************************* */ -Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { +Values InitializePose3::computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { gttic(InitializePose3_computePoses); // put into Values structure @@ -339,7 +338,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { } /* ************************************************************************* */ -Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, +Values InitializePose3::initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient) { gttic(InitializePose3_initialize); Values initialValues; @@ -360,9 +359,8 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, } /* ************************************************************************* */ -Values initialize(const NonlinearFactorGraph& graph) { +Values InitializePose3::initialize(const NonlinearFactorGraph& graph) { return initialize(graph, Values(), false); } -} // namespace initialize_pose3 } // namespace gtsam diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h index d9bea7932..ce1b28854 100644 --- a/gtsam/slam/InitializePose3.h +++ b/gtsam/slam/InitializePose3.h @@ -31,54 +31,57 @@ namespace gtsam { typedef std::map > KeyVectorMap; typedef std::map KeyRotMap; -namespace initialize_pose3 { +struct GTSAM_EXPORT InitializePose3 { + static GaussianFactorGraph buildLinearOrientationGraph( + const NonlinearFactorGraph& g); -GTSAM_EXPORT GaussianFactorGraph -buildLinearOrientationGraph(const NonlinearFactorGraph& g); + static Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); -GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); + /** + * Return the orientations of a graph including only BetweenFactors + */ + static Values computeOrientationsChordal( + const NonlinearFactorGraph& pose3Graph); -/** - * Return the orientations of a graph including only BetweenFactors - */ -GTSAM_EXPORT Values -computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph); + /** + * Return the orientations of a graph including only BetweenFactors + */ + static Values computeOrientationsGradient( + const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, + size_t maxIter = 10000, const bool setRefFrame = true); -/** - * Return the orientations of a graph including only BetweenFactors - */ -GTSAM_EXPORT Values computeOrientationsGradient( - const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, - size_t maxIter = 10000, const bool setRefFrame = true); + static void createSymbolicGraph(KeyVectorMap& adjEdgesMap, + KeyRotMap& factorId2RotMap, + const NonlinearFactorGraph& pose3Graph); -GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, - KeyRotMap& factorId2RotMap, - const NonlinearFactorGraph& pose3Graph); + static Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, + const double b); -GTSAM_EXPORT Vector3 gradientTron(const Rot3& R1, const Rot3& R2, - const double a, const double b); + /** + * Select the subgraph of betweenFactors and transforms priors into between + * wrt a fictitious node + */ + static NonlinearFactorGraph buildPose3graph( + const NonlinearFactorGraph& graph); -/** - * Select the subgraph of betweenFactors and transforms priors into between wrt - * a fictitious node - */ -GTSAM_EXPORT NonlinearFactorGraph -buildPose3graph(const NonlinearFactorGraph& graph); + static Values computePoses(NonlinearFactorGraph& pose3graph, + Values& initialRot); -GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph, - Values& initialRot); + /** + * "extract" the Pose3 subgraph of the original graph, get orientations from + * relative orientation measurements using chordal method. + */ + static Values initializeOrientations(const NonlinearFactorGraph& graph); -/** - * "extract" the Pose3 subgraph of the original graph, get orientations from - * relative orientation measurements (using either gradient or chordal method), - * and finish up with 1 GN iteration on full poses. - */ -GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, - const Values& givenGuess, - bool useGradient = false); + /** + * "extract" the Pose3 subgraph of the original graph, get orientations from + * relative orientation measurements (using either gradient or chordal + * method), and finish up with 1 GN iteration on full poses. + */ + static Values initialize(const NonlinearFactorGraph& graph, + const Values& givenGuess, bool useGradient = false); -/// Calls initialize above using Chordal method. -GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph); - -} // namespace initialize_pose3 + /// Calls initialize above using Chordal method. + static Values initialize(const NonlinearFactorGraph& graph); +}; } // end of namespace gtsam diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index ea474c480..e6f08286a 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -74,15 +74,15 @@ NonlinearFactorGraph graph() { /* *************************************************************************** */ TEST( InitializePose3, buildPose3graph ) { - NonlinearFactorGraph pose3graph = initialize_pose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3graph = InitializePose3::buildPose3graph(simple::graph()); // pose3graph.print(""); } /* *************************************************************************** */ TEST( InitializePose3, orientations ) { - NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); - Values initial = initialize_pose3::computeOrientationsChordal(pose3Graph); + 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)); @@ -93,12 +93,12 @@ TEST( InitializePose3, orientations ) { /* *************************************************************************** */ TEST( InitializePose3, orientationsGradientSymbolicGraph ) { - NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); KeyVectorMap adjEdgesMap; KeyRotMap factorId2RotMap; - initialize_pose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph); + InitializePose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph); EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[0], 0, 1e-9); EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[1], 3, 1e-9); @@ -132,7 +132,7 @@ TEST( InitializePose3, singleGradient ) { double a = 6.010534238540223; double b = 1.0; - Vector actual = initialize_pose3::gradientTron(R1, R2, a, b); + Vector actual = InitializePose3::gradientTron(R1, R2, a, b); Vector expected = Vector3::Zero(); expected(2) = 1.962658662803917; @@ -142,7 +142,7 @@ TEST( InitializePose3, singleGradient ) { /* *************************************************************************** */ TEST( InitializePose3, iterationGradient ) { - NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); // Wrong initial guess - initialization should fix the rotations Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); @@ -154,7 +154,7 @@ TEST( InitializePose3, iterationGradient ) { size_t maxIter = 1; // test gradient at the first iteration bool setRefFrame = false; - Values orientations = initialize_pose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame); + Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame); Matrix M0 = (Matrix(3,3) << 0.999435813876064, -0.033571481675497, 0.001004768630281, 0.033572116359134, 0.999436104312325, -0.000621610948719, @@ -183,7 +183,7 @@ TEST( InitializePose3, iterationGradient ) { /* *************************************************************************** */ TEST( InitializePose3, orientationsGradient ) { - NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph()); + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); // Wrong initial guess - initialization should fix the rotations Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); @@ -194,7 +194,7 @@ TEST( InitializePose3, orientationsGradient ) { givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) )); // do 10 gradient iterations bool setRefFrame = false; - Values orientations = initialize_pose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame); + Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame); // const Key keyAnchor = symbol('Z', 9999999); // givenPoses.insert(keyAnchor,simple::pose0); @@ -228,7 +228,7 @@ TEST( InitializePose3, posesWithGivenGuess ) { givenPoses.insert(x2,simple::pose2); givenPoses.insert(x3,simple::pose3); - Values initial = initialize_pose3::initialize(simple::graph(), givenPoses); + Values initial = InitializePose3::initialize(simple::graph(), givenPoses); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI EXPECT(assert_equal(givenPoses, initial, 1e-6)); @@ -245,7 +245,7 @@ TEST( InitializePose3, initializePoses ) noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); inputGraph->add(PriorFactor(0, Pose3(), priorModel)); - Values initial = initialize_pose3::initialize(*inputGraph); + Values initial = InitializePose3::initialize(*inputGraph); EXPECT(assert_equal(*expectedValues,initial,0.1)); // TODO(frank): very loose !! }