From d34ba9c8bf2642bd5db5d0470926baa40ef65e57 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 21 Sep 2014 13:40:38 +0200 Subject: [PATCH 1/3] The range adaptor scheme did not work for std::map TERMS in creating a JacobianFacor. Hence, I removed it - which I think is more readable - and replaced it with an explicit creationg of dimensions. I also added a test with std::map, which works. --- gtsam/linear/JacobianFactor-inl.h | 44 ++++++++++------------- gtsam/linear/tests/testJacobianFactor.cpp | 18 ++++++++++ 2 files changed, 36 insertions(+), 26 deletions(-) diff --git a/gtsam/linear/JacobianFactor-inl.h b/gtsam/linear/JacobianFactor-inl.h index 7b0741ed5..293653f42 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,31 @@ 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; + 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( From 156c1abf4e638645c45dfa4cc9f12805d7301982 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 30 Sep 2014 11:27:19 +0200 Subject: [PATCH 2/3] Added reserve as suggested by Richard --- gtsam/linear/JacobianFactor-inl.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/linear/JacobianFactor-inl.h b/gtsam/linear/JacobianFactor-inl.h index 293653f42..6c4cb969a 100644 --- a/gtsam/linear/JacobianFactor-inl.h +++ b/gtsam/linear/JacobianFactor-inl.h @@ -68,6 +68,7 @@ namespace gtsam { // 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; From 34dcfa4e89ee79f8a2c36d7d687ce07855d852d4 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 30 Sep 2014 11:31:17 +0200 Subject: [PATCH 3/3] Added example from Pull Request #18 discussion --- examples/SolverComparer.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) 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