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,