diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 5f78df004..a9bac6065 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -82,18 +82,14 @@ public: // Get dimensions of matrices std::vector dimensions; dimensions.reserve(n); - std::vector keys; - keys.reserve(n); for (JacobianMap::const_iterator it = terms.begin(); it != terms.end(); ++it) { const std::pair& term = *it; - Key key = term.first; const Matrix& Ai = term.second; 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); // Check and add terms @@ -107,6 +103,9 @@ public: Ab(n).col(0) = b; + // Get keys to construct factor + std::set keys = expression_.keys(); + // 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 = //