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

keys now from expression_

release/4.3a0
dellaert 2014-10-14 08:59:01 +02:00
parent 0a41b0a027
commit 781cc6daa9
1 changed files with 4 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -82,18 +82,14 @@ public:
// Get dimensions of matrices // Get dimensions of matrices
std::vector<size_t> dimensions; std::vector<size_t> dimensions;
dimensions.reserve(n); dimensions.reserve(n);
std::vector<Key> keys;
keys.reserve(n);
for (JacobianMap::const_iterator it = terms.begin(); it != terms.end(); for (JacobianMap::const_iterator it = terms.begin(); it != terms.end();
++it) { ++it) {
const std::pair<Key, Matrix>& term = *it; const std::pair<Key, Matrix>& term = *it;
Key key = term.first;
const Matrix& Ai = term.second; const Matrix& Ai = term.second;
dimensions.push_back(Ai.cols()); dimensions.push_back(Ai.cols());
keys.push_back(key);
} }
// Construct block matrix // Construct block matrix, is of right size but un-initialized
VerticalBlockMatrix Ab(dimensions, b.size(), true); VerticalBlockMatrix Ab(dimensions, b.size(), true);
// Check and add terms // Check and add terms
@ -107,6 +103,9 @@ public:
Ab(n).col(0) = b; Ab(n).col(0) = b;
// Get keys to construct factor
std::set<Key> keys = expression_.keys();
// TODO pass unwhitened + noise model to Gaussian factor // TODO pass unwhitened + noise model to Gaussian factor
// For now, only linearized constrained factors have noise model at linear level!!! // For now, only linearized constrained factors have noise model at linear level!!!
noiseModel::Constrained::shared_ptr constrained = // noiseModel::Constrained::shared_ptr constrained = //