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

Merged in fix/sparse_jacobian_output (pull request #121) 

fixed sparseJacobian() to use a std::map such that it works with symbol keys
release/4.3a0
Chris Beall 2015-06-17 12:17:59 -04:00
parent b68f763fe7 a9339baef0
commit 19bc1cd686
1 changed files with 20 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -123,26 +123,32 @@ namespace gtsam {
/* ************************************************************************* */
vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian() const {
// First find dimensions of each variable
vector<size_t> dims;
typedef std::map<Key, size_t> KeySizeMap;
KeySizeMap dims;
BOOST_FOREACH(const sharedFactor& factor, *this) {
for (GaussianFactor::const_iterator pos = factor->begin();
pos != factor->end(); ++pos) {
if (dims.size() <= *pos)
dims.resize(*pos + 1, 0);
dims[*pos] = factor->getDim(pos);
if (!static_cast<bool>(factor)) continue;
for (GaussianFactor::const_iterator key = factor->begin();
key != factor->end(); ++key) {
dims[*key] = factor->getDim(key);
}
}
// Compute first scalar column of each variable
vector<size_t> columnIndices(dims.size() + 1, 0);
for (size_t j = 1; j < dims.size() + 1; ++j)
columnIndices[j] = columnIndices[j - 1] + dims[j - 1];
size_t currentColIndex = 0;
KeySizeMap columnIndices = dims;
BOOST_FOREACH(const KeySizeMap::value_type& col, dims) {
columnIndices[col.first] = currentColIndex;
currentColIndex += dims[col.first];
}
// Iterate over all factors, adding sparse scalar entries
typedef boost::tuple<size_t, size_t, double> triplet;
vector<triplet> entries;
size_t row = 0;
BOOST_FOREACH(const sharedFactor& factor, *this) {
if (!static_cast<bool>(factor)) continue;
// Convert to JacobianFactor if necessary
JacobianFactor::shared_ptr jacobianFactor(
boost::dynamic_pointer_cast<JacobianFactor>(factor));
@ -159,11 +165,11 @@ namespace gtsam {
// Whiten the factor and add entries for it
// iterate over all variables in the factor
const JacobianFactor whitened(jacobianFactor->whiten());
for (JacobianFactor::const_iterator pos = whitened.begin();
pos < whitened.end(); ++pos) {
JacobianFactor::constABlock whitenedA = whitened.getA(pos);
for (JacobianFactor::const_iterator key = whitened.begin();
key < whitened.end(); ++key) {
JacobianFactor::constABlock whitenedA = whitened.getA(key);
// find first column index for this key
size_t column_start = columnIndices[*pos];
size_t column_start = columnIndices[*key];
for (size_t i = 0; i < (size_t) whitenedA.rows(); i++)
for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) {
double s = whitenedA(i, j);
@ -173,7 +179,7 @@ namespace gtsam {
}
JacobianFactor::constBVector whitenedb(whitened.getb());
size_t bcolumn = columnIndices.back();
size_t bcolumn = currentColIndex;
for (size_t i = 0; i < (size_t) whitenedb.size(); i++)
entries.push_back(boost::make_tuple(row + i, bcolumn, whitenedb(i)));