diff --git a/.cproject b/.cproject index af6b32cd2..a43d32f58 100644 --- a/.cproject +++ b/.cproject @@ -568,6 +568,7 @@ make + tests/testBayesTree.run true false @@ -575,6 +576,7 @@ make + testBinaryBayesNet.run true false @@ -622,6 +624,7 @@ make + testSymbolicBayesNet.run true false @@ -629,6 +632,7 @@ make + tests/testSymbolicFactor.run true false @@ -636,6 +640,7 @@ make + testSymbolicFactorGraph.run true false @@ -651,6 +656,7 @@ make + tests/testBayesTree true false @@ -728,46 +734,6 @@ true true - - make - -j5 - testValues.run - true - true - true - - - make - -j5 - testOrdering.run - true - true - true - - - make - -j5 - testKey.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j6 -j8 - testWhiteNoiseFactor.run - true - true - true - make -j2 @@ -1114,6 +1080,7 @@ make + testErrors.run true false @@ -1159,6 +1126,14 @@ true true + + make + -j5 + testParticleFactor.run + true + true + true + make -j2 @@ -1239,14 +1214,6 @@ true true - - make - -j5 - testParticleFactor.run - true - true - true - make -j2 @@ -1351,6 +1318,22 @@ true true + + make + -j5 + testImuFactor.run + true + true + true + + + make + -j5 + testCombinedImuFactor.run + true + true + true + make -j2 @@ -1433,7 +1416,6 @@ make - testSimulated2DOriented.run true false @@ -1473,7 +1455,6 @@ make - testSimulated2D.run true false @@ -1481,7 +1462,6 @@ make - testSimulated3D.run true false @@ -1495,22 +1475,6 @@ true true - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - make -j5 @@ -1768,6 +1732,7 @@ cpack + -G DEB true false @@ -1775,6 +1740,7 @@ cpack + -G RPM true false @@ -1782,6 +1748,7 @@ cpack + -G TGZ true false @@ -1789,6 +1756,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2547,6 +2515,7 @@ make + testGraph.run true false @@ -2554,6 +2523,7 @@ make + testJunctionTree.run true false @@ -2561,6 +2531,7 @@ make + testSymbolicBayesNetB.run true false @@ -2830,6 +2801,62 @@ true true + + make + -j5 + Pose2SLAMExample_lago.run + true + true + true + + + make + -j5 + Pose2SLAMExample_g2o.run + true + true + true + + + make + -j5 + testLago.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run + true + true + true + + + make + -j5 + testOrdering.run + true + true + true + + + make + -j5 + testValues.run + true + true + true + + + make + -j5 + testWhiteNoiseFactor.run + true + true + true + make -j4 @@ -2848,7 +2875,6 @@ make - tests/testGaussianISAM2 true false diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index 393bba86d..565a26965 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -18,27 +18,22 @@ * @author Luca Carlone */ -#include -#include -#include #include -#include -#include +#include #include -#include -#include #include -#include using namespace std; using namespace gtsam; +int main(const int argc, const char *argv[]) { -int main(const int argc, const char *argv[]){ - + // Read graph from file + string g2oFile; if (argc < 2) - std::cout << "Please specify: 1st argument: input file (in g2o format) and 2nd argument: output file" << std::endl; - const string g2oFile = argv[1]; + g2oFile = "../../examples/Data/noisyToyGraph.txt"; + else + g2oFile = argv[1]; NonlinearFactorGraph graph; Values initial; @@ -46,7 +41,8 @@ int main(const int argc, const char *argv[]){ // Add prior on the pose having index (key) = 0 NonlinearFactorGraph graphWithPrior = graph; - noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); + noiseModel::Diagonal::shared_ptr priorModel = // + noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); std::cout << "Optimizing the factor graph" << std::endl; @@ -54,10 +50,13 @@ int main(const int argc, const char *argv[]){ Values result = optimizer.optimize(); std::cout << "Optimization complete" << std::endl; - const string outputFile = argv[2]; - std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(outputFile, graph, result); - std::cout << "done! " << std::endl; - + if (argc < 3) { + result.print("result"); + } else { + const string outputFile = argv[2]; + std::cout << "Writing results to file: " << outputFile << std::endl; + writeG2o(outputFile, graph, result); + std::cout << "done! " << std::endl; + } return 0; } diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index d61f1b533..a6340caf7 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -12,33 +12,29 @@ /** * @file Pose2SLAMExample_lago.cpp * @brief A 2D Pose SLAM example that reads input from g2o, and solve the Pose2 problem - * using LAGO (Linear Approximation for Graph Optimization). See class LagoInitializer.h + * using LAGO (Linear Approximation for Graph Optimization). See class lago.h * Output is written on a file, in g2o format * Syntax for the script is ./Pose2SLAMExample_lago input.g2o output.g2o * @date May 15, 2014 * @author Luca Carlone */ -#include -#include -#include +#include #include -#include -#include -#include -#include +#include #include -#include using namespace std; using namespace gtsam; +int main(const int argc, const char *argv[]) { -int main(const int argc, const char *argv[]){ - + // Read graph from file + string g2oFile; if (argc < 2) - std::cout << "Please specify: 1st argument: input file (in g2o format) and 2nd argument: output file" << std::endl; - const string g2oFile = argv[1]; + g2oFile = "../../examples/Data/noisyToyGraph.txt"; + else + g2oFile = argv[1]; NonlinearFactorGraph graph; Values initial; @@ -46,17 +42,23 @@ int main(const int argc, const char *argv[]){ // Add prior on the pose having index (key) = 0 NonlinearFactorGraph graphWithPrior = graph; - noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); + noiseModel::Diagonal::shared_ptr priorModel = // + noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + graphWithPrior.print(); std::cout << "Computing LAGO estimate" << std::endl; - Values estimateLago = initializeLago(graphWithPrior); + Values estimateLago = lago::initialize(graphWithPrior); std::cout << "done!" << std::endl; - const string outputFile = argv[2]; - std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(outputFile, graph, estimateLago); - std::cout << "done! " << std::endl; + if (argc < 3) { + estimateLago.print("estimateLago"); + } else { + const string outputFile = argv[2]; + std::cout << "Writing results to file: " << outputFile << std::endl; + writeG2o(outputFile, graph, estimateLago); + std::cout << "done! " << std::endl; + } return 0; } diff --git a/gtsam/nonlinear/LagoInitializer.h b/gtsam/nonlinear/LagoInitializer.h deleted file mode 100644 index b351365c1..000000000 --- a/gtsam/nonlinear/LagoInitializer.h +++ /dev/null @@ -1,100 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 LagoInitializer.h - * @brief Initialize Pose2 in a factor graph using LAGO - * (Linear Approximation for Graph Optimization). see papers: - * - * L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate - * approximation for planar pose graph optimization, IJRR, 2014. - * - * L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation - * for graph-based simultaneous localization and mapping, RSS, 2011. - * - * @param graph: nonlinear factor graph (can include arbitrary factors but we assume - * that there is a subgraph involving Pose2 and betweenFactors). Also in the current - * version we assume that there is an odometric spanning path (x0->x1, x1->x2, etc) - * and a prior on x0. This assumption can be relaxed by using the extra argument - * useOdometricPath = false, although this part of code is not stable yet. - * @return Values: initial guess from LAGO (only pose2 are initialized) - * - * @author Luca Carlone - * @author Frank Dellaert - * @date May 14, 2014 - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace gtsam { - -typedef std::map key2doubleMap; -const Key keyAnchor = symbol('Z',9999999); -noiseModel::Diagonal::shared_ptr priorOrientationNoise = noiseModel::Diagonal::Variances((Vector(1) << 1e-8)); -noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); - -/* This function computes the cumulative orientation (without wrapping) wrt the root of a spanning tree (tree) - * for a node (nodeKey). The function starts at the nodes and moves towards the root - * summing up the (directed) rotation measurements. Relative measurements are encoded in "deltaThetaMap" - * The root is assumed to have orientation zero. - */ -GTSAM_EXPORT double computeThetaToRoot(const Key nodeKey, const PredecessorMap& tree, - const key2doubleMap& deltaThetaMap, const key2doubleMap& thetaFromRootMap); - -/* This function computes the cumulative orientations (without wrapping) - * for all node wrt the root (root has zero orientation) - */ -GTSAM_EXPORT key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, - const PredecessorMap& tree); - -/* Given a factor graph "g", and a spanning tree "tree", the function selects the nodes belonging to the tree and to g, - * and stores the factor slots corresponding to edges in the tree and to chordsIds wrt this tree - * Also it computes deltaThetaMap which is a fast way to encode relative orientations along the tree: - * for a node key2, s.t. tree[key2]=key1, the values deltaThetaMap[key2] is the relative orientation theta[key2]-theta[key1] - */ -GTSAM_EXPORT void getSymbolicGraph( - /*OUTPUTS*/ std::vector& spanningTreeIds, std::vector& chordsIds, key2doubleMap& deltaThetaMap, - /*INPUTS*/ const PredecessorMap& tree, const NonlinearFactorGraph& g); - -/* Retrieves the deltaTheta and the corresponding noise model from a BetweenFactor */ -GTSAM_EXPORT void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, - Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta); - -/* 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); - -/* Selects the subgraph of betweenFactors and transforms priors into between wrt a fictitious node */ -GTSAM_EXPORT NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph); - -/* Returns the orientations of a graph including only BetweenFactors */ -GTSAM_EXPORT VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true); - -/* LAGO: Returns the orientations of the Pose2 in a generic factor graph */ -GTSAM_EXPORT VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true); - -/* Returns the values for the Pose2 in a generic factor graph */ -GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true); - -/* Only corrects the orientation part in initialGuess */ -GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess); - -} // end of namespace gtsam diff --git a/gtsam/nonlinear/LagoInitializer.cpp b/gtsam/nonlinear/lago.cpp similarity index 53% rename from gtsam/nonlinear/LagoInitializer.cpp rename to gtsam/nonlinear/lago.cpp index 3451beaf3..c2a7d9454 100644 --- a/gtsam/nonlinear/LagoInitializer.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -10,38 +10,57 @@ * -------------------------------------------------------------------------- */ /** - * @file LagoInitializer.h + * @file lago.h * @author Luca Carlone * @author Frank Dellaert * @date May 14, 2014 */ -#include -#include +#include +#include +#include +#include +#include + #include namespace gtsam { +namespace lago { -static Matrix I = eye(1); -static Matrix I3 = eye(3); +static const Matrix I = eye(1); +static const Matrix I3 = eye(3); + +static const Key keyAnchor = symbol('Z', 9999999); +static const noiseModel::Diagonal::shared_ptr priorOrientationNoise = + noiseModel::Diagonal::Variances((Vector(1) << 1e-8)); +static const noiseModel::Diagonal::shared_ptr priorPose2Noise = + noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); /* ************************************************************************* */ -double computeThetaToRoot(const Key nodeKey, const PredecessorMap& tree, - const key2doubleMap& deltaThetaMap, const key2doubleMap& thetaFromRootMap) { +/** + * Compute the cumulative orientation (without wrapping) wrt the root of a + * spanning tree (tree) for a node (nodeKey). The function starts at the nodes and + * moves towards the root summing up the (directed) rotation measurements. + * Relative measurements are encoded in "deltaThetaMap". + * The root is assumed to have orientation zero. + */ +static double computeThetaToRoot(const Key nodeKey, + const PredecessorMap& tree, const key2doubleMap& deltaThetaMap, + const key2doubleMap& thetaFromRootMap) { double nodeTheta = 0; Key key_child = nodeKey; // the node Key key_parent = 0; // the initialization does not matter - while(1){ + while (1) { // We check if we reached the root - if(tree.at(key_child)==key_child) // if we reached the root + if (tree.at(key_child) == key_child) // if we reached the root break; // we sum the delta theta corresponding to the edge parent->child nodeTheta += deltaThetaMap.at(key_child); // we get the parent key_parent = tree.at(key_child); // the parent // we check if we connected to some part of the tree we know - if(thetaFromRootMap.find(key_parent)!=thetaFromRootMap.end()){ + if (thetaFromRootMap.find(key_parent) != thetaFromRootMap.end()) { nodeTheta += thetaFromRootMap.at(key_parent); break; } @@ -55,15 +74,14 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, const PredecessorMap& tree) { key2doubleMap thetaToRootMap; - key2doubleMap::const_iterator it; // Orientation of the roo thetaToRootMap.insert(std::pair(keyAnchor, 0.0)); // for all nodes in the tree - for(it = deltaThetaMap.begin(); it != deltaThetaMap.end(); ++it ){ + BOOST_FOREACH(const key2doubleMap::value_type& it, deltaThetaMap) { // compute the orientation wrt root - Key nodeKey = it->first; + Key nodeKey = it.first; double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap, thetaToRootMap); thetaToRootMap.insert(std::pair(nodeKey, nodeTheta)); @@ -73,35 +91,38 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, /* ************************************************************************* */ void getSymbolicGraph( - /*OUTPUTS*/ std::vector& spanningTreeIds, std::vector& chordsIds, key2doubleMap& deltaThetaMap, - /*INPUTS*/ const PredecessorMap& tree, const NonlinearFactorGraph& g){ +/*OUTPUTS*/std::vector& spanningTreeIds, std::vector& chordsIds, + key2doubleMap& deltaThetaMap, + /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g) { // Get keys for which you want the orientation - size_t id=0; + size_t id = 0; // Loop over the factors - BOOST_FOREACH(const boost::shared_ptr& factor, g){ - if (factor->keys().size() == 2){ + BOOST_FOREACH(const boost::shared_ptr& factor, g) { + if (factor->keys().size() == 2) { Key key1 = factor->keys()[0]; Key key2 = factor->keys()[1]; // recast to a between - boost::shared_ptr< BetweenFactor > pose2Between = - boost::dynamic_pointer_cast< BetweenFactor >(factor); - if (!pose2Between) continue; + boost::shared_ptr > pose2Between = + boost::dynamic_pointer_cast >(factor); + if (!pose2Between) + continue; // get the orientation - measured().theta(); double deltaTheta = pose2Between->measured().theta(); // insert (directed) orientations in the map "deltaThetaMap" - bool inTree=false; - if(tree.at(key1)==key2){ // key2 -> key1 + bool inTree = false; + if (tree.at(key1) == key2) { // key2 -> key1 deltaThetaMap.insert(std::pair(key1, -deltaTheta)); inTree = true; - } else if(tree.at(key2)==key1){ // key1 -> key2 - deltaThetaMap.insert(std::pair(key2, deltaTheta)); + } else if (tree.at(key2) == key1) { // key1 -> key2 + deltaThetaMap.insert(std::pair(key2, deltaTheta)); inTree = true; } // store factor slot, distinguishing spanning tree edges from chordsIds - if(inTree == true) + if (inTree == true) spanningTreeIds.push_back(id); - else // it's a chord! + else + // it's a chord! chordsIds.push_back(id); } id++; @@ -109,14 +130,16 @@ void getSymbolicGraph( } /* ************************************************************************* */ -void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, +// Retrieve the deltaTheta and the corresponding noise model from a BetweenFactor +static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta) { // Get the relative rotation measurement from the between factor boost::shared_ptr > pose2Between = boost::dynamic_pointer_cast >(factor); if (!pose2Between) - throw std::invalid_argument("buildLinearOrientationGraph: invalid between factor!"); + throw std::invalid_argument( + "buildLinearOrientationGraph: invalid between factor!"); deltaTheta = (Vector(1) << pose2Between->measured().theta()); // Retrieve the noise model for the relative rotation @@ -124,114 +147,130 @@ void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, boost::shared_ptr diagonalModel = boost::dynamic_pointer_cast(model); if (!diagonalModel) - throw std::invalid_argument("buildLinearOrientationGraph: invalid noise model " - "(current version assumes diagonal noise model)!"); - Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2) ); // std on the angular measurement + throw std::invalid_argument( + "buildLinearOrientationGraph: invalid noise model " + "(current version assumes diagonal noise model)!"); + Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2)); // std on the angular measurement model_deltaTheta = noiseModel::Diagonal::Sigmas(std_deltaTheta); } /* ************************************************************************* */ -GaussianFactorGraph buildLinearOrientationGraph(const std::vector& spanningTreeIds, const std::vector& chordsIds, - const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap& tree){ +GaussianFactorGraph buildLinearOrientationGraph( + const std::vector& spanningTreeIds, + const std::vector& chordsIds, const NonlinearFactorGraph& g, + const key2doubleMap& orientationsToRoot, const PredecessorMap& tree) { GaussianFactorGraph lagoGraph; Vector deltaTheta; noiseModel::Diagonal::shared_ptr model_deltaTheta; // put original measurements in the spanning tree - BOOST_FOREACH(const size_t& factorId, spanningTreeIds){ + BOOST_FOREACH(const size_t& factorId, spanningTreeIds) { const FastVector& keys = g[factorId]->keys(); Key key1 = keys[0], key2 = keys[1]; getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); - lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaTheta, model_deltaTheta)); + lagoGraph.add( + JacobianFactor(key1, -I, key2, I, deltaTheta, model_deltaTheta)); } // put regularized measurements in the chordsIds - BOOST_FOREACH(const size_t& factorId, chordsIds){ + BOOST_FOREACH(const size_t& factorId, chordsIds) { const FastVector& keys = g[factorId]->keys(); Key key1 = keys[0], key2 = keys[1]; getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); double key1_DeltaTheta_key2 = deltaTheta(0); ///std::cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << std::endl; - double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1) - orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord - double k = boost::math::round(k2pi_noise/(2*M_PI)); + double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1) + - orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord + double k = boost::math::round(k2pi_noise / (2 * M_PI)); //if (k2pi_noise - 2*k*M_PI > 1e-5) std::cout << k2pi_noise - 2*k*M_PI << std::endl; // for debug - Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2*k*M_PI); - lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta)); + Vector deltaThetaRegularized = (Vector(1) + << key1_DeltaTheta_key2 - 2 * k * M_PI); + lagoGraph.add( + JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, + model_deltaTheta)); } // prior on the anchor orientation - lagoGraph.add(JacobianFactor(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise)); + lagoGraph.add( + JacobianFactor(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise)); return lagoGraph; } /* ************************************************************************* */ -NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph){ +// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node +static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { NonlinearFactorGraph pose2Graph; - BOOST_FOREACH(const boost::shared_ptr& factor, graph){ + BOOST_FOREACH(const boost::shared_ptr& factor, graph) { // recast to a between on Pose2 - boost::shared_ptr< BetweenFactor > pose2Between = - boost::dynamic_pointer_cast< BetweenFactor >(factor); + boost::shared_ptr > pose2Between = + boost::dynamic_pointer_cast >(factor); if (pose2Between) pose2Graph.add(pose2Between); // recast PriorFactor to BetweenFactor - boost::shared_ptr< PriorFactor > pose2Prior = - boost::dynamic_pointer_cast< PriorFactor >(factor); + boost::shared_ptr > pose2Prior = + boost::dynamic_pointer_cast >(factor); if (pose2Prior) - pose2Graph.add(BetweenFactor(keyAnchor, pose2Prior->keys()[0], - pose2Prior->prior(), pose2Prior->get_noiseModel())); + pose2Graph.add( + BetweenFactor(keyAnchor, pose2Prior->keys()[0], + pose2Prior->prior(), pose2Prior->get_noiseModel())); } return pose2Graph; } /* ************************************************************************* */ -PredecessorMap findOdometricPath(const NonlinearFactorGraph& pose2Graph) { +static PredecessorMap findOdometricPath( + const NonlinearFactorGraph& pose2Graph) { PredecessorMap tree; Key minKey; bool minUnassigned = true; - BOOST_FOREACH(const boost::shared_ptr& factor, pose2Graph){ + BOOST_FOREACH(const boost::shared_ptr& factor, pose2Graph) { Key key1 = std::min(factor->keys()[0], factor->keys()[1]); Key key2 = std::max(factor->keys()[0], factor->keys()[1]); - if(minUnassigned){ + if (minUnassigned) { minKey = key1; minUnassigned = false; } - if( key2 - key1 == 1){ // consecutive keys + if (key2 - key1 == 1) { // consecutive keys tree.insert(key2, key1); - if(key1 < minKey) + if (key1 < minKey) minKey = key1; } } - tree.insert(minKey,keyAnchor); - tree.insert(keyAnchor,keyAnchor); // root + tree.insert(minKey, keyAnchor); + tree.insert(keyAnchor, keyAnchor); // root return tree; } /* ************************************************************************* */ -VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath){ +// Return the orientations of a graph including only BetweenFactors +static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, + bool useOdometricPath) { // Find a minimum spanning tree PredecessorMap tree; if (useOdometricPath) tree = findOdometricPath(pose2Graph); else - tree = findMinimumSpanningTree >(pose2Graph); + tree = findMinimumSpanningTree >(pose2Graph); // Create a linear factor graph (LFG) of scalars key2doubleMap deltaThetaMap; std::vector spanningTreeIds; // ids of between factors forming the spanning tree T - std::vector chordsIds; // ids of between factors corresponding to chordsIds wrt T + std::vector chordsIds; // ids of between factors corresponding to chordsIds wrt T getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, pose2Graph); // temporary structure to correct wraparounds along loops key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree); // regularize measurements and plug everything in a factor graph - GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, chordsIds, pose2Graph, orientationsToRoot, tree); + GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, + chordsIds, pose2Graph, orientationsToRoot, tree); // Solve the LFG VectorValues orientationsLago = lagoGraph.optimize(); @@ -240,70 +279,81 @@ VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, boo } /* ************************************************************************* */ -VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph, bool useOdometricPath) { +VectorValues initializeOrientations(const NonlinearFactorGraph& graph, + bool useOdometricPath) { // 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 - return computeLagoOrientations(pose2Graph, useOdometricPath); + return computeOrientations(pose2Graph, useOdometricPath); } /* ************************************************************************* */ -Values computeLagoPoses(const NonlinearFactorGraph& pose2graph, VectorValues& orientationsLago) { +Values computePoses(const NonlinearFactorGraph& pose2graph, + VectorValues& orientationsLago) { // 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_FOREACH(const boost::shared_ptr& factor, pose2graph) { - boost::shared_ptr< BetweenFactor > pose2Between = - boost::dynamic_pointer_cast< BetweenFactor >(factor); + boost::shared_ptr > pose2Between = + boost::dynamic_pointer_cast >(factor); - if(pose2Between){ + if (pose2Between) { Key key1 = pose2Between->keys()[0]; double theta1 = orientationsLago.at(key1)(0); - double s1 = sin(theta1); double c1 = cos(theta1); + 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(); + 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; + 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()); + boost::dynamic_pointer_cast( + pose2Between->get_noiseModel()); - linearPose2graph.add(JacobianFactor(key1, J1, key2, I3, b, diagonalModel)); - }else{ - throw std::invalid_argument("computeLagoPoses: cannot manage non between factor here!"); + linearPose2graph.add( + JacobianFactor(key1, J1, key2, I3, b, diagonalModel)); + } else { + throw std::invalid_argument( + "computeLagoPoses: cannot manage non between factor here!"); } } // add prior - noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); - linearPose2graph.add(JacobianFactor(keyAnchor, I3, (Vector(3) << 0.0,0.0,0.0), priorModel)); + noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances( + (Vector(3) << 1e-2, 1e-2, 1e-4)); + linearPose2graph.add( + JacobianFactor(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0), priorModel)); // optimize VectorValues posesLago = linearPose2graph.optimize(); // put into Values structure Values initialGuessLago; - for(VectorValues::const_iterator it = posesLago.begin(); it != posesLago.end(); ++it ){ - Key key = it->first; - if (key != keyAnchor){ - Vector poseVector = posesLago.at(key); - Pose2 poseLago = Pose2(poseVector(0),poseVector(1),orientationsLago.at(key)(0)+poseVector(2)); + 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); } } @@ -311,37 +361,40 @@ Values computeLagoPoses(const NonlinearFactorGraph& pose2graph, VectorValues& or } /* ************************************************************************* */ -Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath) { +Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) { // 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 = computeLagoOrientations(pose2Graph, useOdometricPath); + VectorValues orientationsLago = computeOrientations(pose2Graph, + useOdometricPath); // Compute the full poses - return computeLagoPoses(pose2Graph, orientationsLago); + return computePoses(pose2Graph, orientationsLago); } /* ************************************************************************* */ -Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess) { +Values initialize(const NonlinearFactorGraph& graph, + const Values& initialGuess) { Values initialGuessLago; // get the orientation estimates from LAGO - VectorValues orientations = initializeOrientationsLago(graph); + VectorValues orientations = initializeOrientations(graph); // for all nodes in the tree - for(VectorValues::const_iterator it = orientations.begin(); it != orientations.end(); ++it ){ - Key key = it->first; - if (key != keyAnchor){ - Pose2 pose = initialGuess.at(key); - Vector orientation = orientations.at(key); - Pose2 poseLago = Pose2(pose.x(),pose.y(),orientation(0)); + 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/nonlinear/lago.h b/gtsam/nonlinear/lago.h new file mode 100644 index 000000000..0df80ac42 --- /dev/null +++ b/gtsam/nonlinear/lago.h @@ -0,0 +1,86 @@ +/* ---------------------------------------------------------------------------- + + * 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 lago.h + * @brief Initialize Pose2 in a factor graph using LAGO + * (Linear Approximation for Graph Optimization). see papers: + * + * L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate + * approximation for planar pose graph optimization, IJRR, 2014. + * + * L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation + * for graph-based simultaneous localization and mapping, RSS, 2011. + * + * @param graph: nonlinear factor graph (can include arbitrary factors but we assume + * that there is a subgraph involving Pose2 and betweenFactors). Also in the current + * version we assume that there is an odometric spanning path (x0->x1, x1->x2, etc) + * and a prior on x0. This assumption can be relaxed by using the extra argument + * useOdometricPath = false, although this part of code is not stable yet. + * @return Values: initial guess from LAGO (only pose2 are initialized) + * + * @author Luca Carlone + * @author Frank Dellaert + * @date May 14, 2014 + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { +namespace lago { + +typedef std::map key2doubleMap; + +/** + * Compute the cumulative orientations (without wrapping) + * for all nodes wrt the root (root has zero orientation). + */ +GTSAM_EXPORT key2doubleMap computeThetasToRoot( + const key2doubleMap& deltaThetaMap, const PredecessorMap& tree); + +/** + * Given a factor graph "g", and a spanning tree "tree", select the nodes belonging + * to the tree and to g, and stores the factor slots corresponding to edges in the + * tree and to chordsIds wrt this tree. + * Also it computes deltaThetaMap which is a fast way to encode relative + * orientations along the tree: for a node key2, s.t. tree[key2]=key1, + * the value deltaThetaMap[key2] is relative orientation theta[key2]-theta[key1] + */ +GTSAM_EXPORT void getSymbolicGraph( +/*OUTPUTS*/std::vector& spanningTreeIds, std::vector& chordsIds, + key2doubleMap& deltaThetaMap, + /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g); + +/** 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/nonlinear/tests/testLagoInitializer.cpp b/gtsam/nonlinear/tests/testLago.cpp similarity index 75% rename from gtsam/nonlinear/tests/testLagoInitializer.cpp rename to gtsam/nonlinear/tests/testLago.cpp index 64e43ae9b..f2cab4506 100644 --- a/gtsam/nonlinear/tests/testLagoInitializer.cpp +++ b/gtsam/nonlinear/tests/testLago.cpp @@ -19,20 +19,14 @@ * @date May 14, 2014 */ -#include - +#include +#include +#include +#include #include -#include -#include -#include - -#include -#include - -#include #include -#include + #include using namespace std; @@ -77,10 +71,10 @@ TEST( Lago, checkSTandChords ) { PredecessorMap tree = findMinimumSpanningTree >(g); - key2doubleMap deltaThetaMap; + lago::key2doubleMap deltaThetaMap; vector spanningTreeIds; // ids of between factors forming the spanning tree T vector chordsIds; // ids of between factors corresponding to chordsIds wrt T - getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); + lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); DOUBLES_EQUAL(spanningTreeIds[0], 0, 1e-6); // factor 0 is the first in the ST (0->1) DOUBLES_EQUAL(spanningTreeIds[1], 3, 1e-6); // factor 3 is the second in the ST(2->0) @@ -100,19 +94,19 @@ TEST( Lago, orientationsOverSpanningTree ) { EXPECT_LONGS_EQUAL(tree[x2], x0); EXPECT_LONGS_EQUAL(tree[x3], x0); - key2doubleMap expected; + lago::key2doubleMap expected; expected[x0]= 0; expected[x1]= M_PI/2; // edge x0->x1 (consistent with edge (x0,x1)) expected[x2]= -M_PI; // edge x0->x2 (traversed backwards wrt edge (x2,x0)) expected[x3]= -M_PI/2; // edge x0->x3 (consistent with edge (x0,x3)) - key2doubleMap deltaThetaMap; + lago::key2doubleMap deltaThetaMap; vector spanningTreeIds; // ids of between factors forming the spanning tree T vector chordsIds; // ids of between factors corresponding to chordsIds wrt T - getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); + lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); - key2doubleMap actual; - actual = computeThetasToRoot(deltaThetaMap, tree); + lago::key2doubleMap actual; + actual = lago::computeThetasToRoot(deltaThetaMap, tree); DOUBLES_EQUAL(expected[x0], actual[x0], 1e-6); DOUBLES_EQUAL(expected[x1], actual[x1], 1e-6); DOUBLES_EQUAL(expected[x2], actual[x2], 1e-6); @@ -125,14 +119,14 @@ TEST( Lago, regularizedMeasurements ) { PredecessorMap tree = findMinimumSpanningTree >(g); - key2doubleMap deltaThetaMap; + lago::key2doubleMap deltaThetaMap; vector spanningTreeIds; // ids of between factors forming the spanning tree T vector chordsIds; // ids of between factors corresponding to chordsIds wrt T - getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); + lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); - key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree); + lago::key2doubleMap orientationsToRoot = lago::computeThetasToRoot(deltaThetaMap, tree); - GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, chordsIds, g, orientationsToRoot, tree); + GaussianFactorGraph lagoGraph = lago::buildLinearOrientationGraph(spanningTreeIds, chordsIds, g, orientationsToRoot, tree); std::pair actualAb = lagoGraph.jacobian(); // jacobian corresponding to the orientation measurements (last entry is the prior on the anchor and is disregarded) Vector actual = (Vector(5) << actualAb.second(0),actualAb.second(1),actualAb.second(2),actualAb.second(3),actualAb.second(4)); @@ -147,25 +141,25 @@ TEST( Lago, regularizedMeasurements ) { /* *************************************************************************** */ TEST( Lago, smallGraphVectorValues ) { bool useOdometricPath = false; - VectorValues initialGuessLago = initializeOrientationsLago(simple::graph(), useOdometricPath); + VectorValues initial = lago::initializeOrientations(simple::graph(), useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI - EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); - EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); - EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6)); - EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6)); } /* *************************************************************************** */ TEST( Lago, smallGraphVectorValuesSP ) { - VectorValues initialGuessLago = initializeOrientationsLago(simple::graph()); + VectorValues initial = lago::initializeOrientations(simple::graph()); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI - EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); - EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); - EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6)); - EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6)); } /* *************************************************************************** */ @@ -173,26 +167,26 @@ TEST( Lago, multiplePosePriors ) { bool useOdometricPath = false; NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1, model)); - VectorValues initialGuessLago = initializeOrientationsLago(g, useOdometricPath); + VectorValues initial = lago::initializeOrientations(g, useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI - EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); - EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); - EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6)); - EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6)); } /* *************************************************************************** */ TEST( Lago, multiplePosePriorsSP ) { NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1, model)); - VectorValues initialGuessLago = initializeOrientationsLago(g); + VectorValues initial = lago::initializeOrientations(g); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI - EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); - EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); - EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6)); - EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6)); } /* *************************************************************************** */ @@ -200,26 +194,26 @@ TEST( Lago, multiplePoseAndRotPriors ) { bool useOdometricPath = false; NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1.theta(), model)); - VectorValues initialGuessLago = initializeOrientationsLago(g, useOdometricPath); + VectorValues initial = lago::initializeOrientations(g, useOdometricPath); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI - EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); - EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); - EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6)); - EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6)); } /* *************************************************************************** */ TEST( Lago, multiplePoseAndRotPriorsSP ) { NonlinearFactorGraph g = simple::graph(); g.add(PriorFactor(x1, simple::pose1.theta(), model)); - VectorValues initialGuessLago = initializeOrientationsLago(g); + VectorValues initial = lago::initializeOrientations(g); // comparison is up to M_PI, that's why we add some multiples of 2*M_PI - EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); - EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); - EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6)); - EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); + EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6)); + EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6)); + EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6)); } /* *************************************************************************** */ @@ -233,7 +227,7 @@ TEST( Lago, smallGraphValues ) { initialGuess.insert(x3,Pose2(simple::pose3.x(),simple::pose3.y(),0.0)); // lago does not touch the Cartesian part and only fixed the orientations - Values actual = initializeLago(simple::graph(), initialGuess); + Values actual = lago::initialize(simple::graph(), initialGuess); // we are in a noiseless case Values expected; @@ -249,7 +243,7 @@ TEST( Lago, smallGraphValues ) { TEST( Lago, smallGraph2 ) { // lago does not touch the Cartesian part and only fixed the orientations - Values actual = initializeLago(simple::graph()); + Values actual = lago::initialize(simple::graph()); // we are in a noiseless case Values expected; @@ -274,7 +268,7 @@ TEST( Lago, largeGraphNoisy_orientations ) { noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); - VectorValues actualVV = initializeOrientationsLago(graphWithPrior); + VectorValues actualVV = lago::initializeOrientations(graphWithPrior); Values actual; Key keyAnc = symbol('Z',9999999); for(VectorValues::const_iterator it = actualVV.begin(); it != actualVV.end(); ++it ){ @@ -309,7 +303,7 @@ TEST( Lago, largeGraphNoisy ) { noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); - Values actual = initializeLago(graphWithPrior); + Values actual = lago::initialize(graphWithPrior); NonlinearFactorGraph gmatlab; Values expected;