Finish timeLago

.cproject

@@ -2857,6 +2857,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="timeLago.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>timeLago.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testImuFactor.run" path="build-debug/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j4</buildArguments>

gtsam/inference/VariableIndex-inl.h

@@ -23,43 +23,35 @@ namespace gtsam {
 
 /* ************************************************************************* */
 template<class FG>
-void VariableIndex::augment(const FG& factors, boost::optional<const FastVector<size_t>&> newFactorIndices)
+void VariableIndex::augment(const FG& factors,
-{
+    boost::optional<const FastVector<size_t>&> newFactorIndices) {
   gttic(VariableIndex_augment);
 
   // Augment index for each factor
-  for(size_t i = 0; i < factors.size(); ++i)
+  for (size_t i = 0; i < factors.size(); ++i) {
-  {
+    if (factors[i]) {
-    if(factors[i])
-    {
       const size_t globalI =
-        newFactorIndices ?
+          newFactorIndices ? (*newFactorIndices)[i] : nFactors_;
-        (*newFactorIndices)[i] :
+      BOOST_FOREACH(const Key key, *factors[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) {
-    {
+      if ((*newFactorIndices)[i] >= nFactors_)
-      if((*newFactorIndices)[i] >= nFactors_)
         nFactors_ = (*newFactorIndices)[i] + 1;
-    }
+    } else {
-    else
+      ++nFactors_;
-    {
-      ++ nFactors_;
     }
   }
 }
 
 /* ************************************************************************* */
 template<typename ITERATOR, class FG>
-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) {
+  for (; factorIndex != lastFactor; ++factorIndex, ++i) {
-    if(i >= factors.size())
+    if (i >= factors.size())
-      throw std::invalid_argument("Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove");
+      throw std::invalid_argument(
-    if(factors[i]) {
+          "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);
+        Factors::iterator entry = std::find(factorEntries.begin(),
-        if(entry == factorEntries.end())
+            factorEntries.end(), *factorIndex);
-          throw std::invalid_argument("Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index");
+        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<typename ITERATOR>
 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())
+    if (!entry->second.empty())
-      throw std::invalid_argument("Asking to remove variables from the variable index that are not unused");
+      throw std::invalid_argument(
+          "Asking to remove variables from the variable index that are not unused");
     index_.erase(entry);
   }
 }

gtsam/nonlinear/lago.cpp

@@ -21,9 +21,12 @@
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/geometry/Pose2.h>
+#include <gtsam/base/timing.h>
 
 #include <boost/math/special_functions.hpp>
 
+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<Key, double>(keyAnchor, 0.0));
+  thetaToRootMap.insert(pair<Key, double>(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<Key, double>(nodeKey, nodeTheta));
+    thetaToRootMap.insert(pair<Key, double>(nodeKey, nodeTheta));
   }
   return thetaToRootMap;
 }
 
 /* ************************************************************************* */
 void getSymbolicGraph(
-/*OUTPUTS*/std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds,
+/*OUTPUTS*/vector<size_t>& spanningTreeIds, vector<size_t>& chordsIds,
     key2doubleMap& deltaThetaMap,
     /*INPUTS*/const PredecessorMap<Key>& 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<Key, double>(key1, -deltaTheta));
+        deltaThetaMap.insert(pair<Key, double>(key1, -deltaTheta));
         inTree = true;
       } else if (tree.at(key2) == key1) { // key1 -> key2
-        deltaThetaMap.insert(std::pair<Key, double>(key2, deltaTheta));
+        deltaThetaMap.insert(pair<Key, double>(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<BetweenFactor<Pose2> > pose2Between =
       boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
   if (!pose2Between)
-    throw std::invalid_argument(
+    throw invalid_argument(
         "buildLinearOrientationGraph: invalid between factor!");
   deltaTheta = (Vector(1) << pose2Between->measured().theta());
 
@@ -147,8 +150,7 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
   boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
       boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
   if (!diagonalModel)
-    throw std::invalid_argument(
+    throw invalid_argument("buildLinearOrientationGraph: invalid noise model "
-        "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);
@@ -156,9 +158,9 @@ static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
 
 /* ************************************************************************* */
 GaussianFactorGraph buildLinearOrientationGraph(
-    const std::vector<size_t>& spanningTreeIds,
+    const vector<size_t>& spanningTreeIds, const vector<size_t>& chordsIds,
-    const std::vector<size_t>& chordsIds, const NonlinearFactorGraph& g,
+    const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot,
-    const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree) {
+    const PredecessorMap<Key>& tree) {
 
   GaussianFactorGraph lagoGraph;
   Vector deltaTheta;
@@ -169,8 +171,7 @@ GaussianFactorGraph buildLinearOrientationGraph(
     const FastVector<Key>& keys = g[factorId]->keys();
     Key key1 = keys[0], key2 = keys[1];
     getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
-    lagoGraph.add(
+    lagoGraph.add(key1, -I, key2, I, deltaTheta, model_deltaTheta);
-        JacobianFactor(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(
+    lagoGraph.add(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta);
-        JacobianFactor(key1, -I, key2, I, deltaThetaRegularized,
-            model_deltaTheta));
   }
   // prior on the anchor orientation
-  lagoGraph.add(
+  lagoGraph.add(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise);
-      JacobianFactor(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<NonlinearFactor>& factor, graph) {
@@ -250,6 +249,7 @@ static PredecessorMap<Key> findOdometricPath(
 // Return the orientations of a graph including only BetweenFactors<Pose2>
 static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
     bool useOdometricPath) {
+  gttic(lago_computeOrientations);
 
   // Find a minimum spanning tree
   PredecessorMap<Key> tree;
@@ -261,8 +261,8 @@ static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
 
   // Create a linear factor graph (LFG) of scalars
   key2doubleMap deltaThetaMap;
-  std::vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
+  vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
-  std::vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
+  vector<size_t> 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
+      Vector globalDeltaCart = //
-          + c1 * dy);
+          (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<noiseModel::Diagonal>(
               pose2Between->get_noiseModel());
 
-      linearPose2graph.add(
+      linearPose2graph.add(key1, J1, key2, I3, b, diagonalModel);
-          JacobianFactor(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(
+  linearPose2graph.add(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0),
-      (Vector(3) << 1e-2, 1e-2, 1e-4));
+      priorPose2Noise);
-  linearPose2graph.add(
-      JacobianFactor(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0), priorModel));
 
   // 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

gtsam/nonlinear/tests/CMakeLists.txt

@@ -1 +1 @@
-gtsamAddTestsGlob(nonlinear "test*.cpp" "" "gtsam")
+gtsamAddTestsGlob(nonlinear "*.cpp" "" "gtsam")

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 <gtsam/slam/dataset.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/nonlinear/lago.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/linear/Sampler.h>
+#include <gtsam/base/timing.h>
+
+#include <iostream>
+
+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<Pose2> poses = solution->filter<Pose2>();
+  SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0));
+  BOOST_FOREACH(const Values::ConstFiltered<Pose2>::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<Pose2>(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;
+}

gtsam/slam/dataset.cpp

@@ -100,7 +100,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> 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);