Inlined VerticalBlockMatrix construction
parent
ed62271f81
commit
fea2eb0b5f
|
@ -68,7 +68,7 @@ public:
|
|||
|
||||
// Evaluate error to get Jacobians and RHS vector b
|
||||
Augmented<T> augmented = expression_.augmented(x);
|
||||
Vector b = - measurement_.localCoordinates(augmented.value());
|
||||
Vector b = -measurement_.localCoordinates(augmented.value());
|
||||
// check(noiseModel_, b); // TODO: use, but defined in NonlinearFactor.cpp
|
||||
|
||||
// Whiten the corresponding system now
|
||||
|
@ -76,15 +76,44 @@ public:
|
|||
|
||||
// Terms, needed to create JacobianFactor below, are already here!
|
||||
const JacobianMap& terms = augmented.jacobians();
|
||||
size_t n = terms.size();
|
||||
|
||||
// Get dimensions of matrices
|
||||
std::vector<size_t> dimensions;
|
||||
dimensions.reserve(n);
|
||||
std::vector<Key> keys;
|
||||
keys.reserve(n);
|
||||
for (JacobianMap::const_iterator it = terms.begin(); it != terms.end();
|
||||
++it) {
|
||||
const std::pair<Key, Matrix>& term = *it;
|
||||
Key key = term.first;
|
||||
const Matrix& Ai = term.second;
|
||||
dimensions.push_back(Ai.cols());
|
||||
keys.push_back(key);
|
||||
}
|
||||
|
||||
// Construct block matrix
|
||||
VerticalBlockMatrix Ab(dimensions, b.size(), true);
|
||||
|
||||
// Check and add terms
|
||||
DenseIndex i = 0; // For block index
|
||||
for (JacobianMap::const_iterator it = terms.begin(); it != terms.end();
|
||||
++it) {
|
||||
const std::pair<Key, Matrix>& term = *it;
|
||||
const Matrix& Ai = term.second;
|
||||
Ab(i++) = Ai;
|
||||
}
|
||||
|
||||
Ab(n).col(0) = b;
|
||||
|
||||
// TODO pass unwhitened + noise model to Gaussian factor
|
||||
// For now, only linearized constrained factors have noise model at linear level!!!
|
||||
noiseModel::Constrained::shared_ptr constrained = //
|
||||
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
|
||||
if (constrained) {
|
||||
return boost::make_shared<JacobianFactor>(terms, b, constrained->unit());
|
||||
return boost::make_shared<JacobianFactor>(keys, Ab, constrained->unit());
|
||||
} else
|
||||
return boost::make_shared<JacobianFactor>(terms, b);
|
||||
return boost::make_shared<JacobianFactor>(keys, Ab);
|
||||
}
|
||||
};
|
||||
// ExpressionFactor
|
||||
|
|
Loading…
Reference in New Issue