diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 2f0c71a40..26abfff85 100644 --- a/examples/SolverComparer.cpp +++ b/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 diff --git a/gtsam/linear/JacobianFactor-inl.h b/gtsam/linear/JacobianFactor-inl.h index 7b0741ed5..6c4cb969a 100644 --- a/gtsam/linear/JacobianFactor-inl.h +++ b/gtsam/linear/JacobianFactor-inl.h @@ -19,10 +19,6 @@ #pragma once #include -#include -#include -#include -#include namespace gtsam { @@ -59,20 +55,10 @@ namespace gtsam { model_ = model; } - /* ************************************************************************* */ - namespace internal { - static inline DenseIndex getColsJF(const std::pair& p) { - return p.second.cols(); - } - } - /* ************************************************************************* */ template 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 dimensions; + dimensions.reserve(terms.size()); + for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); ++it) { + const std::pair& 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& term = *termIt; + for(typename TERMS::const_iterator it = terms.begin(); it != terms.end(); ++it) { + const std::pair& 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; diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp index 1fc7365e7..f70c3496a 100644 --- a/gtsam/linear/tests/testJacobianFactor.cpp +++ b/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 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(