12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Working around compiler problem with fillTerms

release/4.3a0
Richard Roberts 2013-08-13 18:00:58 +00:00
parent d6501bcf9e
commit 96ea505dc7
1 changed files with 51 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
gtsam/linear/JacobianFactor-inl.h
View File

@ -19,6 +19,7 @@
#pragma once #pragma once
#include <gtsam/linear/linearExceptions.h> #include <gtsam/linear/linearExceptions.h>
#include <boost/assign/list_of.hpp>
#include <boost/range/adaptor/transformed.hpp> #include <boost/range/adaptor/transformed.hpp>
#include <boost/range/join.hpp> #include <boost/range/join.hpp>
#include <boost/range/algorithm/for_each.hpp> #include <boost/range/algorithm/for_each.hpp>
@ -65,24 +66,55 @@ namespace gtsam {
return p.second.cols(); return p.second.cols();
} }
//struct _fillTerm { // Helper to fill terms from any Eigen matrix type, e.g. could be a matrix expression. Also
// mutable std::vector<Key>& keys; // handles the cases where the matrix part of the term pair is or isn't a reference, and also
// mutable VerticalBlockMatrix& Ab; // assignment from boost::cref_list_of, where the term ends up wrapped in a assign_reference<T>
// mutable DenseIndex& i; // that is implicitly convertible to T&. This was introduced to work around a problem where
// _fillTerm(std::vector<Key>& keys, VerticalBlockMatrix& Ab, DenseIndex& i) // BOOST_FOREACH over terms did not work on GCC.
// : keys(keys), Ab(Ab), i(i) {} struct _fillTerm {
// template<class TERM> void operator()(const TERM& term) const std::vector<Key>& keys;
// { VerticalBlockMatrix& Ab;
// // Check block rows DenseIndex& i;
// if(term.second.rows() != Ab.rows()) _fillTerm(std::vector<Key>& keys, VerticalBlockMatrix& Ab, DenseIndex& i)
// throw InvalidMatrixBlock(Ab.rows(), term.second.rows()); : keys(keys), Ab(Ab), i(i) {}
// // Assign key and matrix
// keys[i] = term.first; template<class MATRIX>
// Ab(i) = term.second; void operator()(const std::pair<Key, const MATRIX&>& term) const
// // Increment block index {
// ++ i; // Check block rows
// } if(term.second.rows() != Ab.rows())
//}; throw InvalidMatrixBlock(Ab.rows(), term.second.rows());
// Assign key and matrix
keys[i] = term.first;
Ab(i) = term.second;
// Increment block index
++ i;
}
template<class MATRIX>
void operator()(const std::pair<int, const MATRIX&>& term) const
{
operator()(std::pair<Key, const MATRIX&>(term));
}
template<class MATRIX>
void operator()(const std::pair<Key, MATRIX>& term) const
{
operator()(std::pair<Key, const MATRIX&>(term));
}
template<class MATRIX>
void operator()(const std::pair<int, MATRIX>& term) const
{
operator()(std::pair<Key, const MATRIX&>(term));
}
template<class T>
void operator()(boost::assign_detail::assign_reference<T> assignReference) const
{
operator()(assignReference.get_ref());
}
};
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -107,21 +139,8 @@ namespace gtsam {
} }
// Check and add terms // Check and add terms
//DenseIndex i = 0; // For block index
//br::for_each(terms, _fillTerm(Base::keys_, Ab_, i));
typedef std::pair<Key, Matrix> Term;
DenseIndex i = 0; // For block index DenseIndex i = 0; // For block index
BOOST_FOREACH(const Term& term, terms) { br::for_each(terms, _fillTerm(Base::keys_, Ab_, i));
// Check block rows
if(term.second.rows() != b.size())
throw InvalidMatrixBlock(b.size(), term.second.rows());
// Assign key and matrix
Base::keys_[i] = term.first;
Ab_(i) = term.second;
// Increment block index
++ i;
}
// Assign RHS vector // Assign RHS vector
getb() = b; getb() = b;