Merged in feature/allowMap (pull request #18)

Replaced range adaptor pattern with more explicit loop to create dimensions, which also allows maps to be supplied as TERMS containers.

examples/SolverComparer.cpp

@@ -13,6 +13,22 @@
 * @brief   Incremental and batch solving, timing, and accuracy comparisons
 * @author  Richard Roberts
 * @date    August, 2013
+*
+* Here is an example. Below, to run in batch mode, we first generate an initialization in incremental mode.
+*
+* Solve in incremental and write to file w_inc:
+* ./SolverComparer --incremental -d w10000 -o w_inc
+*
+* You can then perturb that initialization to get batch something to optimize.
+* Read in w_inc, perturb it with noise of stddev 0.6, and write to w_pert:
+* ./SolverComparer --perturb 0.6 -i w_inc -o w_pert
+*
+* Then optimize with batch, read in w_pert, solve in batch, and write to w_batch:
+* ./SolverComparer --batch -d w10000 -i w_pert -o w_batch
+*
+* And finally compare solutions in w_inc and w_batch to check that batch converged to the global minimum
+* ./SolverComparer --compare w_inc w_batch
+*
 */
 
 #include <gtsam/base/timing.h>

gtsam/linear/JacobianFactor-inl.h

@@ -19,10 +19,6 @@
 #pragma once
 
 #include <gtsam/linear/linearExceptions.h>
-#include <boost/assign/list_of.hpp>
-#include <boost/range/adaptor/transformed.hpp>
-#include <boost/range/algorithm/for_each.hpp>
-#include <boost/foreach.hpp>
 
 namespace gtsam {
 
@@ -59,20 +55,10 @@ namespace gtsam {
     model_ = model;
   }
 
-  /* ************************************************************************* */
-  namespace internal {
-    static inline DenseIndex getColsJF(const std::pair<Key,Matrix>& p) {
-      return p.second.cols();
-    }
-  }
-
   /* ************************************************************************* */
   template<typename TERMS>
   void JacobianFactor::fillTerms(const TERMS& terms, const Vector& b, const SharedDiagonal& noiseModel)
   {
-    using boost::adaptors::transformed;
-    namespace br = boost::range;
-
     // Check noise model dimension
     if(noiseModel && (DenseIndex)noiseModel->dim() != b.size())
       throw InvalidNoiseModel(b.size(), noiseModel->dim());
@@ -80,25 +66,32 @@ namespace gtsam {
     // Resize base class key vector
     Base::keys_.resize(terms.size());
 
-    // Construct block matrix - this uses the boost::range 'transformed' construct to apply
-    // internal::getColsJF (defined just above here in this file) to each term.  This
-    // transforms the list of terms into a list of variable dimensions, by extracting the
-    // number of columns in each matrix.  This is done to avoid separately allocating an
-    // array of dimensions before constructing the VerticalBlockMatrix.
-    Ab_ = VerticalBlockMatrix(terms | transformed(&internal::getColsJF), b.size(), true);
+    // Get dimensions of matrices
+    std::vector<size_t> dimensions;
+    dimensions.reserve(terms.size());
+    for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); ++it) {
+      const std::pair<Key, Matrix>& term = *it;
+      const Matrix& Ai = term.second;
+      dimensions.push_back(Ai.cols());
+    }
+
+    // Construct block matrix
+    Ab_ = VerticalBlockMatrix(dimensions, b.size(), true);
 
     // Check and add terms
     DenseIndex i = 0; // For block index
-    for(typename TERMS::const_iterator termIt = terms.begin(); termIt != terms.end(); ++termIt) {
-      const std::pair<Key, Matrix>& term = *termIt;
+    for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); ++it) {
+      const std::pair<Key, Matrix>& term = *it;
+      Key key = term.first;
+      const Matrix& Ai = term.second;
 
       // Check block rows
-      if(term.second.rows() != Ab_.rows())
-        throw InvalidMatrixBlock(Ab_.rows(), term.second.rows());
+      if(Ai.rows() != Ab_.rows())
+        throw InvalidMatrixBlock(Ab_.rows(), Ai.rows());
 
       // Assign key and matrix
-      Base::keys_[i] = term.first;
-      Ab_(i) = term.second;
+      Base::keys_[i] = key;
+      Ab_(i) = Ai;
 
       // Increment block index
       ++ i;

gtsam/linear/tests/testJacobianFactor.cpp

@@ -105,6 +105,24 @@ TEST(JacobianFactor, constructors_and_accessors)
     EXPECT(noise == expected.get_model());
     EXPECT(noise == actual.get_model());
   }
+  {
+    // Test three-term constructor with std::map
+    JacobianFactor expected(
+      boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, noise);
+    map<Key,Matrix> mapTerms;
+    // note order of insertion plays no role: order will be determined by keys
+    mapTerms.insert(terms[2]);
+    mapTerms.insert(terms[1]);
+    mapTerms.insert(terms[0]);
+    JacobianFactor actual(mapTerms, b, noise);
+    EXPECT(assert_equal(expected, actual));
+    LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back());
+    EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1)));
+    EXPECT(assert_equal(b, expected.getb()));
+    EXPECT(assert_equal(b, actual.getb()));
+    EXPECT(noise == expected.get_model());
+    EXPECT(noise == actual.get_model());
+  }
   {
     // VerticalBlockMatrix constructor
     JacobianFactor expected(