From 80de8c43a12970410f4e8377cbf505281c9699bc Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 16 Aug 2013 19:42:25 +0000 Subject: [PATCH] Fixed LinearContainerFactor linearization - now passes tests --- gtsam/nonlinear/LinearContainerFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp index 6fda8e7ed..238fc9d41 100644 --- a/gtsam/nonlinear/LinearContainerFactor.cpp +++ b/gtsam/nonlinear/LinearContainerFactor.cpp @@ -124,7 +124,7 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) con HessianFactor::shared_ptr hesFactor = boost::dynamic_pointer_cast(linFactor); size_t dim = hesFactor->linearTerm().size(); Eigen::Block Gview = hesFactor->info().block(0, 0, dim, dim); - Vector deltaVector = delta.vector(); + Vector deltaVector = delta.vector(keys()); Vector G_delta = Gview.selfadjointView() * deltaVector; hesFactor->constantTerm() += deltaVector.dot(G_delta) - 2.0 * deltaVector.dot(hesFactor->linearTerm()); hesFactor->linearTerm() -= G_delta;