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

Removed "full VectorValues" version of HessianFactor::error

release/4.3a0
Richard Roberts 2012-12-18 14:20:49 +00:00
parent bd285fbb52
commit c7b9345aa1
1 changed files with 5 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/linear/HessianFactor.cpp
View File

@ -325,17 +325,11 @@ double HessianFactor::error(const VectorValues& c) const {
// error 0.5*(f - 2*x'*g + x'*G*x) // error 0.5*(f - 2*x'*g + x'*G*x)
const double f = constantTerm(); const double f = constantTerm();
double xtg = 0, xGx = 0; double xtg = 0, xGx = 0;
if (c.dim() == this->rows()) { // extract the relevant subset of the VectorValues
// If using the full vector values, this will reduce copying // NOTE may not be as efficient
xtg = c.vector().dot(linearTerm()); const Vector x = c.vector(this->keys());
xGx = c.vector().transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>() * c.asVector(); xtg = x.dot(linearTerm());
} else { xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>() * x;
// extract the relevant subset of the VectorValues
// NOTE may not be as efficient
const Vector x = c.vector(this->keys());
xtg = x.dot(linearTerm());
xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView<Eigen::Upper>() * x;
}
return 0.5 * (f - 2.0 * xtg + xGx); return 0.5 * (f - 2.0 * xtg + xGx);
} }