From 41bb99b48af0d565d7f298a19feedbcd99f43f85 Mon Sep 17 00:00:00 2001 From: Luca Date: Sat, 6 Sep 2014 10:57:22 -0400 Subject: [PATCH 1/6] fixed gradient --- ...se3SLAMExample_initializePose3Gradient.cpp | 3 +++ gtsam/slam/InitializePose3.cpp | 20 ++++++++++++++----- gtsam/slam/InitializePose3.h | 2 +- 3 files changed, 19 insertions(+), 6 deletions(-) diff --git a/examples/Pose3SLAMExample_initializePose3Gradient.cpp b/examples/Pose3SLAMExample_initializePose3Gradient.cpp index f2f549c1a..898986b0a 100644 --- a/examples/Pose3SLAMExample_initializePose3Gradient.cpp +++ b/examples/Pose3SLAMExample_initializePose3Gradient.cpp @@ -57,6 +57,9 @@ int main(const int argc, const char *argv[]) { Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient); std::cout << "done!" << std::endl; + std::cout << "initial error=" <error(*initial)<< std::endl; + std::cout << "initialization error=" <error(initialization)<< std::endl; + if (argc < 3) { initialization.print("initialization"); } else { diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index c0e2ef6d9..3fc082d92 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -184,6 +184,8 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const double mu_max = maxNodeDeg * rho; double stepsize = 2/mu_max; // = 1/(a b dG) + std::cout <<" b " << b <<" f0 " << f0 <<" a " << a <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; + // gradient iterations size_t it; for(it=0; it < maxIter; it++){ @@ -217,7 +219,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 @@ -235,7 +237,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const std::cout << "nr of gradient iterations " << it << std::endl; // Return correct rotations - const Rot3& Rref = Rot3(); // inverseRot.at(keyAnchor); + const Rot3& Rref = inverseRot.at(keyAnchor); // This will be set to the identity as so far we included no prior Values estimateRot; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { Key key = key_value.key; @@ -284,13 +286,19 @@ 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 + double th = logRot.norm(); + if(th < 1e-4 || th != th){ // the second case means that th = nan (logRot does not work well for +/-pi) Rot3 R1pert = R1.compose( Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01)) ); // some perturbation logRot = Rot3::Logmap(R1pert.between(R2)); + th = logRot.norm(); } - double th = logRot.norm(); - if (th > 1e-5) + // exclude small or invalid rotations + if (th > 1e-5 || th == th){ // the second case means that th = nan (logRot does not work well for +/-pi) logRot = logRot / th; + }else{ + logRot = Vector3::Zero(); + th = 0.0; + } double fdot = a*b*th*exp(-b*th); return fdot*logRot; @@ -372,6 +380,8 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, b else orientations = computeOrientationsChordal(pose3Graph); +// orientations.print("orientations\n"); + // Compute the full poses (1 GN iteration on full poses) return computePoses(pose3Graph, orientations); diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h index 1bb9a8e31..a19933bb5 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, size_t maxIter = 1000); +GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, size_t maxIter = 10000); GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph); From e3ec28ebeb3e557a70375c4778a45a401230a00b Mon Sep 17 00:00:00 2001 From: Luca Date: Sat, 6 Sep 2014 18:27:07 -0400 Subject: [PATCH 2/6] added debug info for gradient code --- gtsam/slam/InitializePose3.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 3fc082d92..f1415a8fa 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -185,7 +185,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const double stepsize = 2/mu_max; // = 1/(a b dG) std::cout <<" b " << b <<" f0 " << f0 <<" a " << a <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; - + double maxGrad; // gradient iterations size_t it; for(it=0; it < maxIter; it++){ @@ -193,7 +193,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const // 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; + maxGrad = 0; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { Key key = key_value.key; //std::cout << "---------------------------key " << DefaultKeyFormatter(key) << std::endl; @@ -234,7 +234,7 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const break; } // enf of gradient iterations - std::cout << "nr of gradient iterations " << it << std::endl; + std::cout << "nr of gradient iterations " << it << "maxGrad " << maxGrad << std::endl; // Return correct rotations const Rot3& Rref = inverseRot.at(keyAnchor); // This will be set to the identity as so far we included no prior @@ -287,13 +287,13 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl Vector3 logRot = Rot3::Logmap(R1.between(R2)); double th = logRot.norm(); - if(th < 1e-4 || th != th){ // the second case means that th = nan (logRot does not work well for +/-pi) + if(th != th){ // the second case means that th = nan (logRot does not work well for +/-pi) Rot3 R1pert = R1.compose( Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01)) ); // some perturbation logRot = Rot3::Logmap(R1pert.between(R2)); th = logRot.norm(); } // exclude small or invalid rotations - if (th > 1e-5 || th == th){ // the second case means that th = nan (logRot does not work well for +/-pi) + if (th > 1e-5 && th == th){ // nonzero valid rotations logRot = logRot / th; }else{ logRot = Vector3::Zero(); @@ -334,7 +334,13 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { // Create optimizer GaussNewtonParams params; - params.maxIterations = 1; + bool singleIter = false; + if(singleIter){ + params.maxIterations = 1; + }else{ + std::cout << " \n\n\n\n performing more than 1 GN iterations \n\n\n" < Date: Wed, 10 Sep 2014 09:35:49 -0400 Subject: [PATCH 3/6] added possibility to specify number of iterations --- examples/Pose2SLAMExample_g2o.cpp | 12 +++- examples/Pose3SLAMExample_changeKeys.cpp | 73 ++++++++++++++++++++++++ gtsam/slam/InitializePose3.cpp | 2 +- 3 files changed, 85 insertions(+), 2 deletions(-) create mode 100644 examples/Pose3SLAMExample_changeKeys.cpp diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index b46a53198..8f1a53a66 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -45,11 +45,21 @@ int main(const int argc, const char *argv[]) { noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + GaussNewtonParams params; + params.setVerbosity("TERMINATION"); + if (argc == 4) { + params.maxIterations = atoi(argv[3]); + std::cout << "User required to perform " << params.maxIterations << " iterations "<< std::endl; + } + std::cout << "Optimizing the factor graph" << std::endl; - GaussNewtonOptimizer optimizer(graphWithPrior, *initial); + 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 { diff --git a/examples/Pose3SLAMExample_changeKeys.cpp b/examples/Pose3SLAMExample_changeKeys.cpp new file mode 100644 index 000000000..1e28c2097 --- /dev/null +++ b/examples/Pose3SLAMExample_changeKeys.cpp @@ -0,0 +1,73 @@ +/* ---------------------------------------------------------------------------- + + * 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 rewritted.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); + + Key firstKey = 8646911284551352320; + + std::cout << "Using reference key: " << firstKey << std::endl; + + if (argc < 3) { + std::cout << "Please provide output file to write " << std::endl; + } else { + const string inputFileRewritten = argv[2]; + 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/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index f1415a8fa..c571cd8e7 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -334,7 +334,7 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { // Create optimizer GaussNewtonParams params; - bool singleIter = false; + bool singleIter = true; if(singleIter){ params.maxIterations = 1; }else{ From 7322a74bbdf356d7401daaa7558e74eb7f42a848 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 11 Sep 2014 12:05:36 -0400 Subject: [PATCH 4/6] nice and clean unit tests for gradient initialization --- .cproject | 24 ++++++++++-- examples/Data/simpleGraph10gradIter.txt | 11 ++++++ gtsam/slam/InitializePose3.cpp | 9 +++-- gtsam/slam/InitializePose3.h | 3 +- gtsam/slam/tests/testInitializePose3.cpp | 50 ++++++++---------------- 5 files changed, 55 insertions(+), 42 deletions(-) create mode 100644 examples/Data/simpleGraph10gradIter.txt diff --git a/.cproject b/.cproject index 1b4d0b1ec..90e48745b 100644 --- a/.cproject +++ b/.cproject @@ -788,18 +788,18 @@ true true - + make -j5 - testGaussianFactorGraph.run + testGaussianFactorGraphUnordered.run true true true - + make -j5 - testGaussianBayesNet.run + testGaussianBayesNetUnordered.run true true true @@ -2631,6 +2631,22 @@ true true + + make + -j5 + testGPSFactor.run + true + true + true + + + make + -j5 + testGaussMarkov1stOrderFactor.run + true + true + true + make -j5 diff --git a/examples/Data/simpleGraph10gradIter.txt b/examples/Data/simpleGraph10gradIter.txt new file mode 100644 index 000000000..c50171468 --- /dev/null +++ b/examples/Data/simpleGraph10gradIter.txt @@ -0,0 +1,11 @@ +VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.0008187 0.0011723 0.0895466 0.9959816 +VERTEX_SE3:QUAT 1 0.000000 -0.000000 0.000000 0.0010673 0.0015636 0.1606931 0.9870026 +VERTEX_SE3:QUAT 2 -0.388822 0.632954 0.001223 0.0029920 0.0014066 0.0258235 0.9996610 +VERTEX_SE3:QUAT 3 -1.143204 0.050638 0.006026 -0.0012800 -0.0002767 -0.2850291 0.9585180 +VERTEX_SE3:QUAT 4 -0.512416 0.486441 0.005171 0.0002681 0.0023574 0.0171476 0.9998502 +EDGE_SE3:QUAT 1 2 1.000000 2.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000 +EDGE_SE3:QUAT 2 3 -0.000000 1.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000 +EDGE_SE3:QUAT 3 4 1.000000 1.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000 +EDGE_SE3:QUAT 3 1 0.000001 2.000000 0.000000 0.0000000 0.0000000 1.0000000 0.0000002 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000 +EDGE_SE3:QUAT 1 4 -1.000000 1.000000 0.000000 0.0000000 0.0000000 -0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000 +EDGE_SE3:QUAT 0 1 0.000000 0.000000 0.000000 0.0000000 0.0000000 0.0000000 1.0000000 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000 diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index c571cd8e7..97c8a541e 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, const size_t maxIter) { +Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter, const bool setRefFrame) { gttic(InitializePose3_computeOrientationsGradient); // this works on the inverse rotations, according to Tron&Vidal,2011 @@ -219,7 +219,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 @@ -243,7 +243,10 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Key key = key_value.key; if (key != keyAnchor) { const Rot3& R = inverseRot.at(key); - estimateRot.insert(key, Rref.compose(R.inverse())); + if(setRefFrame) + estimateRot.insert(key, Rref.compose(R.inverse())); + else + estimateRot.insert(key, R.inverse()); } } return estimateRot; diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h index a19933bb5..dba5ceac3 100644 --- a/gtsam/slam/InitializePose3.h +++ b/gtsam/slam/InitializePose3.h @@ -39,7 +39,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, size_t maxIter = 10000); +GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, + const Values& givenGuess, size_t maxIter = 10000, const bool setRefFrame = true); 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 76e278f28..0cea2cead 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -153,18 +153,12 @@ TEST( InitializePose3, iterationGradient ) { 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); + bool setRefFrame = false; + Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame); - // 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); + Matrix M0 = (Matrix(3,3) << 0.999435813876064, -0.033571481675497, 0.001004768630281, + 0.033572116359134, 0.999436104312325, -0.000621610948719, + -0.000983333645009, 0.000654992453817, 0.999999302019670); Rot3 R0Expected = Rot3(M0); EXPECT(assert_equal(R0Expected, orientations.at(x0), 1e-5)); @@ -199,42 +193,30 @@ TEST( InitializePose3, orientationsGradient ) { givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) )); givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) )); // do 10 gradient iterations - Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10); + bool setRefFrame = false; + Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame); // 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); - // 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 string matlabResultsfile = findExampleDataFile("simpleGraph10gradIter"); + NonlinearFactorGraph::shared_ptr matlabGraph; + Values::shared_ptr matlabValues; + bool is3D = true; + boost::tie(matlabGraph, matlabValues) = readG2o(matlabResultsfile, is3D); - 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); + Rot3 R0Expected = matlabValues->at(1).rotation(); 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); + Rot3 R1Expected = matlabValues->at(2).rotation(); 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); + Rot3 R2Expected = matlabValues->at(3).rotation(); 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); + Rot3 R3Expected = matlabValues->at(4).rotation(); EXPECT(assert_equal(R3Expected, orientations.at(x3), 1e-4)); } From 661862f3e38f41dde28b093fddcbe576f728feec Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 11 Sep 2014 12:08:42 -0400 Subject: [PATCH 5/6] small comment --- examples/Pose3SLAMExample_g2o.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/Pose3SLAMExample_g2o.cpp b/examples/Pose3SLAMExample_g2o.cpp index ddb900901..c2acb8091 100644 --- a/examples/Pose3SLAMExample_g2o.cpp +++ b/examples/Pose3SLAMExample_g2o.cpp @@ -55,7 +55,7 @@ int main(const int argc, const char *argv[]) { std::cout << "Optimizing the factor graph" << std::endl; GaussNewtonParams params; - params.setVerbosity("TERMINATION"); + params.setVerbosity("TERMINATION"); // this will show info about stopping conditions GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params); Values result = optimizer.optimize(); std::cout << "Optimization complete" << std::endl; From 3ad83e6394d4293008d87e1ae292376512ac4f66 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 11 Sep 2014 20:54:46 -0400 Subject: [PATCH 6/6] small improvements --- examples/Pose3SLAMExample_changeKeys.cpp | 25 ++++++++++++++++--- ...ose3SLAMExample_initializePose3Chordal.cpp | 25 ------------------- ...se3SLAMExample_initializePose3Gradient.cpp | 2 +- 3 files changed, 22 insertions(+), 30 deletions(-) diff --git a/examples/Pose3SLAMExample_changeKeys.cpp b/examples/Pose3SLAMExample_changeKeys.cpp index 1e28c2097..ae4ee5a8c 100644 --- a/examples/Pose3SLAMExample_changeKeys.cpp +++ b/examples/Pose3SLAMExample_changeKeys.cpp @@ -12,7 +12,7 @@ /** * @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 rewritted.g2o + * Syntax for the script is ./Pose3SLAMExample_changeKeys input.g2o rewritted.g2o * @date Aug 25, 2014 * @author Luca Carlone */ @@ -40,9 +40,15 @@ int main(const int argc, const char *argv[]) { bool is3D = true; boost::tie(graph, initial) = readG2o(g2oFile, is3D); + bool add = false; Key firstKey = 8646911284551352320; std::cout << "Using reference key: " << firstKey << std::endl; + if(add) + std::cout << "adding key " << std::endl; + else + std::cout << "subtracting key " << std::endl; + if (argc < 3) { std::cout << "Please provide output file to write " << std::endl; @@ -52,7 +58,12 @@ int main(const int argc, const char *argv[]) { // Additional: rewrite input with simplified keys 0,1,... Values simpleInitial; BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) { - Key key = key_value.key + firstKey; + Key key; + if(add) + key = key_value.key + firstKey; + else + key = key_value.key - firstKey; + simpleInitial.insert(key, initial->at(key_value.key)); } NonlinearFactorGraph simpleGraph; @@ -60,8 +71,14 @@ int main(const int argc, const char *argv[]) { boost::shared_ptr > pose3Between = boost::dynamic_pointer_cast >(factor); if (pose3Between){ - Key key1 = pose3Between->key1() + firstKey; - Key key2 = pose3Between->key2() + firstKey; + Key key1, key2; + if(add){ + key1 = pose3Between->key1() + firstKey; + key2 = pose3Between->key2() + firstKey; + }else{ + key1 = pose3Between->key1() - firstKey; + key2 = pose3Between->key2() - firstKey; + } NonlinearFactor::shared_ptr simpleFactor( new BetweenFactor(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel())); simpleGraph.add(simpleFactor); diff --git a/examples/Pose3SLAMExample_initializePose3Chordal.cpp b/examples/Pose3SLAMExample_initializePose3Chordal.cpp index e52534b93..afc66ea1e 100644 --- a/examples/Pose3SLAMExample_initializePose3Chordal.cpp +++ b/examples/Pose3SLAMExample_initializePose3Chordal.cpp @@ -63,31 +63,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_initializePose3Gradient.cpp b/examples/Pose3SLAMExample_initializePose3Gradient.cpp index 898986b0a..9dc410692 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) << 0.1, 0.1, 0.1, 0.01, 0.01, 0.01)); + 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;