diff --git a/.cproject b/.cproject index a43d32f58..d759a7a65 100644 --- a/.cproject +++ b/.cproject @@ -2857,6 +2857,14 @@ true true + + make + -j5 + timeLago.run + true + true + true + make -j4 diff --git a/gtsam/inference/VariableIndex-inl.h b/gtsam/inference/VariableIndex-inl.h index a88ea5d57..82eb85c76 100644 --- a/gtsam/inference/VariableIndex-inl.h +++ b/gtsam/inference/VariableIndex-inl.h @@ -23,43 +23,35 @@ namespace gtsam { /* ************************************************************************* */ template -void VariableIndex::augment(const FG& factors, boost::optional&> newFactorIndices) -{ +void VariableIndex::augment(const FG& factors, + boost::optional&> newFactorIndices) { gttic(VariableIndex_augment); // Augment index for each factor - for(size_t i = 0; i < factors.size(); ++i) - { - if(factors[i]) - { + for (size_t i = 0; i < factors.size(); ++i) { + if (factors[i]) { const size_t globalI = - newFactorIndices ? - (*newFactorIndices)[i] : - nFactors_; - BOOST_FOREACH(const Key key, *factors[i]) - { + newFactorIndices ? (*newFactorIndices)[i] : nFactors_; + BOOST_FOREACH(const Key key, *factors[i]) { index_[key].push_back(globalI); - ++ nEntries_; + ++nEntries_; } } // Increment factor count even if factors are null, to keep indices consistent - if(newFactorIndices) - { - if((*newFactorIndices)[i] >= nFactors_) + if (newFactorIndices) { + if ((*newFactorIndices)[i] >= nFactors_) nFactors_ = (*newFactorIndices)[i] + 1; - } - else - { - ++ nFactors_; + } else { + ++nFactors_; } } } /* ************************************************************************* */ template -void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors) -{ +void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, + const FG& factors) { gttic(VariableIndex_remove); // NOTE: We intentionally do not decrement nFactors_ because the factor @@ -68,17 +60,20 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& // one greater than the highest-numbered factor referenced in a VariableIndex. ITERATOR factorIndex = firstFactor; size_t i = 0; - for( ; factorIndex != lastFactor; ++factorIndex, ++i) { - if(i >= factors.size()) - throw std::invalid_argument("Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove"); - if(factors[i]) { + for (; factorIndex != lastFactor; ++factorIndex, ++i) { + if (i >= factors.size()) + throw std::invalid_argument( + "Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove"); + if (factors[i]) { BOOST_FOREACH(Key j, *factors[i]) { Factors& factorEntries = internalAt(j); - Factors::iterator entry = std::find(factorEntries.begin(), factorEntries.end(), *factorIndex); - if(entry == factorEntries.end()) - throw std::invalid_argument("Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index"); + Factors::iterator entry = std::find(factorEntries.begin(), + factorEntries.end(), *factorIndex); + if (entry == factorEntries.end()) + throw std::invalid_argument( + "Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index"); factorEntries.erase(entry); - -- nEntries_; + --nEntries_; } } } @@ -87,10 +82,11 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& /* ************************************************************************* */ template void VariableIndex::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) { - for(ITERATOR key = firstKey; key != lastKey; ++key) { + for (ITERATOR key = firstKey; key != lastKey; ++key) { KeyMap::iterator entry = index_.find(*key); - if(!entry->second.empty()) - throw std::invalid_argument("Asking to remove variables from the variable index that are not unused"); + if (!entry->second.empty()) + throw std::invalid_argument( + "Asking to remove variables from the variable index that are not unused"); index_.erase(entry); } } diff --git a/gtsam/nonlinear/lago.cpp b/gtsam/nonlinear/lago.cpp index c2a7d9454..0f69f6ef0 100644 --- a/gtsam/nonlinear/lago.cpp +++ b/gtsam/nonlinear/lago.cpp @@ -21,9 +21,12 @@ #include #include #include +#include #include +using namespace std; + namespace gtsam { namespace lago { @@ -32,7 +35,7 @@ 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)); + noiseModel::Diagonal::Sigmas((Vector(1) << 0)); static const noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); @@ -76,7 +79,7 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, key2doubleMap thetaToRootMap; // Orientation of the roo - thetaToRootMap.insert(std::pair(keyAnchor, 0.0)); + thetaToRootMap.insert(pair(keyAnchor, 0.0)); // for all nodes in the tree BOOST_FOREACH(const key2doubleMap::value_type& it, deltaThetaMap) { @@ -84,14 +87,14 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap, Key nodeKey = it.first; double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap, thetaToRootMap); - thetaToRootMap.insert(std::pair(nodeKey, nodeTheta)); + thetaToRootMap.insert(pair(nodeKey, nodeTheta)); } return thetaToRootMap; } /* ************************************************************************* */ void getSymbolicGraph( -/*OUTPUTS*/std::vector& spanningTreeIds, std::vector& chordsIds, +/*OUTPUTS*/vector& spanningTreeIds, vector& chordsIds, key2doubleMap& deltaThetaMap, /*INPUTS*/const PredecessorMap& tree, const NonlinearFactorGraph& g) { @@ -112,10 +115,10 @@ void getSymbolicGraph( // insert (directed) orientations in the map "deltaThetaMap" bool inTree = false; if (tree.at(key1) == key2) { // key2 -> key1 - deltaThetaMap.insert(std::pair(key1, -deltaTheta)); + deltaThetaMap.insert(pair(key1, -deltaTheta)); inTree = true; } else if (tree.at(key2) == key1) { // key1 -> key2 - deltaThetaMap.insert(std::pair(key2, deltaTheta)); + deltaThetaMap.insert(pair(key2, deltaTheta)); inTree = true; } // store factor slot, distinguishing spanning tree edges from chordsIds @@ -138,7 +141,7 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, boost::shared_ptr > pose2Between = boost::dynamic_pointer_cast >(factor); if (!pose2Between) - throw std::invalid_argument( + throw invalid_argument( "buildLinearOrientationGraph: invalid between factor!"); deltaTheta = (Vector(1) << pose2Between->measured().theta()); @@ -147,18 +150,17 @@ static 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)!"); + throw 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) { + const vector& spanningTreeIds, const vector& chordsIds, + const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, + const PredecessorMap& tree) { GaussianFactorGraph lagoGraph; Vector deltaTheta; @@ -169,8 +171,7 @@ GaussianFactorGraph buildLinearOrientationGraph( 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(key1, -I, key2, I, deltaTheta, model_deltaTheta); } // put regularized measurements in the chordsIds BOOST_FOREACH(const size_t& factorId, chordsIds) { @@ -178,26 +179,24 @@ GaussianFactorGraph buildLinearOrientationGraph( 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; + ///cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << 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)); - //if (k2pi_noise - 2*k*M_PI > 1e-5) std::cout << k2pi_noise - 2*k*M_PI << std::endl; // for debug + //if (k2pi_noise - 2*k*M_PI > 1e-5) cout << k2pi_noise - 2*k*M_PI << endl; // for debug Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2 * k * M_PI); - lagoGraph.add( - JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, - model_deltaTheta)); + lagoGraph.add(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta); } // prior on the anchor orientation - lagoGraph.add( - JacobianFactor(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise)); + lagoGraph.add(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise); return lagoGraph; } /* ************************************************************************* */ // Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) { + gttic(lago_buildPose2graph); NonlinearFactorGraph pose2Graph; BOOST_FOREACH(const boost::shared_ptr& factor, graph) { @@ -250,6 +249,7 @@ static PredecessorMap findOdometricPath( // Return the orientations of a graph including only BetweenFactors static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath) { + gttic(lago_computeOrientations); // Find a minimum spanning tree PredecessorMap tree; @@ -261,8 +261,8 @@ static VectorValues computeOrientations(const NonlinearFactorGraph& 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 + 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, pose2Graph); // temporary structure to correct wraparounds along loops @@ -293,6 +293,7 @@ VectorValues initializeOrientations(const NonlinearFactorGraph& graph, /* ************************************************************************* */ Values computePoses(const NonlinearFactorGraph& pose2graph, VectorValues& orientationsLago) { + gttic(lago_computePoses); // Linearized graph on full poses GaussianFactorGraph linearPose2graph; @@ -319,8 +320,8 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, double dx = pose2Between->measured().x(); double dy = pose2Between->measured().y(); - Vector globalDeltaCart = (Vector(2) << c1 * dx - s1 * dy, s1 * dx - + c1 * 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; @@ -330,18 +331,15 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, boost::dynamic_pointer_cast( pose2Between->get_noiseModel()); - linearPose2graph.add( - JacobianFactor(key1, J1, key2, I3, b, diagonalModel)); + linearPose2graph.add(key1, J1, key2, I3, b, diagonalModel); } else { - throw std::invalid_argument( + throw 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)); + linearPose2graph.add(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0), + priorPose2Noise); // optimize VectorValues posesLago = linearPose2graph.optimize(); @@ -362,6 +360,7 @@ Values computePoses(const NonlinearFactorGraph& pose2graph, /* ************************************************************************* */ Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) { + gttic(lago_initialize); // We "extract" the Pose2 subgraph of the original graph: this // is done to properly model priors and avoiding operating on a larger graph diff --git a/gtsam/nonlinear/tests/CMakeLists.txt b/gtsam/nonlinear/tests/CMakeLists.txt index 69a3700f2..378d93de4 100644 --- a/gtsam/nonlinear/tests/CMakeLists.txt +++ b/gtsam/nonlinear/tests/CMakeLists.txt @@ -1 +1 @@ -gtsamAddTestsGlob(nonlinear "test*.cpp" "" "gtsam") +gtsamAddTestsGlob(nonlinear "*.cpp" "" "gtsam") diff --git a/gtsam/nonlinear/tests/timeLago.cpp b/gtsam/nonlinear/tests/timeLago.cpp new file mode 100644 index 000000000..d3c86d0a4 --- /dev/null +++ b/gtsam/nonlinear/tests/timeLago.cpp @@ -0,0 +1,82 @@ +/* ---------------------------------------------------------------------------- + + * 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 timeVirtual.cpp + * @brief Time the overhead of using virtual destructors and methods + * @author Richard Roberts + * @date Dec 3, 2010 + */ + +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +int main(int argc, char *argv[]) { + + size_t trials = 1; + + // read graph + Values::shared_ptr solution; + NonlinearFactorGraph::shared_ptr g; + string inputFile = findExampleDataFile("w10000"); + SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0)); + boost::tie(g, solution) = load2D(inputFile, model); + + // add noise to create initial estimate + Values initial; + Sampler sampler(42u); + Values::ConstFiltered poses = solution->filter(); + SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0)); + BOOST_FOREACH(const Values::ConstFiltered::KeyValuePair& it, poses) + initial.insert(it.key, it.value.retract(sampler.sampleNewModel(noise))); + + // Add prior on the pose having index (key) = 0 + noiseModel::Diagonal::shared_ptr priorModel = // + noiseModel::Diagonal::Sigmas(Vector3(1e-6, 1e-6, 1e-8)); + g->add(PriorFactor(0, Pose2(), priorModel)); + + // LAGO + for (size_t i = 0; i < trials; i++) { + { + gttic_(lago); + + gttic_(init); + Values lagoInitial = lago::initialize(*g); + gttoc_(init); + + gttic_(refine); + GaussNewtonOptimizer optimizer(*g, lagoInitial); + Values result = optimizer.optimize(); + gttoc_(refine); + } + + { + gttic_(optimize); + GaussNewtonOptimizer optimizer(*g, initial); + Values result = optimizer.optimize(); + } + + tictoc_finishedIteration_(); + } + + tictoc_print_(); + + return 0; +} diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 7da45a904..cb2d44224 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -100,7 +100,7 @@ pair load2D( cout << "Will try to read " << filename << endl; ifstream is(filename.c_str()); if (!is) - throw std::invalid_argument("load2D: can not find the file!"); + throw std::invalid_argument("load2D: can not find file " + filename); Values::shared_ptr initial(new Values); NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);