From 2f2a40a737eac17d73eac733ee6eaed660ec6e1e Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 17 Aug 2014 20:02:46 -0400 Subject: [PATCH 01/19] initial implementation of chordal relaxation --- gtsam/slam/InitializePose3.cpp | 237 +++++++++++++++++++++++ gtsam/slam/InitializePose3.h | 60 ++++++ gtsam/slam/tests/testInitializePose3.cpp | 96 +++++++++ 3 files changed, 393 insertions(+) create mode 100644 gtsam/slam/InitializePose3.cpp create mode 100644 gtsam/slam/InitializePose3.h create mode 100644 gtsam/slam/tests/testInitializePose3.cpp diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp new file mode 100644 index 000000000..d4f93293c --- /dev/null +++ b/gtsam/slam/InitializePose3.cpp @@ -0,0 +1,237 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file InitializePose3.h + * @author Luca Carlone + * @author Frank Dellaert + * @date August, 2014 + */ + +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; + +namespace gtsam { +namespace InitializePose3 { + +//static const Matrix I = eye(1); +static const Matrix I9 = eye(9); +static const Vector zero9 = Vector::Zero(9); +static const Vector zero33= Matrix::Zero(3,3); + +static const Key keyAnchor = symbol('Z', 9999999); + +/* ************************************************************************* */ +GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { + + GaussianFactorGraph linearGraph; + noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9); + + BOOST_FOREACH(const boost::shared_ptr& factor, g) { + Matrix3 Rijt; + + boost::shared_ptr > pose3Between = + boost::dynamic_pointer_cast >(factor); + if (pose3Between) + Rijt = pose3Between->measured().rotation().matrix().transpose(); + else + std::cout << "Error in buildLinearOrientationGraph" << std::endl; + + const FastVector& keys = factor->keys(); + Key key1 = keys[0], key2 = keys[1]; + Matrix M9 = (Matrix(9,9) << Rijt, zero33, zero33, + zero33, Rijt, zero33, + zero33, zero33, Rijt); + linearGraph.add(key1, -I9, key2, M9, zero9, model); + } + // prior on the anchor orientation + linearGraph.add(keyAnchor, I9, (Vector(1) << 1.0, 0.0, 0.0,/* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0), model); + return linearGraph; +} + +/* ************************************************************************* */ +// Transform VectorValues into valid Rot3 +Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { + gttic(InitializePose3_computeOrientations); + + Values validRot3; + + return validRot3; +} + +/* ************************************************************************* */ +// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node +NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { + gttic(InitializePose3_buildPose3graph); + NonlinearFactorGraph pose3Graph; + + BOOST_FOREACH(const boost::shared_ptr& factor, graph) { + + // recast to a between on Pose3 + boost::shared_ptr > pose3Between = + boost::dynamic_pointer_cast >(factor); + if (pose3Between) + pose3Graph.add(pose3Between); + + // recast PriorFactor to BetweenFactor + boost::shared_ptr > pose3Prior = + boost::dynamic_pointer_cast >(factor); + if (pose3Prior) + pose3Graph.add( + BetweenFactor(keyAnchor, pose3Prior->keys()[0], + pose3Prior->prior(), pose3Prior->get_noiseModel())); + } + return pose3Graph; +} + +/* ************************************************************************* */ +// Return the orientations of a graph including only BetweenFactors +Values computeOrientations(const NonlinearFactorGraph& pose3Graph) { + gttic(InitializePose3_computeOrientations); + + // regularize measurements and plug everything in a factor graph + GaussianFactorGraph relaxedGraph = buildLinearOrientationGraph(pose3Graph); + + // Solve the LFG + VectorValues relaxedRot3 = relaxedGraph.optimize(); + + // normalize and compute Rot3 + Values initializedRot3 = normalizeRelaxedRotations(relaxedRot3); + + return initializedRot3; +} + +/* ************************************************************************* */ +Values 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); + + // Get orientations from relative orientation measurements + return computeOrientations(pose3Graph); +} + +///* ************************************************************************* */ +//Values computePoses(const NonlinearFactorGraph& pose2graph, +// VectorValues& orientationsLago) { +// gttic(lago_computePoses); +// +// // Linearized graph on full poses +// GaussianFactorGraph linearPose2graph; +// +// // We include the linear version of each between factor +// BOOST_FOREACH(const boost::shared_ptr& factor, pose2graph) { +// +// boost::shared_ptr > pose2Between = +// boost::dynamic_pointer_cast >(factor); +// +// if (pose2Between) { +// Key key1 = pose2Between->keys()[0]; +// double theta1 = orientationsLago.at(key1)(0); +// double s1 = sin(theta1); +// double c1 = cos(theta1); +// +// Key key2 = pose2Between->keys()[1]; +// double theta2 = orientationsLago.at(key2)(0); +// +// double linearDeltaRot = theta2 - theta1 +// - pose2Between->measured().theta(); +// linearDeltaRot = Rot2(linearDeltaRot).theta(); // to normalize +// +// double dx = pose2Between->measured().x(); +// double dy = pose2Between->measured().y(); +// +// Vector globalDeltaCart = // +// (Vector(2) << c1 * dx - s1 * dy, s1 * dx + c1 * dy); +// Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot); // rhs +// Matrix J1 = -I3; +// J1(0, 2) = s1 * dx + c1 * dy; +// J1(1, 2) = -c1 * dx + s1 * dy; +// // Retrieve the noise model for the relative rotation +// boost::shared_ptr diagonalModel = +// boost::dynamic_pointer_cast( +// pose2Between->get_noiseModel()); +// +// linearPose2graph.add(key1, J1, key2, I3, b, diagonalModel); +// } else { +// throw invalid_argument( +// "computeLagoPoses: cannot manage non between factor here!"); +// } +// } +// // add prior +// linearPose2graph.add(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0), +// priorPose2Noise); +// +// // optimize +// VectorValues posesLago = linearPose2graph.optimize(); +// +// // put into Values structure +// Values initialGuessLago; +// BOOST_FOREACH(const VectorValues::value_type& it, posesLago) { +// Key key = it.first; +// if (key != keyAnchor) { +// const Vector& poseVector = it.second; +// Pose2 poseLago = Pose2(poseVector(0), poseVector(1), +// orientationsLago.at(key)(0) + poseVector(2)); +// initialGuessLago.insert(key, poseLago); +// } +// } +// return initialGuessLago; +//} + +///* ************************************************************************* */ +//Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) { +// gttic(lago_initialize); +// +// // We "extract" the Pose2 subgraph of the original graph: this +// // is done to properly model priors and avoiding operating on a larger graph +// NonlinearFactorGraph pose2Graph = buildPose2graph(graph); +// +// // Get orientations from relative orientation measurements +// VectorValues orientationsLago = computeOrientations(pose2Graph, +// useOdometricPath); +// +// // Compute the full poses +// return computePoses(pose2Graph, orientationsLago); +//} +// +///* ************************************************************************* */ +//Values initialize(const NonlinearFactorGraph& graph, +// const Values& initialGuess) { +// Values initialGuessLago; +// +// // get the orientation estimates from LAGO +// VectorValues orientations = initializeOrientations(graph); +// +// // for all nodes in the tree +// BOOST_FOREACH(const VectorValues::value_type& it, orientations) { +// Key key = it.first; +// if (key != keyAnchor) { +// const Pose2& pose = initialGuess.at(key); +// const Vector& orientation = it.second; +// Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0)); +// initialGuessLago.insert(key, poseLago); +// } +// } +// return initialGuessLago; +//} + +} // end of namespace lago +} // end of namespace gtsam diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h new file mode 100644 index 000000000..33efe3f85 --- /dev/null +++ b/gtsam/slam/InitializePose3.h @@ -0,0 +1,60 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file InitializePose3.h + * @brief Initialize Pose3 in a factor graph + * + * @author Luca Carlone + * @author Frank Dellaert + * @date August, 2014 + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { +namespace InitializePose3 { + +GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g); + +GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); + +GTSAM_EXPORT Values computeOrientations(const NonlinearFactorGraph& pose3Graph); + +GTSAM_EXPORT Values initializeOrientations(const NonlinearFactorGraph& graph); + +GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph); + +///** Linear factor graph with regularized orientation measurements */ +//GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph( +// const std::vector& spanningTreeIds, +// const std::vector& chordsIds, const NonlinearFactorGraph& g, +// const key2doubleMap& orientationsToRoot, const PredecessorMap& tree); +// +///** LAGO: Return the orientations of the Pose2 in a generic factor graph */ +//GTSAM_EXPORT VectorValues initializeOrientations( +// const NonlinearFactorGraph& graph, bool useOdometricPath = true); +// +///** Return the values for the Pose2 in a generic factor graph */ +//GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, +// bool useOdometricPath = true); +// +///** Only correct the orientation part in initialGuess */ +//GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, +// const Values& initialGuess); + +} // end of namespace lago +} // end of namespace gtsam diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp new file mode 100644 index 000000000..4cd374b64 --- /dev/null +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -0,0 +1,96 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testInitializePose3.cpp + * @brief Unit tests for 3D SLAM initialization, using rotation relaxation + * + * @author Luca Carlone + * @author Frank Dellaert + * @date August, 2014 + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +static Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3); +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(6, 0.1)); + +namespace simple { +// We consider a small graph: +// symbolic FG +// x2 0 1 +// / | \ 1 2 +// / | \ 2 3 +// x3 | x1 2 0 +// \ | / 0 3 +// \ | / +// x0 +// +static Point3 p1 = Point3(0,0,0); +static Rot3 R1 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,0.0 ) ); +static Point3 p2 = Point3(1,2,0); +static Rot3 R2 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,1.570796 ) ); +static Point3 p3 = Point3(0,2,0); +static Rot3 R3 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,3.141593 ) ); +static Point3 p4 = Point3(-1,1,0); +static Rot3 R4 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,4.712389 ) ); + +static Pose3 pose0 = Pose3(R1,p1); +static Pose3 pose1 = Pose3(R2,p2); +static Pose3 pose2 = Pose3(R3,p3); +static Pose3 pose3 = Pose3(R4,p4); + +NonlinearFactorGraph graph() { + NonlinearFactorGraph g; + g.add(BetweenFactor(x0, x1, pose0.between(pose1), model)); + g.add(BetweenFactor(x1, x2, pose1.between(pose2), model)); + g.add(BetweenFactor(x2, x3, pose2.between(pose3), model)); + g.add(BetweenFactor(x2, x0, pose2.between(pose0), model)); + g.add(BetweenFactor(x0, x3, pose0.between(pose3), model)); + g.add(PriorFactor(x0, pose0, model)); + return g; +} +} + +/* *************************************************************************** */ +TEST( InitializePose3, buildPose3graph ) { + NonlinearFactorGraph pose3graph = InitializePose3::buildPose3graph(simple::graph()); + // pose3graph.print(""); +} + +/* *************************************************************************** */ +TEST( InitializePose3, orientations ) { + Values initial = InitializePose3::initializeOrientations(simple::graph()); + + // comparison is up to M_PI, that's why we add some multiples of 2*M_PI + EXPECT(assert_equal(initial.at(0), simple::pose0, 1e-6)); +} + + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + From e031ba036d09f6ec75305b49ffc2cd4eb1ecf83b Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 17 Aug 2014 20:24:53 -0400 Subject: [PATCH 02/19] first working version with (unnormalized) rotations only --- .cproject | 138 +++++++++++++---------- gtsam/slam/InitializePose3.cpp | 32 ++++-- gtsam/slam/tests/testInitializePose3.cpp | 29 ++--- 3 files changed, 115 insertions(+), 84 deletions(-) diff --git a/.cproject b/.cproject index 21ac9d913..2846f289c 100644 --- a/.cproject +++ b/.cproject @@ -1,19 +1,17 @@ - - - + - - + + @@ -62,13 +60,13 @@ - - + + @@ -118,13 +116,13 @@ - - + + @@ -584,6 +582,7 @@ make + tests/testBayesTree.run true false @@ -591,6 +590,7 @@ make + testBinaryBayesNet.run true false @@ -638,6 +638,7 @@ make + testSymbolicBayesNet.run true false @@ -645,6 +646,7 @@ make + tests/testSymbolicFactor.run true false @@ -652,6 +654,7 @@ make + testSymbolicFactorGraph.run true false @@ -667,6 +670,7 @@ make + tests/testBayesTree true false @@ -1002,6 +1006,7 @@ make + testErrors.run true false @@ -1231,6 +1236,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1313,7 +1358,6 @@ make - testSimulated2DOriented.run true false @@ -1353,7 +1397,6 @@ make - testSimulated2D.run true false @@ -1361,7 +1404,6 @@ make - testSimulated3D.run true false @@ -1375,46 +1417,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1672,6 +1674,7 @@ cpack + -G DEB true false @@ -1679,6 +1682,7 @@ cpack + -G RPM true false @@ -1686,6 +1690,7 @@ cpack + -G TGZ true false @@ -1693,6 +1698,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2419,6 +2425,7 @@ make + testGraph.run true false @@ -2426,6 +2433,7 @@ make + testJunctionTree.run true false @@ -2433,6 +2441,7 @@ make + testSymbolicBayesNetB.run true false @@ -2622,6 +2631,22 @@ true true + + make + -j5 + testInitializePose3.run + true + true + true + + + make + -j5 + testLago.run + true + true + true + make -j2 @@ -2798,14 +2823,6 @@ true true - - make - -j5 - testLago.run - true - true - true - make -j5 @@ -2896,7 +2913,6 @@ make - tests/testGaussianISAM2 true false diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index d4f93293c..232733273 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -44,24 +44,27 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9); BOOST_FOREACH(const boost::shared_ptr& factor, g) { - Matrix3 Rijt; + Matrix3 Rij; boost::shared_ptr > pose3Between = boost::dynamic_pointer_cast >(factor); if (pose3Between) - Rijt = pose3Between->measured().rotation().matrix().transpose(); + Rij = pose3Between->measured().rotation().matrix(); else std::cout << "Error in buildLinearOrientationGraph" << std::endl; + // std::cout << "Rij \n" << Rij << std::endl; + const FastVector& keys = factor->keys(); Key key1 = keys[0], key2 = keys[1]; - Matrix M9 = (Matrix(9,9) << Rijt, zero33, zero33, - zero33, Rijt, zero33, - zero33, zero33, Rijt); + Matrix M9 = Matrix::Zero(9,9); + M9.block(0,0,3,3) = Rij; + M9.block(3,3,3,3) = Rij; + M9.block(6,6,3,3) = Rij; linearGraph.add(key1, -I9, key2, M9, zero9, model); } // prior on the anchor orientation - linearGraph.add(keyAnchor, I9, (Vector(1) << 1.0, 0.0, 0.0,/* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0), model); + linearGraph.add(keyAnchor, I9, (Vector(9) << 1.0, 0.0, 0.0,/* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0), model); return linearGraph; } @@ -71,7 +74,18 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { gttic(InitializePose3_computeOrientations); Values validRot3; - + BOOST_FOREACH(const VectorValues::value_type& it, relaxedRot3) { + Key key = it.first; + if (key != keyAnchor) { + const Vector& rotVector = it.second; + Matrix3 rotMat; + rotMat(0,0) = rotVector(0); rotMat(0,1) = rotVector(1); rotMat(0,2) = rotVector(2); + rotMat(1,0) = rotVector(3); rotMat(1,1) = rotVector(4); rotMat(1,2) = rotVector(5); + rotMat(2,0) = rotVector(6); rotMat(2,1) = rotVector(7); rotMat(2,2) = rotVector(8); + Rot3 initRot = Rot3(rotMat); + validRot3.insert(key, initRot); + } + } return validRot3; } @@ -112,9 +126,7 @@ Values computeOrientations(const NonlinearFactorGraph& pose3Graph) { VectorValues relaxedRot3 = relaxedGraph.optimize(); // normalize and compute Rot3 - Values initializedRot3 = normalizeRelaxedRotations(relaxedRot3); - - return initializedRot3; + return normalizeRelaxedRotations(relaxedRot3); } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index 4cd374b64..6800cc66c 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -46,19 +46,19 @@ namespace simple { // \ | / // x0 // -static Point3 p1 = Point3(0,0,0); -static Rot3 R1 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,0.0 ) ); -static Point3 p2 = Point3(1,2,0); -static Rot3 R2 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,1.570796 ) ); -static Point3 p3 = Point3(0,2,0); -static Rot3 R3 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,3.141593 ) ); -static Point3 p4 = Point3(-1,1,0); -static Rot3 R4 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,4.712389 ) ); +static Point3 p0 = Point3(0,0,0); +static Rot3 R0 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,0.0 ) ); +static Point3 p1 = Point3(1,2,0); +static Rot3 R1 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,1.570796 ) ); +static Point3 p2 = Point3(0,2,0); +static Rot3 R2 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,3.141593 ) ); +static Point3 p3 = Point3(-1,1,0); +static Rot3 R3 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,4.712389 ) ); -static Pose3 pose0 = Pose3(R1,p1); -static Pose3 pose1 = Pose3(R2,p2); -static Pose3 pose2 = Pose3(R3,p3); -static Pose3 pose3 = Pose3(R4,p4); +static Pose3 pose0 = Pose3(R0,p0); +static Pose3 pose1 = Pose3(R1,p1); +static Pose3 pose2 = Pose3(R2,p2); +static Pose3 pose3 = Pose3(R3,p3); NonlinearFactorGraph graph() { NonlinearFactorGraph g; @@ -83,7 +83,10 @@ TEST( InitializePose3, orientations ) { Values initial = InitializePose3::initializeOrientations(simple::graph()); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI - EXPECT(assert_equal(initial.at(0), simple::pose0, 1e-6)); + 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)); } From 4cd4023ef874e7b3ee610141def351a9dc4fe284 Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 17 Aug 2014 20:34:43 -0400 Subject: [PATCH 03/19] included normalization --- gtsam/slam/InitializePose3.cpp | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 232733273..37b33204b 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -33,7 +33,7 @@ namespace InitializePose3 { //static const Matrix I = eye(1); static const Matrix I9 = eye(9); static const Vector zero9 = Vector::Zero(9); -static const Vector zero33= Matrix::Zero(3,3); +static const Matrix zero33= Matrix::Zero(3,3); static const Key keyAnchor = symbol('Z', 9999999); @@ -73,6 +73,9 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { gttic(InitializePose3_computeOrientations); + Matrix ppm = Matrix::Zero(3,3); // plus plus minus + ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1; + Values validRot3; BOOST_FOREACH(const VectorValues::value_type& it, relaxedRot3) { Key key = it.first; @@ -82,7 +85,19 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { rotMat(0,0) = rotVector(0); rotMat(0,1) = rotVector(1); rotMat(0,2) = rotVector(2); rotMat(1,0) = rotVector(3); rotMat(1,1) = rotVector(4); rotMat(1,2) = rotVector(5); rotMat(2,0) = rotVector(6); rotMat(2,1) = rotVector(7); rotMat(2,2) = rotVector(8); - Rot3 initRot = Rot3(rotMat); + + Matrix U, V; Vector s; + svd(rotMat, U, s, V); + Matrix3 normalizedRotMat = U * V.transpose(); + + // std::cout << "rotMat \n" << rotMat << std::endl; + // std::cout << "U V' \n" << U * V.transpose() << std::endl; + // std::cout << "V \n" << V << std::endl; + + if(normalizedRotMat.determinant() < 0) + normalizedRotMat = U * ppm * V.transpose(); + + Rot3 initRot = Rot3(normalizedRotMat); validRot3.insert(key, initRot); } } From c0f880d52b61d711af21dc722a9c147214500e32 Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 17 Aug 2014 20:50:18 -0400 Subject: [PATCH 04/19] working version with pose --- gtsam/slam/InitializePose3.cpp | 39 ++++++++++++------------ gtsam/slam/InitializePose3.h | 2 ++ gtsam/slam/tests/testInitializePose3.cpp | 14 +++++++++ 3 files changed, 35 insertions(+), 20 deletions(-) diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 37b33204b..4ae9b1d36 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -239,26 +239,25 @@ Values initializeOrientations(const NonlinearFactorGraph& graph) { // return computePoses(pose2Graph, orientationsLago); //} // -///* ************************************************************************* */ -//Values initialize(const NonlinearFactorGraph& graph, -// const Values& initialGuess) { -// Values initialGuessLago; -// -// // get the orientation estimates from LAGO -// VectorValues orientations = initializeOrientations(graph); -// -// // for all nodes in the tree -// BOOST_FOREACH(const VectorValues::value_type& it, orientations) { -// Key key = it.first; -// if (key != keyAnchor) { -// const Pose2& pose = initialGuess.at(key); -// const Vector& orientation = it.second; -// Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0)); -// initialGuessLago.insert(key, poseLago); -// } -// } -// return initialGuessLago; -//} +/* ************************************************************************* */ +Values initialize(const NonlinearFactorGraph& graph, + const Values& givenGuess) { + Values initialValues; + + // get the orientation estimates from LAGO + Values orientations = initializeOrientations(graph); + + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, orientations) { + Key key = key_value.key; + if (key != keyAnchor) { + const Point3& pos = givenGuess.at(key).translation(); + const Rot3& rot = orientations.at(key); + Pose3 initializedPoses = Pose3(rot, pos); + initialValues.insert(key, initializedPoses); + } + } + return initialValues; +} } // end of namespace lago } // end of namespace gtsam diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h index 33efe3f85..04a10659c 100644 --- a/gtsam/slam/InitializePose3.h +++ b/gtsam/slam/InitializePose3.h @@ -38,6 +38,8 @@ GTSAM_EXPORT Values initializeOrientations(const NonlinearFactorGraph& graph); GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph); +GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess); + ///** Linear factor graph with regularized orientation measurements */ //GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph( // const std::vector& spanningTreeIds, diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index 6800cc66c..77cfe8e12 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -89,6 +89,20 @@ TEST( InitializePose3, orientations ) { EXPECT(assert_equal(simple::R3, initial.at(x3), 1e-6)); } +/* *************************************************************************** */ +TEST( InitializePose3, posesWithGivenGuess ) { + Values givenPoses; + givenPoses.insert(x0,simple::pose0); + givenPoses.insert(x1,simple::pose1); + givenPoses.insert(x2,simple::pose2); + givenPoses.insert(x3,simple::pose3); + + 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)); +} + /* ************************************************************************* */ int main() { From f5cc247b1ce5fd429d06bff304e56c75c3066680 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 19 Aug 2014 20:50:48 -0400 Subject: [PATCH 05/19] added pose initializer --- .cproject | 122 +++++++++++--------------- examples/Data/pose3exampleGrid.g2o | 77 +++++++++++++++++ gtsam/slam/InitializePose3.cpp | 133 +++++++++++------------------ gtsam/slam/InitializePose3.h | 22 +---- 4 files changed, 182 insertions(+), 172 deletions(-) create mode 100644 examples/Data/pose3exampleGrid.g2o diff --git a/.cproject b/.cproject index 2846f289c..23ec7bdc0 100644 --- a/.cproject +++ b/.cproject @@ -582,7 +582,6 @@ make - tests/testBayesTree.run true false @@ -590,7 +589,6 @@ make - testBinaryBayesNet.run true false @@ -638,7 +636,6 @@ make - testSymbolicBayesNet.run true false @@ -646,7 +643,6 @@ make - tests/testSymbolicFactor.run true false @@ -654,7 +650,6 @@ make - testSymbolicFactorGraph.run true false @@ -670,7 +665,6 @@ make - tests/testBayesTree true false @@ -1006,7 +1000,6 @@ make - testErrors.run true false @@ -1236,46 +1229,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j2 @@ -1358,6 +1311,7 @@ make + testSimulated2DOriented.run true false @@ -1397,6 +1351,7 @@ make + testSimulated2D.run true false @@ -1404,6 +1359,7 @@ make + testSimulated3D.run true false @@ -1417,6 +1373,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j5 @@ -1674,7 +1670,6 @@ cpack - -G DEB true false @@ -1682,7 +1677,6 @@ cpack - -G RPM true false @@ -1690,7 +1684,6 @@ cpack - -G TGZ true false @@ -1698,7 +1691,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2425,7 +2417,6 @@ make - testGraph.run true false @@ -2433,7 +2424,6 @@ make - testJunctionTree.run true false @@ -2441,7 +2431,6 @@ make - testSymbolicBayesNetB.run true false @@ -2631,22 +2620,6 @@ true true - - make - -j5 - testInitializePose3.run - true - true - true - - - make - -j5 - testLago.run - true - true - true - make -j2 @@ -2823,6 +2796,14 @@ true true + + make + -j5 + testLago.run + true + true + true + make -j5 @@ -2913,6 +2894,7 @@ make + tests/testGaussianISAM2 true false diff --git a/examples/Data/pose3exampleGrid.g2o b/examples/Data/pose3exampleGrid.g2o new file mode 100644 index 000000000..c93ee9c19 --- /dev/null +++ b/examples/Data/pose3exampleGrid.g2o @@ -0,0 +1,77 @@ +VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 +VERTEX_SE3:QUAT 1 0.968478 -0.015273 -0.024009 -0.568468 -0.224707 0.094925 0.785710 +VERTEX_SE3:QUAT 2 1.991529 -0.016938 -0.045515 -0.542127 -0.179147 -0.494163 0.655597 +VERTEX_SE3:QUAT 3 2.026568 0.958559 -0.048399 0.363597 0.453322 -0.812746 0.041715 +VERTEX_SE3:QUAT 4 0.987528 0.968615 -0.085312 -0.082238 0.306441 0.936656 0.148344 +VERTEX_SE3:QUAT 5 -0.031537 0.995686 -0.066640 0.335085 0.235204 -0.686500 0.600929 +VERTEX_SE3:QUAT 6 -0.002507 1.983505 -0.158449 -0.401363 0.515966 0.711353 0.258194 +VERTEX_SE3:QUAT 7 1.018807 1.972634 -0.131017 0.323170 -0.282056 0.470606 0.771062 +VERTEX_SE3:QUAT 8 1.994170 1.909926 -0.104255 -0.137653 -0.266858 -0.928006 0.220551 +VERTEX_SE3:QUAT 9 1.910394 1.961603 0.916595 -0.028577 -0.021584 -0.026441 0.999009 +VERTEX_SE3:QUAT 10 0.933629 2.024306 0.857267 -0.303946 -0.378940 -0.856390 0.174981 +VERTEX_SE3:QUAT 11 -0.061508 2.088874 0.841008 0.562526 -0.203739 0.259658 0.758045 +VERTEX_SE3:QUAT 12 -0.106592 1.071471 0.945747 -0.397133 0.277162 -0.755756 0.440794 +VERTEX_SE3:QUAT 13 0.897260 1.009421 1.068154 -0.534411 0.418094 -0.189137 0.709809 +VERTEX_SE3:QUAT 14 1.907694 0.966386 1.128843 0.576449 -0.149518 0.310096 0.741074 +VERTEX_SE3:QUAT 15 1.827009 -0.065936 1.212580 0.440256 -0.304111 0.217991 0.816193 +VERTEX_SE3:QUAT 16 0.811179 -0.009700 1.175299 0.228574 0.021414 0.687899 0.688543 +VERTEX_SE3:QUAT 17 -0.207493 0.046063 1.198636 0.083786 0.149500 0.278888 0.944908 +VERTEX_SE3:QUAT 18 -0.239598 0.069543 2.175883 -0.020534 -0.002367 -0.032999 0.999242 +VERTEX_SE3:QUAT 19 0.795282 0.024403 2.172337 -0.059128 -0.663282 -0.736161 0.120951 +VERTEX_SE3:QUAT 20 1.795463 -0.039545 2.189997 0.262525 0.561623 0.108661 0.777080 +VERTEX_SE3:QUAT 21 1.838289 0.918009 2.142889 0.391707 0.271811 0.534284 0.698015 +VERTEX_SE3:QUAT 22 0.843244 0.956966 2.182675 0.175401 -0.294535 0.796879 0.497462 +VERTEX_SE3:QUAT 23 -0.184445 0.969193 2.212233 -0.022667 -0.309094 0.188130 0.931963 +VERTEX_SE3:QUAT 24 -0.188460 1.988074 2.109480 0.280735 -0.333968 -0.767404 0.469834 +VERTEX_SE3:QUAT 25 0.832834 1.987249 2.119934 0.409102 -0.211526 0.520233 0.719201 +VERTEX_SE3:QUAT 26 1.842345 1.933435 2.082102 0.498150 -0.149112 0.556190 0.648278 +EDGE_SE3:QUAT 0 1 0.968478 -0.015273 -0.024009 -0.568468 -0.224707 0.094925 0.785710 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 1 2 0.895353 0.128330 -0.478510 -0.181316 0.338937 -0.430522 0.816638 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 2 3 -0.429131 -0.043280 0.875673 -0.108628 0.924958 -0.331599 0.150650 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 3 4 0.786542 -0.392786 0.555103 0.731034 -0.219263 -0.010938 0.646060 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 4 5 0.962014 0.324025 0.095456 -0.529804 0.406661 0.542675 0.509350 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 5 6 -0.592671 -0.143889 -0.783004 0.849231 -0.212159 -0.337427 0.346321 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 6 7 -0.578557 -0.780086 -0.317320 -0.050541 -0.889438 -0.373451 0.258610 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 7 8 0.373848 -0.901266 -0.062758 0.564749 0.378680 0.694277 0.235883 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 8 9 0.436073 0.363895 0.853958 0.144187 0.238953 0.925910 0.254564 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 9 10 -0.980372 0.013025 -0.014878 -0.307108 -0.358351 -0.855183 0.214316 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 10 11 0.735233 -0.578374 -0.345900 -0.601711 -0.654423 -0.419528 0.183497 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 11 12 -0.139622 -0.130002 1.005841 0.631012 0.022105 0.762353 0.141962 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 12 13 -0.198544 0.382112 0.917128 -0.217232 -0.341211 0.470993 0.783933 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 13 14 0.591922 -0.251242 0.782954 0.703837 -0.472659 0.521380 0.096797 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 14 15 -0.309581 -0.032947 0.991103 -0.205941 -0.114195 0.017928 0.971713 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 15 16 -0.752100 0.640733 0.245499 0.097291 0.479897 0.332422 0.806058 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 16 17 0.006363 0.960206 -0.345414 -0.061422 0.088813 -0.490352 0.864809 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 17 18 -0.243141 0.271842 0.907511 -0.098852 -0.148661 -0.312729 0.932914 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 18 19 1.035565 0.023491 -0.008895 -0.036454 -0.649327 -0.745091 0.147935 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 19 20 -0.953182 0.279364 -0.134773 0.263672 -0.770187 -0.444277 0.374040 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 20 21 0.497302 0.783380 -0.244889 -0.149390 -0.083102 0.487968 0.855958 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 21 22 -0.241016 0.568368 -0.782358 -0.446393 -0.122376 0.453495 0.761643 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 22 23 0.481169 0.908199 0.028604 -0.365642 0.171794 -0.588182 0.700596 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 23 24 0.310272 0.963355 -0.156115 -0.027745 -0.201443 -0.897925 0.390360 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 24 25 -0.409898 0.553347 -0.754283 0.326372 0.600803 0.719096 0.124169 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 25 26 0.314171 -0.950746 0.144281 0.133133 -0.001730 0.018387 0.990926 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 0 5 0.003921 0.989262 -0.008668 0.344972 0.271052 -0.662312 0.607345 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 0 17 -0.004748 -0.016661 1.014253 0.095179 0.169387 0.315461 0.928834 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 1 4 0.413069 0.396669 0.824709 0.266584 -0.252890 0.915410 0.164328 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 3 8 0.265427 -0.541821 -0.818705 0.520335 -0.589688 0.189278 0.587956 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 3 14 -0.593398 -0.729533 0.348548 0.279515 -0.277200 -0.916882 0.066028 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 6 11 -0.866710 0.505129 0.215695 0.172623 -0.951867 -0.249430 0.044004 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 7 10 0.743464 0.292393 0.573155 0.712782 0.375290 0.526276 0.272273 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 9 14 -0.011444 -1.026550 0.016772 0.610325 -0.121904 0.314891 0.716580 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 9 26 -0.033212 -0.029706 0.975351 0.510053 -0.198209 0.557527 0.624278 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 10 25 0.655684 0.485916 0.598187 -0.700078 -0.428078 -0.464953 0.332353 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 12 17 0.930615 0.369372 0.051025 0.230774 -0.278742 0.911677 0.194659 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 12 23 0.306720 -0.815553 0.415865 0.581204 -0.505705 0.604084 0.203831 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 13 4 0.474778 0.869505 -0.090395 -0.410825 -0.369465 0.822956 0.132141 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 13 16 0.632289 -0.415884 -0.668153 0.221431 -0.613167 0.709932 0.266441 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 13 22 -0.442996 -0.863878 0.046061 -0.079225 0.840832 -0.535376 0.009883 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 15 20 0.632455 0.661923 0.364197 0.030730 0.661379 -0.405142 0.630470 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 16 1 -0.307083 -0.356121 -0.862632 -0.732371 0.242194 -0.446749 0.453200 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 16 19 0.347792 0.365913 0.878300 0.501749 0.573409 0.434965 0.479850 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 18 23 0.027959 1.027767 -0.055691 0.034139 -0.330077 0.166677 0.928495 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 19 22 -0.173539 -0.125423 0.957399 -0.799298 -0.382195 -0.336755 0.318821 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 21 14 -0.104907 -0.825889 -0.563170 -0.004285 -0.468249 0.018532 0.883392 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 21 26 0.956031 0.050291 -0.309699 -0.152297 -0.325772 0.225738 0.905384 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 24 11 0.143525 -0.745793 -0.608698 0.263093 0.680516 0.559568 0.393147 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 +EDGE_SE3:QUAT 25 22 -0.512781 -0.078741 0.857967 -0.053138 0.120352 0.410455 0.902341 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 4ae9b1d36..bed24fb1e 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -156,92 +157,56 @@ Values initializeOrientations(const NonlinearFactorGraph& graph) { } ///* ************************************************************************* */ -//Values computePoses(const NonlinearFactorGraph& pose2graph, -// VectorValues& orientationsLago) { -// gttic(lago_computePoses); -// -// // Linearized graph on full poses -// GaussianFactorGraph linearPose2graph; -// -// // We include the linear version of each between factor -// BOOST_FOREACH(const boost::shared_ptr& factor, pose2graph) { -// -// boost::shared_ptr > pose2Between = -// boost::dynamic_pointer_cast >(factor); -// -// if (pose2Between) { -// Key key1 = pose2Between->keys()[0]; -// double theta1 = orientationsLago.at(key1)(0); -// double s1 = sin(theta1); -// double c1 = cos(theta1); -// -// Key key2 = pose2Between->keys()[1]; -// double theta2 = orientationsLago.at(key2)(0); -// -// double linearDeltaRot = theta2 - theta1 -// - pose2Between->measured().theta(); -// linearDeltaRot = Rot2(linearDeltaRot).theta(); // to normalize -// -// double dx = pose2Between->measured().x(); -// double dy = pose2Between->measured().y(); -// -// Vector globalDeltaCart = // -// (Vector(2) << c1 * dx - s1 * dy, s1 * dx + c1 * dy); -// Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot); // rhs -// Matrix J1 = -I3; -// J1(0, 2) = s1 * dx + c1 * dy; -// J1(1, 2) = -c1 * dx + s1 * dy; -// // Retrieve the noise model for the relative rotation -// boost::shared_ptr diagonalModel = -// boost::dynamic_pointer_cast( -// pose2Between->get_noiseModel()); -// -// linearPose2graph.add(key1, J1, key2, I3, b, diagonalModel); -// } else { -// throw invalid_argument( -// "computeLagoPoses: cannot manage non between factor here!"); -// } -// } -// // add prior -// linearPose2graph.add(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0), -// priorPose2Noise); -// -// // optimize -// VectorValues posesLago = linearPose2graph.optimize(); -// -// // put into Values structure -// Values initialGuessLago; -// BOOST_FOREACH(const VectorValues::value_type& it, posesLago) { -// Key key = it.first; -// if (key != keyAnchor) { -// const Vector& poseVector = it.second; -// Pose2 poseLago = Pose2(poseVector(0), poseVector(1), -// orientationsLago.at(key)(0) + poseVector(2)); -// initialGuessLago.insert(key, poseLago); -// } -// } -// return initialGuessLago; -//} +Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { + gttic(InitializePose3_computePoses); + + // put into Values structure + Values initialPose; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialRot){ + Key key = key_value.key; + const Rot3& rot = initialRot.at(key); + Pose3 initializedPose = Pose3(rot, Point3()); + initialPose.insert(key, initializedPose); + } + // add prior + noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); + pose3graph.add(PriorFactor(keyAnchor, Pose3(), priorModel)); + + // Create optimizer + GaussNewtonParams params; + params.maxIterations = 1; + GaussNewtonOptimizer optimizer(pose3graph, initialPose, params); + Values GNresult = optimizer.optimize(); + + // put into Values structure + Values estimate; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) { + Key key = key_value.key; + if (key != keyAnchor) { + const Pose3& pose = estimate.at(key); + estimate.insert(key, pose); + } + } + return estimate; +} -///* ************************************************************************* */ -//Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) { -// gttic(lago_initialize); -// -// // We "extract" the Pose2 subgraph of the original graph: this -// // is done to properly model priors and avoiding operating on a larger graph -// NonlinearFactorGraph pose2Graph = buildPose2graph(graph); -// -// // Get orientations from relative orientation measurements -// VectorValues orientationsLago = computeOrientations(pose2Graph, -// useOdometricPath); -// -// // Compute the full poses -// return computePoses(pose2Graph, orientationsLago); -//} -// /* ************************************************************************* */ -Values initialize(const NonlinearFactorGraph& graph, - const Values& givenGuess) { +Values initialize(const NonlinearFactorGraph& graph) { + gttic(InitializePose3_initialize); + + // 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 valueRot3 = computeOrientations(pose3Graph); + + // Compute the full poses (1 GN iteration on full poses) + return computePoses(pose3Graph, valueRot3); +} + +/* ************************************************************************* */ +Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess) { Values initialValues; // get the orientation estimates from LAGO diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h index 04a10659c..4611cf7ee 100644 --- a/gtsam/slam/InitializePose3.h +++ b/gtsam/slam/InitializePose3.h @@ -38,25 +38,11 @@ GTSAM_EXPORT Values initializeOrientations(const NonlinearFactorGraph& graph); GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph); -GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess); +GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot); -///** Linear factor graph with regularized orientation measurements */ -//GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph( -// const std::vector& spanningTreeIds, -// const std::vector& chordsIds, const NonlinearFactorGraph& g, -// const key2doubleMap& orientationsToRoot, const PredecessorMap& tree); -// -///** LAGO: Return the orientations of the Pose2 in a generic factor graph */ -//GTSAM_EXPORT VectorValues initializeOrientations( -// const NonlinearFactorGraph& graph, bool useOdometricPath = true); -// -///** Return the values for the Pose2 in a generic factor graph */ -//GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, -// bool useOdometricPath = true); -// -///** Only correct the orientation part in initialGuess */ -//GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, -// const Values& initialGuess); +GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph); + +GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess); } // end of namespace lago } // end of namespace gtsam From 889bbea8d4d19dc189b2bea1b39d43147c4336ea Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 19 Aug 2014 21:09:51 -0400 Subject: [PATCH 06/19] finished and tested initializer --- .cproject | 106 +++++++++++++---------- examples/Data/pose3example-grid.txt | 71 +++++++++++++++ examples/Data/pose3exampleGrid.g2o | 77 ---------------- gtsam/slam/InitializePose3.cpp | 5 +- gtsam/slam/tests/testInitializePose3.cpp | 15 ++++ 5 files changed, 151 insertions(+), 123 deletions(-) create mode 100644 examples/Data/pose3example-grid.txt delete mode 100644 examples/Data/pose3exampleGrid.g2o diff --git a/.cproject b/.cproject index 23ec7bdc0..1b4d0b1ec 100644 --- a/.cproject +++ b/.cproject @@ -582,6 +582,7 @@ make + tests/testBayesTree.run true false @@ -589,6 +590,7 @@ make + testBinaryBayesNet.run true false @@ -636,6 +638,7 @@ make + testSymbolicBayesNet.run true false @@ -643,6 +646,7 @@ make + tests/testSymbolicFactor.run true false @@ -650,6 +654,7 @@ make + testSymbolicFactorGraph.run true false @@ -665,6 +670,7 @@ make + tests/testBayesTree true false @@ -1000,6 +1006,7 @@ make + testErrors.run true false @@ -1229,6 +1236,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1311,7 +1358,6 @@ make - testSimulated2DOriented.run true false @@ -1351,7 +1397,6 @@ make - testSimulated2D.run true false @@ -1359,7 +1404,6 @@ make - testSimulated3D.run true false @@ -1373,46 +1417,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1670,6 +1674,7 @@ cpack + -G DEB true false @@ -1677,6 +1682,7 @@ cpack + -G RPM true false @@ -1684,6 +1690,7 @@ cpack + -G TGZ true false @@ -1691,6 +1698,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2417,6 +2425,7 @@ make + testGraph.run true false @@ -2424,6 +2433,7 @@ make + testJunctionTree.run true false @@ -2431,6 +2441,7 @@ make + testSymbolicBayesNetB.run true false @@ -2620,6 +2631,14 @@ true true + + make + -j5 + testInitializePose3.run + true + true + true + make -j2 @@ -2894,7 +2913,6 @@ make - tests/testGaussianISAM2 true false diff --git a/examples/Data/pose3example-grid.txt b/examples/Data/pose3example-grid.txt new file mode 100644 index 000000000..854fa0ccb --- /dev/null +++ b/examples/Data/pose3example-grid.txt @@ -0,0 +1,71 @@ +VERTEX_SE3:QUAT 0 1.63791e-12 7.56548e-14 -3.02811e-12 5.35657e-13 2.43616e-13 9.71152e-14 1 +VERTEX_SE3:QUAT 1 1.01609 0.00274307 -0.0351514 -0.499545 0.247735 0.723569 -0.406854 +VERTEX_SE3:QUAT 2 1.99996 0.0304956 -0.040662 0.403501 -0.294714 -0.4254 0.754563 +VERTEX_SE3:QUAT 3 1.94371 1.06535 0.0118614 -0.0471731 -0.541615 0.820893 0.17482 +VERTEX_SE3:QUAT 4 0.962753 0.999477 0.0211017 -0.19663 -0.66009 0.470743 0.551379 +VERTEX_SE3:QUAT 5 -0.00956768 0.965396 -0.021854 -0.320221 -0.518368 0.47521 0.634766 +VERTEX_SE3:QUAT 6 -0.0863793 1.97682 0.000531117 -0.0173439 -0.573793 -0.450627 0.683663 +VERTEX_SE3:QUAT 7 0.918905 2.01556 -0.0139773 0.56169 -0.440513 0.199057 0.671438 +VERTEX_SE3:QUAT 8 1.92094 2.05524 0.0469884 0.0073084 -0.372357 -0.467582 0.801663 +VERTEX_SE3:QUAT 9 1.86182 2.05449 1.09237 0.0131731 -0.05784 0.0335652 0.997674 +VERTEX_SE3:QUAT 10 0.880176 2.02406 1.00997 -0.39342 -0.287909 0.757918 0.433462 +VERTEX_SE3:QUAT 11 -0.0960463 1.98653 0.995791 0.434103 -0.199044 0.585176 0.655367 +VERTEX_SE3:QUAT 12 -0.0911401 0.997117 0.988217 -0.0925477 0.572872 0.537294 0.612019 +VERTEX_SE3:QUAT 13 0.948316 1.02239 0.991745 0.142484 0.560062 0.750078 0.321578 +VERTEX_SE3:QUAT 14 1.92631 1.08945 1.06749 0.23878 0.380837 0.796564 -0.404269 +VERTEX_SE3:QUAT 15 1.95398 0.0777667 0.982353 -0.384392 0.58733 0.685207 -0.194366 +VERTEX_SE3:QUAT 16 0.946032 0.0482667 0.952308 -0.218979 0.186315 -0.494185 0.820437 +VERTEX_SE3:QUAT 17 -0.0625076 -0.034424 0.942171 0.514725 -0.185043 -0.44771 0.707371 +VERTEX_SE3:QUAT 18 -0.083807 -0.0106666 1.9853 0.00792651 1.98919e-05 -0.00128106 0.999968 +VERTEX_SE3:QUAT 19 0.918067 -0.000897795 1.92157 -0.342141 0.241241 -0.726975 0.544288 +VERTEX_SE3:QUAT 20 1.90041 0.0323631 2.00636 0.412572 -0.0930131 -0.133075 0.896339 +VERTEX_SE3:QUAT 21 1.84895 1.05013 2.0738 -0.580757 0.35427 0.729393 -0.0721062 +VERTEX_SE3:QUAT 22 0.880221 1.00671 1.99021 0.147752 0.355662 0.917953 0.095058 +VERTEX_SE3:QUAT 23 -0.0950872 1.00374 1.95013 -0.29909 -0.0578461 0.857019 0.415594 +VERTEX_SE3:QUAT 24 -0.111581 1.97979 1.98762 0.565153 0.214463 -0.523058 0.600848 +VERTEX_SE3:QUAT 25 0.837568 2.01589 2.03075 -0.284756 0.369992 0.875484 -0.124692 +VERTEX_SE3:QUAT 26 1.82708 2.05081 2.07052 0.254696 0.250865 0.653216 0.667462 +EDGE_SE3:QUAT 0 1 1.00497 0.002077 -0.015539 -0.508004 0.250433 0.711222 -0.416386 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 1 2 -0.200593 0.339956 -0.908079 -0.093598 0.151993 0.42829 0.885836 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 2 3 -0.922791 0.330629 -0.292682 0.365657 -0.051986 0.924849 -0.090813 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 3 4 0.893075 0.246476 0.331154 -0.285927 0.341221 -0.267609 0.854517 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 4 5 0.280674 0.244242 0.923726 0.035064 0.21101 0.083834 0.973251 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 5 6 0.955621 0.355669 -0.025152 -0.306713 0.131221 -0.781587 0.527096 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 6 7 -0.076631 0.636081 -0.771439 0.702021 0.326514 0.122181 0.620988 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 7 8 0.582761 -0.721177 -0.376875 -0.733841 -0.170725 -0.256653 0.605359 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 8 9 0.600312 0.298765 0.767014 0.057612 0.332574 0.486324 0.805956 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 9 10 -0.986649 0.03008 -0.008766 -0.362177 -0.253215 0.763748 0.470531 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 10 11 0.275109 0.534769 0.823463 0.450708 -0.472399 -0.432689 0.621677 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 11 12 -0.61882 0.024878 0.773748 0.0927029 0.786162 -0.21122 0.573359 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 12 13 -0.175537 -0.730832 0.634529 -0.018628 0.006375 0.428306 0.903419 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 13 14 -0.700208 -0.245198 0.637353 -0.035865 0.273394 0.645363 0.712374 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 14 15 0.373495 0.373768 -0.846199 0.400323 0.310362 -0.422222 0.751762 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 15 16 0.648588 0.157829 0.72252 0.781502 -0.210141 -0.501005 -0.30674 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 16 17 -0.390339 -0.702656 -0.572321 0.765815 0.055816 0.032478 0.63981 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 17 18 -0.261114 0.908685 0.421318 -0.501833 0.166567 0.448468 0.720622 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 18 19 1.00815 0.012634 -0.029822 -0.347007 0.205082 -0.740641 0.537569 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 19 20 -0.162376 0.581623 0.810804 0.628338 0.075411 0.650639 0.41973 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 20 21 -0.358942 0.627689 -0.704045 -0.469133 0.542456 0.530583 -0.451816 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 21 22 0.362417 0.298352 0.854822 0.004058 -0.696926 0.140345 0.703265 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 22 23 0.934942 0.020321 -0.358044 -0.445461 0.260916 -0.379862 0.767589 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 23 24 0.741887 -0.657659 0.215293 -0.584859 0.196138 0.688031 0.38221 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 24 25 0.300145 0.82011 -0.39974 0.46538 -0.593595 -0.202131 0.624668 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 25 26 -0.85591 0.022701 -0.510794 0.12929 -0.685192 -0.503707 0.509978 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 0 5 0.026721 0.990497 -0.007651 -0.317476 -0.510239 0.467341 0.648427 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 3 8 0.390516 -0.401461 -0.830724 0.503106 -0.367814 0.780584 0.047806 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 4 1 -0.813838 -0.446181 0.319175 0.224903 -0.031827 0.97265 0.048561 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 4 13 0.571273 -0.805401 0.077339 0.892031 0.329761 0.275468 0.140201 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 5 12 0.389794 -0.882655 0.268063 0.712423 0.550662 0.275339 0.33677 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 6 11 0.800298 0.505022 0.361738 0.739335 0.419366 0.443817 0.283801 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 10 13 -0.912531 0.430955 -0.018942 0.830493 -0.093519 0.272041 0.477001 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 12 23 -0.797606 0.437737 0.311476 -0.657137 -0.196625 0.136652 0.714728 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 13 22 -0.116836 0.952032 0.269398 -0.216437 0.086571 0.260965 0.936781 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 14 21 0.749295 0.373389 0.581641 0.253048 0.511007 -0.537262 0.621439 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 16 1 0.160985 0.555966 -0.811911 0.748057 0.122381 -0.369631 0.537407 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 18 23 0.028909 1.02689 -0.00265 -0.294167 -0.071607 0.850901 0.429308 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 19 16 -0.230711 0.750637 -0.607511 0.14647 -0.102538 0.297899 0.937704 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 20 15 -0.031986 -0.741129 -0.728721 -0.278926 0.731172 0.404675 -0.473103 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 22 19 -0.332601 0.704401 -0.687251 -0.372165 -0.054346 0.713024 0.591725 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 22 25 0.347067 -0.634646 0.657147 0.018567 0.476762 0.040939 0.877882 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 25 10 0.388971 -0.723981 -0.559653 -0.373459 -0.014654 -0.696123 0.612965 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 +EDGE_SE3:QUAT 26 21 -0.979482 -0.024822 0.043763 -0.326753 0.819942 0.292615 0.367837 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400 diff --git a/examples/Data/pose3exampleGrid.g2o b/examples/Data/pose3exampleGrid.g2o deleted file mode 100644 index c93ee9c19..000000000 --- a/examples/Data/pose3exampleGrid.g2o +++ /dev/null @@ -1,77 +0,0 @@ -VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000 -VERTEX_SE3:QUAT 1 0.968478 -0.015273 -0.024009 -0.568468 -0.224707 0.094925 0.785710 -VERTEX_SE3:QUAT 2 1.991529 -0.016938 -0.045515 -0.542127 -0.179147 -0.494163 0.655597 -VERTEX_SE3:QUAT 3 2.026568 0.958559 -0.048399 0.363597 0.453322 -0.812746 0.041715 -VERTEX_SE3:QUAT 4 0.987528 0.968615 -0.085312 -0.082238 0.306441 0.936656 0.148344 -VERTEX_SE3:QUAT 5 -0.031537 0.995686 -0.066640 0.335085 0.235204 -0.686500 0.600929 -VERTEX_SE3:QUAT 6 -0.002507 1.983505 -0.158449 -0.401363 0.515966 0.711353 0.258194 -VERTEX_SE3:QUAT 7 1.018807 1.972634 -0.131017 0.323170 -0.282056 0.470606 0.771062 -VERTEX_SE3:QUAT 8 1.994170 1.909926 -0.104255 -0.137653 -0.266858 -0.928006 0.220551 -VERTEX_SE3:QUAT 9 1.910394 1.961603 0.916595 -0.028577 -0.021584 -0.026441 0.999009 -VERTEX_SE3:QUAT 10 0.933629 2.024306 0.857267 -0.303946 -0.378940 -0.856390 0.174981 -VERTEX_SE3:QUAT 11 -0.061508 2.088874 0.841008 0.562526 -0.203739 0.259658 0.758045 -VERTEX_SE3:QUAT 12 -0.106592 1.071471 0.945747 -0.397133 0.277162 -0.755756 0.440794 -VERTEX_SE3:QUAT 13 0.897260 1.009421 1.068154 -0.534411 0.418094 -0.189137 0.709809 -VERTEX_SE3:QUAT 14 1.907694 0.966386 1.128843 0.576449 -0.149518 0.310096 0.741074 -VERTEX_SE3:QUAT 15 1.827009 -0.065936 1.212580 0.440256 -0.304111 0.217991 0.816193 -VERTEX_SE3:QUAT 16 0.811179 -0.009700 1.175299 0.228574 0.021414 0.687899 0.688543 -VERTEX_SE3:QUAT 17 -0.207493 0.046063 1.198636 0.083786 0.149500 0.278888 0.944908 -VERTEX_SE3:QUAT 18 -0.239598 0.069543 2.175883 -0.020534 -0.002367 -0.032999 0.999242 -VERTEX_SE3:QUAT 19 0.795282 0.024403 2.172337 -0.059128 -0.663282 -0.736161 0.120951 -VERTEX_SE3:QUAT 20 1.795463 -0.039545 2.189997 0.262525 0.561623 0.108661 0.777080 -VERTEX_SE3:QUAT 21 1.838289 0.918009 2.142889 0.391707 0.271811 0.534284 0.698015 -VERTEX_SE3:QUAT 22 0.843244 0.956966 2.182675 0.175401 -0.294535 0.796879 0.497462 -VERTEX_SE3:QUAT 23 -0.184445 0.969193 2.212233 -0.022667 -0.309094 0.188130 0.931963 -VERTEX_SE3:QUAT 24 -0.188460 1.988074 2.109480 0.280735 -0.333968 -0.767404 0.469834 -VERTEX_SE3:QUAT 25 0.832834 1.987249 2.119934 0.409102 -0.211526 0.520233 0.719201 -VERTEX_SE3:QUAT 26 1.842345 1.933435 2.082102 0.498150 -0.149112 0.556190 0.648278 -EDGE_SE3:QUAT 0 1 0.968478 -0.015273 -0.024009 -0.568468 -0.224707 0.094925 0.785710 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 1 2 0.895353 0.128330 -0.478510 -0.181316 0.338937 -0.430522 0.816638 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 2 3 -0.429131 -0.043280 0.875673 -0.108628 0.924958 -0.331599 0.150650 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 3 4 0.786542 -0.392786 0.555103 0.731034 -0.219263 -0.010938 0.646060 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 4 5 0.962014 0.324025 0.095456 -0.529804 0.406661 0.542675 0.509350 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 5 6 -0.592671 -0.143889 -0.783004 0.849231 -0.212159 -0.337427 0.346321 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 6 7 -0.578557 -0.780086 -0.317320 -0.050541 -0.889438 -0.373451 0.258610 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 7 8 0.373848 -0.901266 -0.062758 0.564749 0.378680 0.694277 0.235883 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 8 9 0.436073 0.363895 0.853958 0.144187 0.238953 0.925910 0.254564 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 9 10 -0.980372 0.013025 -0.014878 -0.307108 -0.358351 -0.855183 0.214316 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 10 11 0.735233 -0.578374 -0.345900 -0.601711 -0.654423 -0.419528 0.183497 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 11 12 -0.139622 -0.130002 1.005841 0.631012 0.022105 0.762353 0.141962 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 12 13 -0.198544 0.382112 0.917128 -0.217232 -0.341211 0.470993 0.783933 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 13 14 0.591922 -0.251242 0.782954 0.703837 -0.472659 0.521380 0.096797 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 14 15 -0.309581 -0.032947 0.991103 -0.205941 -0.114195 0.017928 0.971713 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 15 16 -0.752100 0.640733 0.245499 0.097291 0.479897 0.332422 0.806058 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 16 17 0.006363 0.960206 -0.345414 -0.061422 0.088813 -0.490352 0.864809 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 17 18 -0.243141 0.271842 0.907511 -0.098852 -0.148661 -0.312729 0.932914 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 18 19 1.035565 0.023491 -0.008895 -0.036454 -0.649327 -0.745091 0.147935 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 19 20 -0.953182 0.279364 -0.134773 0.263672 -0.770187 -0.444277 0.374040 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 20 21 0.497302 0.783380 -0.244889 -0.149390 -0.083102 0.487968 0.855958 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 21 22 -0.241016 0.568368 -0.782358 -0.446393 -0.122376 0.453495 0.761643 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 22 23 0.481169 0.908199 0.028604 -0.365642 0.171794 -0.588182 0.700596 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 23 24 0.310272 0.963355 -0.156115 -0.027745 -0.201443 -0.897925 0.390360 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 24 25 -0.409898 0.553347 -0.754283 0.326372 0.600803 0.719096 0.124169 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 25 26 0.314171 -0.950746 0.144281 0.133133 -0.001730 0.018387 0.990926 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 0 5 0.003921 0.989262 -0.008668 0.344972 0.271052 -0.662312 0.607345 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 0 17 -0.004748 -0.016661 1.014253 0.095179 0.169387 0.315461 0.928834 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 1 4 0.413069 0.396669 0.824709 0.266584 -0.252890 0.915410 0.164328 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 3 8 0.265427 -0.541821 -0.818705 0.520335 -0.589688 0.189278 0.587956 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 3 14 -0.593398 -0.729533 0.348548 0.279515 -0.277200 -0.916882 0.066028 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 6 11 -0.866710 0.505129 0.215695 0.172623 -0.951867 -0.249430 0.044004 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 7 10 0.743464 0.292393 0.573155 0.712782 0.375290 0.526276 0.272273 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 9 14 -0.011444 -1.026550 0.016772 0.610325 -0.121904 0.314891 0.716580 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 9 26 -0.033212 -0.029706 0.975351 0.510053 -0.198209 0.557527 0.624278 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 10 25 0.655684 0.485916 0.598187 -0.700078 -0.428078 -0.464953 0.332353 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 12 17 0.930615 0.369372 0.051025 0.230774 -0.278742 0.911677 0.194659 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 12 23 0.306720 -0.815553 0.415865 0.581204 -0.505705 0.604084 0.203831 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 13 4 0.474778 0.869505 -0.090395 -0.410825 -0.369465 0.822956 0.132141 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 13 16 0.632289 -0.415884 -0.668153 0.221431 -0.613167 0.709932 0.266441 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 13 22 -0.442996 -0.863878 0.046061 -0.079225 0.840832 -0.535376 0.009883 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 15 20 0.632455 0.661923 0.364197 0.030730 0.661379 -0.405142 0.630470 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 16 1 -0.307083 -0.356121 -0.862632 -0.732371 0.242194 -0.446749 0.453200 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 16 19 0.347792 0.365913 0.878300 0.501749 0.573409 0.434965 0.479850 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 18 23 0.027959 1.027767 -0.055691 0.034139 -0.330077 0.166677 0.928495 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 19 22 -0.173539 -0.125423 0.957399 -0.799298 -0.382195 -0.336755 0.318821 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 21 14 -0.104907 -0.825889 -0.563170 -0.004285 -0.468249 0.018532 0.883392 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 21 26 0.956031 0.050291 -0.309699 -0.152297 -0.325772 0.225738 0.905384 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 24 11 0.143525 -0.745793 -0.608698 0.263093 0.680516 0.559568 0.393147 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 -EDGE_SE3:QUAT 25 22 -0.512781 -0.078741 0.857967 -0.053138 0.120352 0.410455 0.902341 2500.000000 0.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 0.000000 2500.000000 0.000000 0.000000 2500.000000 0.000000 2500.000000 diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index bed24fb1e..578b78dd5 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -170,6 +170,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { } // add prior noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); + initialPose.insert(keyAnchor, Pose3()); pose3graph.add(PriorFactor(keyAnchor, Pose3(), priorModel)); // Create optimizer @@ -180,10 +181,10 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { // put into Values structure Values estimate; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) { + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, GNresult) { Key key = key_value.key; if (key != keyAnchor) { - const Pose3& pose = estimate.at(key); + const Pose3& pose = GNresult.at(key); estimate.insert(key, pose); } } diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index 77cfe8e12..355fb1238 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -103,6 +103,21 @@ TEST( InitializePose3, posesWithGivenGuess ) { EXPECT(assert_equal(givenPoses, initial, 1e-6)); } +/* ************************************************************************* */ +TEST( InitializePose3, initializePoses ) +{ + const string g2oFile = findExampleDataFile("pose3example-grid"); + NonlinearFactorGraph::shared_ptr inputGraph; + Values::shared_ptr expectedValues; + bool is3D = true; + boost::tie(inputGraph, expectedValues) = readG2o(g2oFile, is3D); + noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); + inputGraph->add(PriorFactor(0, Pose3(), priorModel)); + + Values initial = InitializePose3::initialize(*inputGraph); + EXPECT(assert_equal(*expectedValues,initial,1e-4)); +} + /* ************************************************************************* */ int main() { From e605c2dbc51dc44f4338239dd5e25a26762f9754 Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 25 Aug 2014 16:01:53 -0400 Subject: [PATCH 07/19] added initialization example --- examples/Pose3SLAMExample_initializePose3.cpp | 63 +++++++++++++++++++ 1 file changed, 63 insertions(+) create mode 100644 examples/Pose3SLAMExample_initializePose3.cpp diff --git a/examples/Pose3SLAMExample_initializePose3.cpp b/examples/Pose3SLAMExample_initializePose3.cpp new file mode 100644 index 000000000..1754239af --- /dev/null +++ b/examples/Pose3SLAMExample_initializePose3.cpp @@ -0,0 +1,63 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Pose3SLAMExample_initializePose3.cpp + * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3 + * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o + * @date Aug 25, 2014 + * @author Luca Carlone + */ + +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main(const int argc, const char *argv[]) { + + // Read graph from file + string g2oFile; + if (argc < 2) + g2oFile = findExampleDataFile("pose3example.txt"); + else + g2oFile = argv[1]; + + NonlinearFactorGraph::shared_ptr graph; + Values::shared_ptr initial; + bool is3D = true; + boost::tie(graph, initial) = readG2o(g2oFile, is3D); + + // Add prior on the pose having index (key) = 0 + NonlinearFactorGraph graphWithPrior = *graph; + noiseModel::Diagonal::shared_ptr priorModel = // + noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)); + graphWithPrior.add(PriorFactor(0, Pose3(), priorModel)); + // graphWithPrior.print(); + + std::cout << "Initializing Pose3" << std::endl; + Values initialization = InitializePose3::initialize(graphWithPrior); + std::cout << "done!" << std::endl; + + if (argc < 3) { + initialization.print("initialization"); + } else { + const string outputFile = argv[2]; + std::cout << "Writing results to file: " << outputFile << std::endl; + writeG2o(*graph, initialization, outputFile); + std::cout << "done! " << std::endl; + } + + return 0; +} From f6adeb8fff4e951810a937b1c8dc24ec8d0bcaa0 Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 25 Aug 2014 16:36:58 -0400 Subject: [PATCH 08/19] working example --- examples/Pose3SLAMExample_initializePose3.cpp | 40 ++++++++++++++++--- gtsam/slam/dataset.cpp | 10 ++--- 2 files changed, 40 insertions(+), 10 deletions(-) diff --git a/examples/Pose3SLAMExample_initializePose3.cpp b/examples/Pose3SLAMExample_initializePose3.cpp index 1754239af..b4bcf970c 100644 --- a/examples/Pose3SLAMExample_initializePose3.cpp +++ b/examples/Pose3SLAMExample_initializePose3.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -39,12 +40,17 @@ int main(const int argc, const char *argv[]) { bool is3D = true; boost::tie(graph, initial) = readG2o(g2oFile, is3D); - // Add prior on the pose having index (key) = 0 + // Add prior on the first key NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)); - graphWithPrior.add(PriorFactor(0, Pose3(), priorModel)); - // graphWithPrior.print(); + Key firstKey = 0; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) { + std::cout << "Adding prior to g2o file " << std::endl; + firstKey = key_value.key; + graphWithPrior.add(PriorFactor(firstKey, Pose3(), priorModel)); + break; + } std::cout << "Initializing Pose3" << std::endl; Values initialization = InitializePose3::initialize(graphWithPrior); @@ -54,10 +60,34 @@ int main(const int argc, const char *argv[]) { initialization.print("initialization"); } else { const string outputFile = argv[2]; - std::cout << "Writing results to file: " << outputFile << std::endl; + std::cout << "Writing results to file: " << outputFile << argc << std::endl; writeG2o(*graph, initialization, outputFile); std::cout << "done! " << std::endl; - } + // This should be deleted: only for debug + if (argc == 4){ + const string inputFileRewritten = argv[3]; + std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl; + // Additional: rewrite input with simplified keys 0,1,... + Values simpleInitial; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) { + Key key = key_value.key - firstKey; + simpleInitial.insert(key, initial->at(key_value.key)); + } + NonlinearFactorGraph simpleGraph; + BOOST_FOREACH(const boost::shared_ptr& factor, *graph) { + boost::shared_ptr > pose3Between = + boost::dynamic_pointer_cast >(factor); + if (pose3Between){ + Key key1 = pose3Between->key1() - firstKey; + Key key2 = pose3Between->key2() - firstKey; + NonlinearFactor::shared_ptr simpleFactor( + new BetweenFactor(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel())); + simpleGraph.add(simpleFactor); + } + } + writeG2o(simpleGraph, simpleInitial, inputFileRewritten); + } + } return 0; } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 7d06ad0cf..e8b266b19 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -182,7 +182,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID, break; if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { - int id; + Key id; double x, y, yaw; is >> id >> x >> y >> yaw; @@ -468,7 +468,7 @@ GraphAndValues load3D(const string& filename) { ls >> tag; if (tag == "VERTEX3") { - int id; + Key id; double x, y, z, roll, pitch, yaw; ls >> id >> x >> y >> z >> roll >> pitch >> yaw; Rot3 R = Rot3::ypr(yaw,pitch,roll); @@ -476,7 +476,7 @@ GraphAndValues load3D(const string& filename) { initial->insert(id, Pose3(R,t)); } if (tag == "VERTEX_SE3:QUAT") { - int id; + Key id; double x, y, z, qx, qy, qz, qw; ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw; Rot3 R = Rot3::quaternion(qw, qx, qy, qz); @@ -495,7 +495,7 @@ GraphAndValues load3D(const string& filename) { ls >> tag; if (tag == "EDGE3") { - int id1, id2; + Key id1, id2; double x, y, z, roll, pitch, yaw; ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; Rot3 R = Rot3::ypr(yaw,pitch,roll); @@ -511,7 +511,7 @@ GraphAndValues load3D(const string& filename) { } if (tag == "EDGE_SE3:QUAT") { Matrix m = eye(6); - int id1, id2; + Key id1, id2; double x, y, z, qx, qy, qz, qw; ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; Rot3 R = Rot3::quaternion(qw, qx, qy, qz); From 16bb90387c13e7c96f1fc8c76b07882e2e20946c Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 31 Aug 2014 13:03:31 -0400 Subject: [PATCH 09/19] added example optimizing rotations only from g2o file --- examples/Pose3SLAMExample_initializePose3.cpp | 2 +- examples/Pose3SLAMExample_rotOnly.cpp | 95 +++++++++++++++++++ 2 files changed, 96 insertions(+), 1 deletion(-) create mode 100644 examples/Pose3SLAMExample_rotOnly.cpp diff --git a/examples/Pose3SLAMExample_initializePose3.cpp b/examples/Pose3SLAMExample_initializePose3.cpp index b4bcf970c..bd46bb917 100644 --- a/examples/Pose3SLAMExample_initializePose3.cpp +++ b/examples/Pose3SLAMExample_initializePose3.cpp @@ -60,7 +60,7 @@ int main(const int argc, const char *argv[]) { initialization.print("initialization"); } else { const string outputFile = argv[2]; - std::cout << "Writing results to file: " << outputFile << argc << std::endl; + std::cout << "Writing results to file: " << outputFile << std::endl; writeG2o(*graph, initialization, outputFile); std::cout << "done! " << std::endl; diff --git a/examples/Pose3SLAMExample_rotOnly.cpp b/examples/Pose3SLAMExample_rotOnly.cpp new file mode 100644 index 000000000..293fa1a09 --- /dev/null +++ b/examples/Pose3SLAMExample_rotOnly.cpp @@ -0,0 +1,95 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Pose3SLAMExample_initializePose3.cpp + * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3 + * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o + * @date Aug 25, 2014 + * @author Luca Carlone + */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main(const int argc, const char *argv[]) { + + // Read graph from file + string g2oFile; + if (argc < 2) + g2oFile = findExampleDataFile("pose3example.txt"); + else + g2oFile = argv[1]; + + NonlinearFactorGraph::shared_ptr graph; + Values::shared_ptr initial; + bool is3D = true; + boost::tie(graph, initial) = readG2o(g2oFile, is3D); + + Values initialRot; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) { + Key key = key_value.key; + Pose3 pose = initial->at(key); + Rot3 R = pose.rotation(); + initialRot.insert(key,R); + } + + noiseModel::Unit::shared_ptr identityModel = noiseModel::Unit::Create(3); + NonlinearFactorGraph graphRot; + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *graph) { + boost::shared_ptr > pose3Between = + boost::dynamic_pointer_cast >(factor); + if(pose3Between){ + Key key1 = pose3Between->key1(); + Key key2 = pose3Between->key2(); + Pose3 Pij = pose3Between->measured(); + Rot3 Rij = Pij.rotation(); + NonlinearFactor::shared_ptr factorRot(new BetweenFactor(key1, key2, Rij, identityModel)); + graphRot.add(factorRot); + }else{ + std::cout << "Found a factor that is not a Between: not admitted" << std::endl; + return 1; + } + } + // Add prior on the first key + graphRot.add(PriorFactor(0, Rot3(), identityModel)); + + std::cout << "Optimizing Rot3 via GN" << std::endl; + // GaussNewtonParams params; + GaussNewtonOptimizer optimizer(graphRot, initialRot); + Values GNrot = optimizer.optimize(); + std::cout << "done!" << std::endl; + + // Wrap estimate as poses to write in g2o format + Values GNposes; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, GNrot) { + Key key = key_value.key; + Rot3 R = GNrot.at(key); + GNposes.insert(key,Pose3(R,Point3())); + } + + if (argc < 3) { + GNrot.print("initialization"); + } else { + const string outputFile = argv[2]; + std::cout << "Writing results to file: " << outputFile << std::endl; + writeG2o(*graph, GNposes, outputFile); + std::cout << "done! " << std::endl; + } + return 0; +} From 15abbc484b93533d5f2c4b2cce8b886eec11149e Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 2 Sep 2014 18:38:12 -0400 Subject: [PATCH 10/19] setting up the gradient method --- gtsam/slam/InitializePose3.cpp | 43 +++++++++++++++++++----- gtsam/slam/InitializePose3.h | 6 ++-- gtsam/slam/tests/testInitializePose3.cpp | 4 ++- 3 files changed, 39 insertions(+), 14 deletions(-) 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)); From 366101176c57f73216709691b4ce44489bccc63a Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 2 Sep 2014 18:50:23 -0400 Subject: [PATCH 11/19] added initial failing unit test --- gtsam/slam/InitializePose3.cpp | 33 ++++++++++++++++++------ gtsam/slam/InitializePose3.h | 2 ++ gtsam/slam/tests/testInitializePose3.cpp | 19 ++++++++++++++ 3 files changed, 46 insertions(+), 8 deletions(-) diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 38bfcf82f..5779a109c 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -38,6 +38,8 @@ static const Matrix zero33= Matrix::Zero(3,3); static const Key keyAnchor = symbol('Z', 9999999); +typedef std::map > KeyVectorMap; + /* ************************************************************************* */ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { @@ -150,16 +152,31 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess) { gttic(InitializePose3_computeOrientationsGradient); + // this works on the inverse rotations, according to Tron&Vidal,2011 + Values inverseRot; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, givenGuess) { + Key key = key_value.key; + const Pose3& pose = givenGuess.at(key); + inverseRot.insert(key, pose.rotation().inverse()); + } + // Create the map of edges incident on each node + KeyVectorMap adjEdgesMap; +// +// // regularize measurements and plug everything in a factor graph +// GaussianFactorGraph relaxedGraph = buildLinearOrientationGraph(pose3Graph); +// +// // Solve the LFG +// VectorValues relaxedRot3 = relaxedGraph.optimize(); - // 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 correct rotations + Values estimateRot; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { + Key key = key_value.key; + const Rot3& R = inverseRot.at(key); + estimateRot.insert(key, R.inverse()); + } + return estimateRot; } /* ************************************************************************* */ diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h index 7b5489353..b6632212c 100644 --- a/gtsam/slam/InitializePose3.h +++ b/gtsam/slam/InitializePose3.h @@ -34,6 +34,8 @@ GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph); +GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess); + GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph); GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot); diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index 5387af7ef..57715deca 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -91,6 +91,25 @@ TEST( InitializePose3, orientations ) { EXPECT(assert_equal(simple::R3, initial.at(x3), 1e-6)); } +/* *************************************************************************** */ +TEST( InitializePose3, orientationsGradient ) { + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); + + // Wrong initial guess - initialization should fix the rotations + Values givenPoses; + givenPoses.insert(x0,simple::pose0); + givenPoses.insert(x1,simple::pose0); + givenPoses.insert(x2,simple::pose0); + givenPoses.insert(x3,simple::pose0); + Values initial = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses); + + // 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, posesWithGivenGuess ) { Values givenPoses; From 8384c37179a9fc94656a72da1ab6a693b1a6d63a Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 2 Sep 2014 20:17:31 -0400 Subject: [PATCH 12/19] fixed 1 unit test --- gtsam/slam/InitializePose3.cpp | 122 +++++++++++++++++++++-- gtsam/slam/InitializePose3.h | 10 ++ gtsam/slam/tests/testInitializePose3.cpp | 32 ++++++ 3 files changed, 156 insertions(+), 8 deletions(-) diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 5779a109c..afb9be15f 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -38,8 +38,6 @@ static const Matrix zero33= Matrix::Zero(3,3); static const Key keyAnchor = symbol('Z', 9999999); -typedef std::map > KeyVectorMap; - /* ************************************************************************* */ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { @@ -154,6 +152,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const // this works on the inverse rotations, according to Tron&Vidal,2011 Values inverseRot; + inverseRot.insert(keyAnchor, Rot3()); BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, givenGuess) { Key key = key_value.key; const Pose3& pose = givenGuess.at(key); @@ -162,12 +161,71 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const // Create the map of edges incident on each node KeyVectorMap adjEdgesMap; -// -// // regularize measurements and plug everything in a factor graph -// GaussianFactorGraph relaxedGraph = buildLinearOrientationGraph(pose3Graph); -// -// // Solve the LFG -// VectorValues relaxedRot3 = relaxedGraph.optimize(); + KeyRotMap factorId2RotMap; + + createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph); + + // calculate max node degree & allocate gradient + size_t maxNodeDeg = 0; + VectorValues grad; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { + Key key = key_value.key; + grad.insert(key,Vector3::Zero()); + size_t currNodeDeg = (adjEdgesMap.at(key)).size(); + if(currNodeDeg > maxNodeDeg) + maxNodeDeg = currNodeDeg; + } + + // Create parameters + double b = 1; + double f0 = 1/b - (1/b + M_PI) * exp(-b*M_PI); + double a = (M_PI*M_PI)/(2*f0); + double rho = 2*a*b; + double mu_max = maxNodeDeg * rho; + double stepsize = 2/mu_max; // = 1/(a b dG) + size_t maxIter = 1000; + + // gradient iterations + vector reshapedCost; + for(size_t it=0; it < maxIter; it++){ + ////////////////////////////////////////////////////////////////////////// + // compute the gradient at each node + double maxGrad = 0; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { + Key key = key_value.key; + grad.at(key) = Vector3::Zero(); // reset gradient + + // collect the gradient for each edge incident on key + BOOST_FOREACH(const size_t& factorId, adjEdgesMap.at(key)){ + Rot3 Rij = factorId2RotMap.at(factorId); + Rot3 Ri = inverseRot.at(key); + if( key == (pose3Graph.at(factorId))->keys()[0] ){ + Key key1 = (pose3Graph.at(factorId))->keys()[1]; + Rot3 Rj = inverseRot.at(key1); + grad.at(key) = grad.at(key) + stepsize * gradientTron(Ri, Rij * Rj, a, b); + }else if( key == (pose3Graph.at(factorId))->keys()[1] ){ + Key key0 = (pose3Graph.at(factorId))->keys()[0]; + Rot3 Rj = inverseRot.at(key0); + grad.at(key) = grad.at(key) + stepsize * gradientTron(Ri, Rij.inverse() * Rj, a, b); + }else{ + std::cout << "Error in gradient computation" << std::endl; + } + } // end of i-th gradient computation + + double normGradKey = (grad.at(key)).norm(); + if(normGradKey>maxGrad) + maxGrad = normGradKey; + } // end of loop over nodes + + ////////////////////////////////////////////////////////////////////////// + // update estimates + inverseRot = inverseRot.retract(grad); + + ////////////////////////////////////////////////////////////////////////// + // check stopping condition + if (it>20 && maxGrad < 5e-3) + break; + } // enf of gradient iterations // Return correct rotations Values estimateRot; @@ -179,6 +237,54 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const return estimateRot; } +/* ************************************************************************* */ +void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph){ + size_t factorId = 0; + BOOST_FOREACH(const boost::shared_ptr& factor, pose3Graph) { + boost::shared_ptr > pose3Between = + boost::dynamic_pointer_cast >(factor); + if (pose3Between){ + Rot3 Rij = pose3Between->measured().rotation(); + factorId2RotMap.insert(pair(factorId,Rij)); + + Key key1 = pose3Between->key1(); + if (adjEdgesMap.find(key1) != adjEdgesMap.end()){ // key is already in + adjEdgesMap.at(key1).push_back(factorId); + }else{ + vector edge_id; + edge_id.push_back(factorId); + adjEdgesMap.insert(pair >(key1, edge_id)); + } + Key key2 = pose3Between->key2(); + if (adjEdgesMap.find(key2) != adjEdgesMap.end()){ // key is already in + adjEdgesMap.at(key2).push_back(factorId); + }else{ + vector edge_id; + edge_id.push_back(factorId); + adjEdgesMap.insert(pair >(key2, edge_id)); + } + }else{ + std::cout << "Error in computeOrientationsGradient" << std::endl; + } + factorId++; + } +} + +/* ************************************************************************* */ +Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) { + Vector3 logRot = Rot3::Logmap(R1.between(R2)); + if(logRot.norm()<1e-4){ // some tolerance + Rot3 R1pert = R1.compose( Rot3::Expmap((Vector(3)<< 0.0, 0.0, 0.0)) ); // some perturbation + logRot = Rot3::Logmap(R1pert.between(R2)); + } + double th = logRot.norm(); + if (th > 1e-5) + logRot = logRot / th; + + double fdot = a*b*th*exp(-b*th); + return fdot*logRot; +} + /* ************************************************************************* */ Values initializeOrientations(const NonlinearFactorGraph& graph) { diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h index b6632212c..2f6fb842e 100644 --- a/gtsam/slam/InitializePose3.h +++ b/gtsam/slam/InitializePose3.h @@ -24,8 +24,13 @@ #include #include #include +#include namespace gtsam { + +typedef std::map > KeyVectorMap; +typedef std::map KeyRotMap; + namespace InitializePose3 { GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g); @@ -36,6 +41,11 @@ GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3 GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess); +GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, + const NonlinearFactorGraph& pose3Graph); + +GTSAM_EXPORT Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b); + GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph); GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot); diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index 57715deca..b13ab9c32 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -91,6 +91,38 @@ TEST( InitializePose3, orientations ) { EXPECT(assert_equal(simple::R3, initial.at(x3), 1e-6)); } +/* *************************************************************************** */ +TEST( InitializePose3, orientationsGradientSymbolicGraph ) { + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); + + KeyVectorMap adjEdgesMap; + KeyRotMap factorId2RotMap; + + 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); + EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[2], 4, 1e-9); + EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[3], 5, 1e-9); + EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0).size(), 4, 1e-9); + + EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x1)[0], 0, 1e-9); + EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x1)[1], 1, 1e-9); + EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x1).size(), 2, 1e-9); + + EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x2)[0], 1, 1e-9); + EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x2)[1], 2, 1e-9); + EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x2)[2], 3, 1e-9); + EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x2).size(), 3, 1e-9); + + EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x3)[0], 2, 1e-9); + EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x3)[1], 4, 1e-9); + EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x3).size(), 2, 1e-9); + + // This includes the anchor + EXPECT_DOUBLES_EQUAL(adjEdgesMap.size(), 5, 1e-9); +} + /* *************************************************************************** */ TEST( InitializePose3, orientationsGradient ) { NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); From 35d5b56b65536b1038485f809594ae6f215242d7 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 2 Sep 2014 20:56:36 -0400 Subject: [PATCH 13/19] still debugging --- gtsam/slam/InitializePose3.cpp | 8 +++-- gtsam/slam/tests/testInitializePose3.cpp | 41 ++++++++++++++++++++++++ 2 files changed, 46 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index afb9be15f..7bf29535b 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -183,13 +183,14 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const double rho = 2*a*b; double mu_max = maxNodeDeg * rho; double stepsize = 2/mu_max; // = 1/(a b dG) - size_t maxIter = 1000; + size_t maxIter = 1; // gradient iterations - vector reshapedCost; for(size_t it=0; it < maxIter; it++){ ////////////////////////////////////////////////////////////////////////// // compute the gradient at each node + std::cout << "it " << it <<" b " << b <<" f0 " << f0 <<" a " << a + <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; double maxGrad = 0; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { Key key = key_value.key; @@ -213,6 +214,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const } // end of i-th gradient computation double normGradKey = (grad.at(key)).norm(); + std::cout << "key " << DefaultKeyFormatter(key) <<" grad " << grad.at(key) << std::endl; if(normGradKey>maxGrad) maxGrad = normGradKey; } // end of loop over nodes @@ -274,7 +276,7 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) { Vector3 logRot = Rot3::Logmap(R1.between(R2)); if(logRot.norm()<1e-4){ // some tolerance - Rot3 R1pert = R1.compose( Rot3::Expmap((Vector(3)<< 0.0, 0.0, 0.0)) ); // some perturbation + Rot3 R1pert = R1.compose( Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01)) ); // some perturbation logRot = Rot3::Logmap(R1pert.between(R2)); } double th = logRot.norm(); diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index b13ab9c32..2eb0b3e67 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -123,6 +123,42 @@ TEST( InitializePose3, orientationsGradientSymbolicGraph ) { EXPECT_DOUBLES_EQUAL(adjEdgesMap.size(), 5, 1e-9); } +/* *************************************************************************** * +TEST( InitializePose3, orientationsCheckGradient ) { + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); + + // Wrong initial guess - initialization should fix the rotations + Values givenPoses; + givenPoses.insert(x0,simple::pose0); + givenPoses.insert(x1,simple::pose0); + givenPoses.insert(x2,simple::pose0); + givenPoses.insert(x3,simple::pose0); + Values initial = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses); + + // 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, singleGradient ) { + Rot3 R1 = Rot3(); + Matrix M = Matrix3::Zero(); + M(0,1) = -1; M(1,0) = 1; M(2,2) = 1; + Rot3 R2 = Rot3(M); + double a = 6.010534238540223; + double b = 1.0; + + Vector actual = InitializePose3::gradientTron(R1, R2, a, b); + Vector expected = Vector3::Zero(); + expected(2) = 1.962658662803917; + + // comparison is up to M_PI, that's why we add some multiples of 2*M_PI + EXPECT(assert_equal(expected, actual, 1e-6)); +} + /* *************************************************************************** */ TEST( InitializePose3, orientationsGradient ) { NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); @@ -135,6 +171,11 @@ TEST( InitializePose3, orientationsGradient ) { givenPoses.insert(x3,simple::pose0); Values initial = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses); + const Key keyAnchor = symbol('Z', 9999999); + givenPoses.insert(keyAnchor,simple::pose0); + string g2oFile = "/home/aspn/Desktop/toyExample.g2o"; + writeG2o(pose3Graph, givenPoses, g2oFile); + // 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)); From 103d2a8ae9be739536b8e51c8b79dc56309ebfd9 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 3 Sep 2014 15:23:35 -0400 Subject: [PATCH 14/19] working unit tests --- gtsam/slam/InitializePose3.cpp | 33 +++++++++------ gtsam/slam/InitializePose3.h | 2 +- gtsam/slam/tests/testInitializePose3.cpp | 54 ++++++++++++++++++++++-- 3 files changed, 71 insertions(+), 18 deletions(-) diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 7bf29535b..c9eff0536 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -147,7 +147,7 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { /* ************************************************************************* */ // Return the orientations of a graph including only BetweenFactors -Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess) { +Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter) { gttic(InitializePose3_computeOrientationsGradient); // this works on the inverse rotations, according to Tron&Vidal,2011 @@ -183,7 +183,6 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const double rho = 2*a*b; double mu_max = maxNodeDeg * rho; double stepsize = 2/mu_max; // = 1/(a b dG) - size_t maxIter = 1; // gradient iterations for(size_t it=0; it < maxIter; it++){ @@ -194,8 +193,8 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const double maxGrad = 0; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { Key key = key_value.key; - grad.at(key) = Vector3::Zero(); // reset gradient - + //std::cout << "---------------------------key " << DefaultKeyFormatter(key) << std::endl; + Vector gradKey = Vector3::Zero(); // collect the gradient for each edge incident on key BOOST_FOREACH(const size_t& factorId, adjEdgesMap.at(key)){ Rot3 Rij = factorId2RotMap.at(factorId); @@ -203,18 +202,21 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const if( key == (pose3Graph.at(factorId))->keys()[0] ){ Key key1 = (pose3Graph.at(factorId))->keys()[1]; Rot3 Rj = inverseRot.at(key1); - grad.at(key) = grad.at(key) + stepsize * gradientTron(Ri, Rij * Rj, a, b); + gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b); + //std::cout << "key1 " << DefaultKeyFormatter(key1) << " gradientTron(Ri, Rij * Rj, a, b) \n " << gradientTron(Ri, Rij * Rj, a, b) << std::endl; }else if( key == (pose3Graph.at(factorId))->keys()[1] ){ Key key0 = (pose3Graph.at(factorId))->keys()[0]; Rot3 Rj = inverseRot.at(key0); - grad.at(key) = grad.at(key) + stepsize * gradientTron(Ri, Rij.inverse() * Rj, a, b); + gradKey = gradKey + gradientTron(Ri, Rij.between(Rj), a, b); + //std::cout << "key0 " << DefaultKeyFormatter(key0) << " gradientTron(Ri, Rij.inverse() * Rj, a, b) \n " << gradientTron(Ri, Rij.between(Rj), a, b) << std::endl; }else{ std::cout << "Error in gradient computation" << std::endl; } } // end of i-th gradient computation + grad.at(key) = stepsize * gradKey; - double normGradKey = (grad.at(key)).norm(); - std::cout << "key " << DefaultKeyFormatter(key) <<" grad " << grad.at(key) << std::endl; + double normGradKey = (gradKey).norm(); + std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl; if(normGradKey>maxGrad) maxGrad = normGradKey; } // end of loop over nodes @@ -223,6 +225,8 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const // update estimates inverseRot = inverseRot.retract(grad); + inverseRot.print("inverseRot \n"); + ////////////////////////////////////////////////////////////////////////// // check stopping condition if (it>20 && maxGrad < 5e-3) @@ -230,12 +234,12 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const } // enf of gradient iterations // Return correct rotations - Values estimateRot; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { - Key key = key_value.key; - const Rot3& R = inverseRot.at(key); - estimateRot.insert(key, R.inverse()); - } + Values estimateRot; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { + Key key = key_value.key; + const Rot3& R = inverseRot.at(key); + estimateRot.insert(key, R.inverse()); + } return estimateRot; } @@ -275,6 +279,7 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, /* ************************************************************************* */ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) { Vector3 logRot = Rot3::Logmap(R1.between(R2)); + if(logRot.norm()<1e-4){ // some tolerance Rot3 R1pert = R1.compose( Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01)) ); // some perturbation logRot = Rot3::Logmap(R1pert.between(R2)); diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h index 2f6fb842e..1bb9a8e31 100644 --- a/gtsam/slam/InitializePose3.h +++ b/gtsam/slam/InitializePose3.h @@ -39,7 +39,7 @@ GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph); -GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess); +GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, size_t maxIter = 1000); GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph); diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index 2eb0b3e67..c3aafa9a3 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -160,15 +160,63 @@ TEST( InitializePose3, singleGradient ) { } /* *************************************************************************** */ +TEST( InitializePose3, iterationGradient ) { + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); + + // Wrong initial guess - initialization should fix the rotations + Rot3 Rpert = Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01)); + Values givenPoses; + givenPoses.insert(x0,simple::pose0); + givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) )); + givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) )); + givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) )); + + size_t maxIter = 1; // test gradient at the first iteration + Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter); + + const Key keyAnchor = symbol('Z', 9999999); + Matrix Mz = (Matrix(3,3) << 0.999993962808392, -0.002454045561375, 0.002460082752984, + 0.002460082752984, 0.999993962808392, -0.002454045561375, + -0.002454045561375, 0.002460082752984, 0.999993962808392); + Rot3 RzExpected = Rot3(Mz); + EXPECT(assert_equal(RzExpected, orientations.at(keyAnchor), 1e-6)); + + Matrix M0 = (Matrix(3,3) << 0.999344848920642, -0.036021919324717, 0.003506317718352, + 0.036032601656108, 0.999346013522419, -0.003032634950127, + -0.003394783302464, 0.003156989865691, 0.999989254372924); + Rot3 R0Expected = Rot3(M0); + EXPECT(assert_equal(R0Expected, orientations.at(x0), 1e-5)); + + Matrix M1 = (Matrix(3,3) << 0.999905367545392, -0.010866391403031, 0.008436675399114, + 0.010943459008004, 0.999898317528125, -0.009143047050380, + -0.008336465609239, 0.009234508232789, 0.999922610604863); + Rot3 R1Expected = Rot3(M1); + EXPECT(assert_equal(R1Expected, orientations.at(x1), 1e-5)); + + Matrix M2 = (Matrix(3,3) << 0.998936644682875, 0.045376417678595, -0.008158469732553, + -0.045306446926148, 0.998936408933058, 0.008566024448664, + 0.008538487960253, -0.008187284445083, 0.999930028850403); + Rot3 R2Expected = Rot3(M2); + EXPECT(assert_equal(R2Expected, orientations.at(x2), 1e-5)); + + Matrix M3 = (Matrix(3,3) << 0.999898767273093, -0.010834701971459, 0.009223038487275, + 0.010911315499947, 0.999906044037258, -0.008297366559388, + -0.009132272433995, 0.008397162077148, 0.999923041673329); + Rot3 R3Expected = Rot3(M3); + EXPECT(assert_equal(R3Expected, orientations.at(x3), 1e-5)); +} + +/* *************************************************************************** * TEST( InitializePose3, orientationsGradient ) { NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); // Wrong initial guess - initialization should fix the rotations + Rot3 Rpert = Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01)); Values givenPoses; givenPoses.insert(x0,simple::pose0); - givenPoses.insert(x1,simple::pose0); - givenPoses.insert(x2,simple::pose0); - givenPoses.insert(x3,simple::pose0); + givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) )); + givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) )); + givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) )); Values initial = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses); const Key keyAnchor = symbol('Z', 9999999); From b19e758f5abb90e5dc0593bfea499c6686b3e19f Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 3 Sep 2014 15:30:41 -0400 Subject: [PATCH 15/19] no test failures --- gtsam/slam/tests/testInitializePose3.cpp | 46 ++++++++++++++++++------ 1 file changed, 36 insertions(+), 10 deletions(-) diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index c3aafa9a3..d17a49bb9 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -206,7 +206,7 @@ TEST( InitializePose3, iterationGradient ) { EXPECT(assert_equal(R3Expected, orientations.at(x3), 1e-5)); } -/* *************************************************************************** * +/* *************************************************************************** */ TEST( InitializePose3, orientationsGradient ) { NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); @@ -217,18 +217,44 @@ TEST( InitializePose3, orientationsGradient ) { givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) )); givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) )); givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) )); - Values initial = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses); + // do 10 gradient iterations + Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10); + + // const Key keyAnchor = symbol('Z', 9999999); + // givenPoses.insert(keyAnchor,simple::pose0); + // string g2oFile = "/home/aspn/Desktop/toyExample.g2o"; + // writeG2o(pose3Graph, givenPoses, g2oFile); const Key keyAnchor = symbol('Z', 9999999); - givenPoses.insert(keyAnchor,simple::pose0); - string g2oFile = "/home/aspn/Desktop/toyExample.g2o"; - writeG2o(pose3Graph, givenPoses, g2oFile); + Matrix Mz = (Matrix(3,3) << 0.983348036379704, -0.181672808000167, 0.004650825895948, + 0.181688570817424, 0.983350839452522, -0.003223318529546, + -0.003987804220587, 0.004014645856811, 0.999983989889910); + Rot3 RzExpected = Rot3(Mz); + EXPECT(assert_equal(RzExpected, orientations.at(keyAnchor), 1e-4)); - // 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)); + Matrix M0 = (Matrix(3,3) << 0.946965375724015, -0.321288672646614, 0.005492359133630, + 0.321308000189570, 0.946969747977338, -0.003076593882320, + -0.004212623179851, 0.004678166811270, 0.999980184084280); + Rot3 R0Expected = Rot3(M0); + EXPECT(assert_equal(R0Expected, orientations.at(x0), 1e-4)); + + Matrix M1 = (Matrix(3,3) << 0.998716651908197, -0.050557818750367, 0.002992685163299, + 0.050577444118653, 0.998696360370342, -0.006892164352146, + -0.002640330984207, 0.007034681672788, 0.999971770554577); + Rot3 R1Expected = Rot3(M1); + EXPECT(assert_equal(R1Expected, orientations.at(x1), 1e-4)); + + Matrix M2 = (Matrix(3,3) << 0.837033946147607, 0.547150466557869, 0.000734807505930, + -0.547150098128722, 0.837029489230720, 0.002899012916604, + 0.000971140718506, -0.002828622220494, 0.999995527881019); + Rot3 R2Expected = Rot3(M2); + EXPECT(assert_equal(R2Expected, orientations.at(x2), 1e-3)); + + Matrix M3 = (Matrix(3,3) << 0.999422151199602, -0.033471815469964, 0.005916186331164, + 0.033474965560969, 0.999439461971174, -0.000434206705656, + -0.005898336397012, 0.000631999933520, 0.999982404947123); + Rot3 R3Expected = Rot3(M3); + EXPECT(assert_equal(R3Expected, orientations.at(x3), 1e-4)); } /* *************************************************************************** */ From 316ce41a22fb8f36d2d90e2b126e32773278525f Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 3 Sep 2014 15:31:30 -0400 Subject: [PATCH 16/19] removed comments --- gtsam/slam/InitializePose3.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index c9eff0536..af3e24da0 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -188,8 +188,8 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const for(size_t it=0; it < maxIter; it++){ ////////////////////////////////////////////////////////////////////////// // compute the gradient at each node - std::cout << "it " << it <<" b " << b <<" f0 " << f0 <<" a " << a - <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; + //std::cout << "it " << it <<" b " << b <<" f0 " << f0 <<" a " << a + // <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; double maxGrad = 0; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { Key key = key_value.key; @@ -216,7 +216,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const grad.at(key) = stepsize * gradKey; double normGradKey = (gradKey).norm(); - std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl; + //std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl; if(normGradKey>maxGrad) maxGrad = normGradKey; } // end of loop over nodes @@ -225,8 +225,6 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const // update estimates inverseRot = inverseRot.retract(grad); - inverseRot.print("inverseRot \n"); - ////////////////////////////////////////////////////////////////////////// // check stopping condition if (it>20 && maxGrad < 5e-3) From 544e56f6341ee9079b5c0049714955e5933ee46f Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 3 Sep 2014 15:40:57 -0400 Subject: [PATCH 17/19] added examples --- ...ose3SLAMExample_initializePose3Chordal.cpp | 93 +++++++++++++++++++ ...e3SLAMExample_initializePose3Gradient.cpp} | 28 +----- ...nly.cpp => Pose3SLAMExample_rotOnlyGN.cpp} | 0 gtsam/slam/InitializePose3.cpp | 23 +++-- 4 files changed, 108 insertions(+), 36 deletions(-) create mode 100644 examples/Pose3SLAMExample_initializePose3Chordal.cpp rename examples/{Pose3SLAMExample_initializePose3.cpp => Pose3SLAMExample_initializePose3Gradient.cpp} (65%) rename examples/{Pose3SLAMExample_rotOnly.cpp => Pose3SLAMExample_rotOnlyGN.cpp} (100%) diff --git a/examples/Pose3SLAMExample_initializePose3Chordal.cpp b/examples/Pose3SLAMExample_initializePose3Chordal.cpp new file mode 100644 index 000000000..03d11d795 --- /dev/null +++ b/examples/Pose3SLAMExample_initializePose3Chordal.cpp @@ -0,0 +1,93 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Pose3SLAMExample_initializePose3.cpp + * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3 + * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o + * @date Aug 25, 2014 + * @author Luca Carlone + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main(const int argc, const char *argv[]) { + + // Read graph from file + string g2oFile; + if (argc < 2) + g2oFile = findExampleDataFile("pose3example.txt"); + else + g2oFile = argv[1]; + + NonlinearFactorGraph::shared_ptr graph; + Values::shared_ptr initial; + bool is3D = true; + boost::tie(graph, initial) = readG2o(g2oFile, is3D); + + // Add prior on the first key + NonlinearFactorGraph graphWithPrior = *graph; + noiseModel::Diagonal::shared_ptr priorModel = // + noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)); + Key firstKey = 0; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) { + std::cout << "Adding prior to g2o file " << std::endl; + firstKey = key_value.key; + graphWithPrior.add(PriorFactor(firstKey, Pose3(), priorModel)); + break; + } + + std::cout << "Initializing Pose3" << std::endl; + Values initialization = InitializePose3::initialize(graphWithPrior); + std::cout << "done!" << std::endl; + + if (argc < 3) { + initialization.print("initialization"); + } else { + const string outputFile = argv[2]; + std::cout << "Writing results to file: " << outputFile << std::endl; + writeG2o(*graph, initialization, outputFile); + std::cout << "done! " << std::endl; + + // This should be deleted: only for debug + // if (argc == 4){ + // const string inputFileRewritten = argv[3]; + // std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl; + // // Additional: rewrite input with simplified keys 0,1,... + // Values simpleInitial; + // BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) { + // Key key = key_value.key - firstKey; + // simpleInitial.insert(key, initial->at(key_value.key)); + // } + // NonlinearFactorGraph simpleGraph; + // BOOST_FOREACH(const boost::shared_ptr& factor, *graph) { + // boost::shared_ptr > pose3Between = + // boost::dynamic_pointer_cast >(factor); + // if (pose3Between){ + // Key key1 = pose3Between->key1() - firstKey; + // Key key2 = pose3Between->key2() - firstKey; + // NonlinearFactor::shared_ptr simpleFactor( + // new BetweenFactor(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel())); + // simpleGraph.add(simpleFactor); + // } + // } + // writeG2o(simpleGraph, simpleInitial, inputFileRewritten); + // } + } + return 0; +} diff --git a/examples/Pose3SLAMExample_initializePose3.cpp b/examples/Pose3SLAMExample_initializePose3Gradient.cpp similarity index 65% rename from examples/Pose3SLAMExample_initializePose3.cpp rename to examples/Pose3SLAMExample_initializePose3Gradient.cpp index bd46bb917..eac964075 100644 --- a/examples/Pose3SLAMExample_initializePose3.cpp +++ b/examples/Pose3SLAMExample_initializePose3Gradient.cpp @@ -53,7 +53,8 @@ int main(const int argc, const char *argv[]) { } std::cout << "Initializing Pose3" << std::endl; - Values initialization = InitializePose3::initialize(graphWithPrior); + bool useGradient = true; + Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient); std::cout << "done!" << std::endl; if (argc < 3) { @@ -63,31 +64,6 @@ int main(const int argc, const char *argv[]) { std::cout << "Writing results to file: " << outputFile << std::endl; writeG2o(*graph, initialization, outputFile); std::cout << "done! " << std::endl; - - // This should be deleted: only for debug - if (argc == 4){ - const string inputFileRewritten = argv[3]; - std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl; - // Additional: rewrite input with simplified keys 0,1,... - Values simpleInitial; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) { - Key key = key_value.key - firstKey; - simpleInitial.insert(key, initial->at(key_value.key)); - } - NonlinearFactorGraph simpleGraph; - BOOST_FOREACH(const boost::shared_ptr& factor, *graph) { - boost::shared_ptr > pose3Between = - boost::dynamic_pointer_cast >(factor); - if (pose3Between){ - Key key1 = pose3Between->key1() - firstKey; - Key key2 = pose3Between->key2() - firstKey; - NonlinearFactor::shared_ptr simpleFactor( - new BetweenFactor(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel())); - simpleGraph.add(simpleFactor); - } - } - writeG2o(simpleGraph, simpleInitial, inputFileRewritten); - } } return 0; } diff --git a/examples/Pose3SLAMExample_rotOnly.cpp b/examples/Pose3SLAMExample_rotOnlyGN.cpp similarity index 100% rename from examples/Pose3SLAMExample_rotOnly.cpp rename to examples/Pose3SLAMExample_rotOnlyGN.cpp diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index af3e24da0..8b3f54644 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -366,16 +366,19 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, b else orientations = computeOrientationsChordal(pose3Graph); - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, orientations) { - Key key = key_value.key; - if (key != keyAnchor) { - const Point3& pos = givenGuess.at(key).translation(); - const Rot3& rot = orientations.at(key); - Pose3 initializedPoses = Pose3(rot, pos); - initialValues.insert(key, initializedPoses); - } - } - return initialValues; + // Compute the full poses (1 GN iteration on full poses) + return computePoses(pose3Graph, orientations); + + // BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, orientations) { + // Key key = key_value.key; + // if (key != keyAnchor) { + // const Point3& pos = givenGuess.at(key).translation(); + // const Rot3& rot = orientations.at(key); + // Pose3 initializedPoses = Pose3(rot, pos); + // initialValues.insert(key, initializedPoses); + // } + // } + // return initialValues; } } // end of namespace lago From 08a6bff803b960be1663ac732c501c65fe0125b3 Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 3 Sep 2014 20:18:46 -0400 Subject: [PATCH 18/19] fixed issues in dataset and added comments --- examples/Pose3SLAMExample_g2o.cpp | 75 +++++++++++++++++++ ...ose3SLAMExample_initializePose3Chordal.cpp | 2 +- ...se3SLAMExample_initializePose3Gradient.cpp | 4 +- gtsam/nonlinear/NonlinearOptimizer.cpp | 3 + gtsam/slam/InitializePose3.cpp | 12 ++- gtsam/slam/dataset.cpp | 15 +++- gtsam/slam/tests/testInitializePose3.cpp | 43 +++-------- 7 files changed, 115 insertions(+), 39 deletions(-) create mode 100644 examples/Pose3SLAMExample_g2o.cpp diff --git a/examples/Pose3SLAMExample_g2o.cpp b/examples/Pose3SLAMExample_g2o.cpp new file mode 100644 index 000000000..ddb900901 --- /dev/null +++ b/examples/Pose3SLAMExample_g2o.cpp @@ -0,0 +1,75 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Pose3SLAMExample_initializePose3.cpp + * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3 + * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o + * @date Aug 25, 2014 + * @author Luca Carlone + */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +int main(const int argc, const char *argv[]) { + + // Read graph from file + string g2oFile; + if (argc < 2) + g2oFile = findExampleDataFile("pose3example.txt"); + else + g2oFile = argv[1]; + + NonlinearFactorGraph::shared_ptr graph; + Values::shared_ptr initial; + bool is3D = true; + boost::tie(graph, initial) = readG2o(g2oFile, is3D); + + // Add prior on the first key + NonlinearFactorGraph graphWithPrior = *graph; + noiseModel::Diagonal::shared_ptr priorModel = // + noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)); + Key firstKey = 0; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) { + std::cout << "Adding prior to g2o file " << std::endl; + firstKey = key_value.key; + graphWithPrior.add(PriorFactor(firstKey, Pose3(), priorModel)); + break; + } + + std::cout << "Optimizing the factor graph" << std::endl; + GaussNewtonParams params; + params.setVerbosity("TERMINATION"); + GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params); + Values result = optimizer.optimize(); + std::cout << "Optimization complete" << std::endl; + + std::cout << "initial error=" <error(*initial)<< std::endl; + std::cout << "final error=" <error(result)<< std::endl; + + if (argc < 3) { + result.print("result"); + } else { + const string outputFile = argv[2]; + std::cout << "Writing results to file: " << outputFile << std::endl; + writeG2o(*graph, result, outputFile); + std::cout << "done! " << std::endl; + } + return 0; +} diff --git a/examples/Pose3SLAMExample_initializePose3Chordal.cpp b/examples/Pose3SLAMExample_initializePose3Chordal.cpp index 03d11d795..e52534b93 100644 --- a/examples/Pose3SLAMExample_initializePose3Chordal.cpp +++ b/examples/Pose3SLAMExample_initializePose3Chordal.cpp @@ -52,7 +52,7 @@ int main(const int argc, const char *argv[]) { break; } - std::cout << "Initializing Pose3" << std::endl; + std::cout << "Initializing Pose3 - chordal relaxation" << std::endl; Values initialization = InitializePose3::initialize(graphWithPrior); std::cout << "done!" << std::endl; diff --git a/examples/Pose3SLAMExample_initializePose3Gradient.cpp b/examples/Pose3SLAMExample_initializePose3Gradient.cpp index eac964075..f2f549c1a 100644 --- a/examples/Pose3SLAMExample_initializePose3Gradient.cpp +++ b/examples/Pose3SLAMExample_initializePose3Gradient.cpp @@ -43,7 +43,7 @@ int main(const int argc, const char *argv[]) { // Add prior on the first key NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // - noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)); + noiseModel::Diagonal::Variances((Vector(6) << 0.1, 0.1, 0.1, 0.01, 0.01, 0.01)); Key firstKey = 0; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) { std::cout << "Adding prior to g2o file " << std::endl; @@ -52,7 +52,7 @@ int main(const int argc, const char *argv[]) { break; } - std::cout << "Initializing Pose3" << std::endl; + std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl; bool useGradient = true; Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient); std::cout << "done!" << std::endl; diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index b4498bee4..42618bbfb 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -76,6 +76,9 @@ void NonlinearOptimizer::defaultOptimize() { !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol, currentError, this->error(), params.verbosity)); + if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) + cout << "Number of iterations: " << this->iterations() << endl; + // Printing if verbose if (params.verbosity >= NonlinearOptimizerParams::TERMINATION && this->iterations() >= params.maxIterations) diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 8b3f54644..f18380b20 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -185,7 +185,8 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const double stepsize = 2/mu_max; // = 1/(a b dG) // gradient iterations - for(size_t it=0; it < maxIter; it++){ + size_t it; + for(it=0; it < maxIter; it++){ ////////////////////////////////////////////////////////////////////////// // compute the gradient at each node //std::cout << "it " << it <<" b " << b <<" f0 " << f0 <<" a " << a @@ -231,12 +232,17 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const break; } // enf of gradient iterations + std::cout << "nr of gradient iterations " << it << std::endl; + // Return correct rotations + const Rot3& Rref = inverseRot.at(keyAnchor); Values estimateRot; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { Key key = key_value.key; - const Rot3& R = inverseRot.at(key); - estimateRot.insert(key, R.inverse()); + if (key != keyAnchor) { + const Rot3& R = inverseRot.at(key); + estimateRot.insert(key, Rref.compose(R.inverse())); + } } return estimateRot; } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index e8b266b19..a6cc2d3f3 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -439,9 +439,15 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, << p.x() << " " << p.y() << " " << p.z() << " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << " " << R.toQuaternion().w(); + Matrix InfoG2o = eye(6); + InfoG2o.block(0,0,3,3) = Info.block(3,3,3,3); // cov translation + InfoG2o.block(3,3,3,3) = Info.block(0,0,3,3); // cov rotation + InfoG2o.block(0,3,3,3) = Info.block(0,3,3,3); // off diagonal + InfoG2o.block(3,0,3,3) = Info.block(3,0,3,3); // off diagonal + for (int i = 0; i < 6; i++){ for (int j = i; j < 6; j++){ - stream << " " << Info(i, j); + stream << " " << InfoG2o(i, j); } } stream << endl; @@ -524,7 +530,12 @@ GraphAndValues load3D(const string& filename) { m(j, i) = mij; } } - SharedNoiseModel model = noiseModel::Gaussian::Information(m); + Matrix mgtsam = eye(6); + mgtsam.block(0,0,3,3) = m.block(3,3,3,3); // cov rotation + mgtsam.block(3,3,3,3) = m.block(0,0,3,3); // cov translation + mgtsam.block(0,3,3,3) = m.block(0,3,3,3); // off diagonal + mgtsam.block(3,0,3,3) = m.block(3,0,3,3); // off diagonal + SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam); NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, Pose3(R,t), model)); graph->push_back(factor); } diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index d17a49bb9..76e278f28 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -123,25 +123,6 @@ TEST( InitializePose3, orientationsGradientSymbolicGraph ) { EXPECT_DOUBLES_EQUAL(adjEdgesMap.size(), 5, 1e-9); } -/* *************************************************************************** * -TEST( InitializePose3, orientationsCheckGradient ) { - NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); - - // Wrong initial guess - initialization should fix the rotations - Values givenPoses; - givenPoses.insert(x0,simple::pose0); - givenPoses.insert(x1,simple::pose0); - givenPoses.insert(x2,simple::pose0); - givenPoses.insert(x3,simple::pose0); - Values initial = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses); - - // 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, singleGradient ) { Rot3 R1 = Rot3(); @@ -174,12 +155,12 @@ TEST( InitializePose3, iterationGradient ) { size_t maxIter = 1; // test gradient at the first iteration Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter); - const Key keyAnchor = symbol('Z', 9999999); - Matrix Mz = (Matrix(3,3) << 0.999993962808392, -0.002454045561375, 0.002460082752984, - 0.002460082752984, 0.999993962808392, -0.002454045561375, - -0.002454045561375, 0.002460082752984, 0.999993962808392); - Rot3 RzExpected = Rot3(Mz); - EXPECT(assert_equal(RzExpected, orientations.at(keyAnchor), 1e-6)); + // const Key keyAnchor = symbol('Z', 9999999); + // Matrix Mz = (Matrix(3,3) << 0.999993962808392, -0.002454045561375, 0.002460082752984, + // 0.002460082752984, 0.999993962808392, -0.002454045561375, + // -0.002454045561375, 0.002460082752984, 0.999993962808392); + // Rot3 RzExpected = Rot3(Mz); + // EXPECT(assert_equal(RzExpected, orientations.at(keyAnchor), 1e-6)); Matrix M0 = (Matrix(3,3) << 0.999344848920642, -0.036021919324717, 0.003506317718352, 0.036032601656108, 0.999346013522419, -0.003032634950127, @@ -225,12 +206,12 @@ TEST( InitializePose3, orientationsGradient ) { // string g2oFile = "/home/aspn/Desktop/toyExample.g2o"; // writeG2o(pose3Graph, givenPoses, g2oFile); - const Key keyAnchor = symbol('Z', 9999999); - Matrix Mz = (Matrix(3,3) << 0.983348036379704, -0.181672808000167, 0.004650825895948, - 0.181688570817424, 0.983350839452522, -0.003223318529546, - -0.003987804220587, 0.004014645856811, 0.999983989889910); - Rot3 RzExpected = Rot3(Mz); - EXPECT(assert_equal(RzExpected, orientations.at(keyAnchor), 1e-4)); + // const Key keyAnchor = symbol('Z', 9999999); + // Matrix Mz = (Matrix(3,3) << 0.983348036379704, -0.181672808000167, 0.004650825895948, + // 0.181688570817424, 0.983350839452522, -0.003223318529546, + // -0.003987804220587, 0.004014645856811, 0.999983989889910); + // Rot3 RzExpected = Rot3(Mz); + // EXPECT(assert_equal(RzExpected, orientations.at(keyAnchor), 1e-4)); Matrix M0 = (Matrix(3,3) << 0.946965375724015, -0.321288672646614, 0.005492359133630, 0.321308000189570, 0.946969747977338, -0.003076593882320, From 8444680e6e7fbc7679350990a064fd10bf51fb6f Mon Sep 17 00:00:00 2001 From: Luca Date: Wed, 3 Sep 2014 20:20:06 -0400 Subject: [PATCH 19/19] fixed unit test --- gtsam/slam/InitializePose3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index f18380b20..c0e2ef6d9 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -235,7 +235,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const std::cout << "nr of gradient iterations " << it << std::endl; // Return correct rotations - const Rot3& Rref = inverseRot.at(keyAnchor); + const Rot3& Rref = Rot3(); // inverseRot.at(keyAnchor); Values estimateRot; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { Key key = key_value.key;