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

Fixed LinearContainerFactor linearization - now passes tests

release/4.3a0
Alex Cunningham 2013-08-16 19:42:25 +00:00
parent af173c1a1c
commit 80de8c43a1
1 changed files with 1 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/nonlinear/LinearContainerFactor.cpp
View File

@ -124,7 +124,7 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) con
HessianFactor::shared_ptr hesFactor = boost::dynamic_pointer_cast<HessianFactor>(linFactor);
size_t dim = hesFactor->linearTerm().size();
Eigen::Block<HessianFactor::Block> Gview = hesFactor->info().block(0, 0, dim, dim);
Vector deltaVector = delta.vector();
Vector deltaVector = delta.vector(keys());
Vector G_delta = Gview.selfadjointView<Eigen::Upper>() * deltaVector;
hesFactor->constantTerm() += deltaVector.dot(G_delta) - 2.0 * deltaVector.dot(hesFactor->linearTerm());
hesFactor->linearTerm() -= G_delta;