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

Prepare VerticalBlockMatrix for filling

release/4.3a0
dellaert 2014-10-14 11:13:49 +02:00
parent 027759300d
commit f3e1561105
1 changed files with 22 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -42,6 +42,11 @@ public:
const T& measurement, const Expression<T>& expression) : const T& measurement, const Expression<T>& expression) :
NoiseModelFactor(noiseModel, expression.keys()), // NoiseModelFactor(noiseModel, expression.keys()), //
measurement_(measurement), expression_(expression) { measurement_(measurement), expression_(expression) {
if (!noiseModel)
throw std::invalid_argument("ExpressionFactor: no NoiseModel.");
if (noiseModel->dim() != T::dimension)
throw std::invalid_argument(
"ExpressionFactor was created with a NoiseModel of incorrect dimension.");
} }
/** /**
@ -72,35 +77,32 @@ public:
if (!this->active(x)) if (!this->active(x))
return boost::shared_ptr<JacobianFactor>(); return boost::shared_ptr<JacobianFactor>();
// Evaluate error to get Jacobians and RHS vector b
JacobianMap terms;
T value = expression_.value(x, terms);
Vector b = -measurement_.localCoordinates(value);
// check(noiseModel_, b); // TODO: use, but defined in NonlinearFactor.cpp
// Whiten the corresponding system now
// TODO ! this->noiseModel_->WhitenSystem(A, b);
// Get dimensions of matrices // Get dimensions of matrices
std::map<Key,size_t> map = expression_.dimensions(); std::map<Key, size_t> map = expression_.dimensions();
size_t n = map.size(); size_t n = map.size();
// Get actual diemnsions. TODO: why can't we pass map | map_values directly? // Get actual dimensions. TODO: why can't we pass map | map_values directly?
std::vector<size_t> dims(n); std::vector<size_t> dims(n);
boost::copy(map | map_values, dims.begin()); boost::copy(map | map_values, dims.begin());
// Construct block matrix, is of right size but un-initialized // Construct block matrix, is of right size but un-initialized
VerticalBlockMatrix Ab(dims, b.size(), true); VerticalBlockMatrix Ab(dims, T::dimension, true);
// Check and add terms // Create and zero out blocks to be passed to expression_
DenseIndex i = 0; // For block index DenseIndex i = 0; // For block index
for (JacobianMap::const_iterator it = terms.begin(); it != terms.end(); typedef std::pair<Key, size_t> Pair;
++it) { BOOST_FOREACH(const Pair& keyValue, map) {
const std::pair<Key, Matrix>& term = *it; Ab(i++) = zeros(T::dimension, keyValue.second);
const Matrix& Ai = term.second;
Ab(i++) = Ai;
} }
// Evaluate error to get Jacobians and RHS vector b
// JacobianMap terms;
T value = expression_.value(x);
Vector b = -measurement_.localCoordinates(value);
// Whiten the corresponding system now
// TODO ! this->noiseModel_->WhitenSystem(A, b);
// Fill in RHS // Fill in RHS
Ab(n).col(0) = b; Ab(n).col(0) = b;
@ -109,7 +111,8 @@ public:
noiseModel::Constrained::shared_ptr constrained = // noiseModel::Constrained::shared_ptr constrained = //
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_); boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
if (constrained) { if (constrained) {
return boost::make_shared<JacobianFactor>(map | map_keys, Ab, constrained->unit()); return boost::make_shared<JacobianFactor>(map | map_keys, Ab,
constrained->unit());
} else } else
return boost::make_shared<JacobianFactor>(map | map_keys, Ab); return boost::make_shared<JacobianFactor>(map | map_keys, Ab);
} }