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/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..f3f770750 --- /dev/null +++ b/examples/Pose3SLAMExample_changeKeys.cpp @@ -0,0 +1,89 @@ +/* ---------------------------------------------------------------------------- + + * 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_changeKeys input.g2o rewritted.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); + + 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; + } 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; + if(add) + key = key_value.key + firstKey; + else + 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, 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); + } + } + writeG2o(simpleGraph, simpleInitial, inputFileRewritten); + } + return 0; +} diff --git a/examples/Pose3SLAMExample_g2o.cpp b/examples/Pose3SLAMExample_g2o.cpp new file mode 100644 index 000000000..f992c78b1 --- /dev/null +++ b/examples/Pose3SLAMExample_g2o.cpp @@ -0,0 +1,74 @@ +/* ---------------------------------------------------------------------------- + + * 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 << "Optimizing the factor graph" << std::endl; + GaussNewtonParams params; + 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; + + 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 new file mode 100644 index 000000000..afc66ea1e --- /dev/null +++ b/examples/Pose3SLAMExample_initializePose3Chordal.cpp @@ -0,0 +1,68 @@ +/* ---------------------------------------------------------------------------- + + * 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 - chordal relaxation" << 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; +} diff --git a/examples/Pose3SLAMExample_initializePose3Gradient.cpp b/examples/Pose3SLAMExample_initializePose3Gradient.cpp new file mode 100644 index 000000000..9dc410692 --- /dev/null +++ b/examples/Pose3SLAMExample_initializePose3Gradient.cpp @@ -0,0 +1,72 @@ +/* ---------------------------------------------------------------------------- + + * 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 - Riemannian gradient" << std::endl; + bool useGradient = true; + 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 { + 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; +} diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp new file mode 100644 index 000000000..97c8a541e --- /dev/null +++ b/gtsam/slam/InitializePose3.cpp @@ -0,0 +1,410 @@ +/* ---------------------------------------------------------------------------- + + * 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 + +#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 Matrix 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 Rij; + + boost::shared_ptr > pose3Between = + boost::dynamic_pointer_cast >(factor); + if (pose3Between) + 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::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(9) << 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_computeOrientationsChordal); + + 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; + 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); + + 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); + } + } + 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 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, const size_t maxIter, const bool setRefFrame) { + gttic(InitializePose3_computeOrientationsGradient); + + // 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); + inverseRot.insert(key, pose.rotation().inverse()); + } + + // Create the map of edges incident on each node + KeyVectorMap adjEdgesMap; + 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) + + 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++){ + ////////////////////////////////////////////////////////////////////////// + // compute the gradient at each node + //std::cout << "it " << it <<" b " << b <<" f0 " << f0 <<" a " << a + // <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; + maxGrad = 0; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { + Key key = key_value.key; + //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); + Rot3 Ri = inverseRot.at(key); + if( key == (pose3Graph.at(factorId))->keys()[0] ){ + Key key1 = (pose3Graph.at(factorId))->keys()[1]; + Rot3 Rj = inverseRot.at(key1); + 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); + 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 = (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 + + ////////////////////////////////////////////////////////////////////////// + // update estimates + inverseRot = inverseRot.retract(grad); + + ////////////////////////////////////////////////////////////////////////// + // check stopping condition + if (it>20 && maxGrad < 5e-3) + break; + } // enf of gradient iterations + + 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 + Values estimateRot; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) { + Key key = key_value.key; + if (key != keyAnchor) { + const Rot3& R = inverseRot.at(key); + if(setRefFrame) + estimateRot.insert(key, Rref.compose(R.inverse())); + else + estimateRot.insert(key, R.inverse()); + } + } + 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)); + + double th = logRot.norm(); + 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){ // nonzero valid rotations + logRot = logRot / th; + }else{ + logRot = Vector3::Zero(); + th = 0.0; + } + + double fdot = a*b*th*exp(-b*th); + return fdot*logRot; +} + +/* ************************************************************************* */ +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 computeOrientationsChordal(pose3Graph); +} + +///* ************************************************************************* */ +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); + initialPose.insert(keyAnchor, Pose3()); + pose3graph.add(PriorFactor(keyAnchor, Pose3(), priorModel)); + + // Create optimizer + GaussNewtonParams params; + bool singleIter = true; + if(singleIter){ + params.maxIterations = 1; + }else{ + std::cout << " \n\n\n\n performing more than 1 GN iterations \n\n\n" <(key); + estimate.insert(key, pose); + } + } + return estimate; +} + +/* ************************************************************************* */ +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 = computeOrientationsChordal(pose3Graph); + + // Compute the full poses (1 GN iteration on full poses) + return computePoses(pose3Graph, valueRot3); +} + +/* ************************************************************************* */ +Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient) { + Values initialValues; + + // 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); + +// orientations.print("orientations\n"); + + // 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 +} // end of namespace gtsam diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h new file mode 100644 index 000000000..dba5ceac3 --- /dev/null +++ b/gtsam/slam/InitializePose3.h @@ -0,0 +1,59 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#include + +namespace gtsam { + +typedef std::map > KeyVectorMap; +typedef std::map KeyRotMap; + +namespace InitializePose3 { + +GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g); + +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, const bool setRefFrame = true); + +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); + +GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph); + +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 new file mode 100644 index 000000000..0cea2cead --- /dev/null +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -0,0 +1,259 @@ +/* ---------------------------------------------------------------------------- + + * 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 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(R0,p0); +static Pose3 pose1 = Pose3(R1,p1); +static Pose3 pose2 = Pose3(R2,p2); +static Pose3 pose3 = Pose3(R3,p3); + +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 ) { + 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)); + 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, 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, 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, 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 + bool setRefFrame = false; + Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame); + + Matrix M0 = (Matrix(3,3) << 0.999435813876064, -0.033571481675497, 0.001004768630281, + 0.033572116359134, 0.999436104312325, -0.000621610948719, + -0.000983333645009, 0.000654992453817, 0.999999302019670); + 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).compose( Pose3(Rpert,Point3()) )); + givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) )); + givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) )); + // do 10 gradient iterations + 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 string matlabResultsfile = findExampleDataFile("simpleGraph10gradIter"); + NonlinearFactorGraph::shared_ptr matlabGraph; + Values::shared_ptr matlabValues; + bool is3D = true; + boost::tie(matlabGraph, matlabValues) = readG2o(matlabResultsfile, is3D); + + Rot3 R0Expected = matlabValues->at(1).rotation(); + EXPECT(assert_equal(R0Expected, orientations.at(x0), 1e-4)); + + Rot3 R1Expected = matlabValues->at(2).rotation(); + EXPECT(assert_equal(R1Expected, orientations.at(x1), 1e-4)); + + Rot3 R2Expected = matlabValues->at(3).rotation(); + EXPECT(assert_equal(R2Expected, orientations.at(x2), 1e-3)); + + Rot3 R3Expected = matlabValues->at(4).rotation(); + EXPECT(assert_equal(R3Expected, orientations.at(x3), 1e-4)); +} + +/* *************************************************************************** */ +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)); +} + +/* ************************************************************************* */ +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() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ +