From 6d6ee8debc1b6cb22da9d7e7c635048a707b6f6d Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Tue, 10 Sep 2013 14:09:25 +0000 Subject: [PATCH] Fixed bug with linear factor not getting cloned on linearization, updated comments to reflect lack of ordering --- gtsam/nonlinear/LinearContainerFactor.cpp | 7 ++++--- gtsam/nonlinear/LinearContainerFactor.h | 10 ++++------ 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/gtsam/nonlinear/LinearContainerFactor.cpp b/gtsam/nonlinear/LinearContainerFactor.cpp index 238fc9d41..8cae3d3d3 100644 --- a/gtsam/nonlinear/LinearContainerFactor.cpp +++ b/gtsam/nonlinear/LinearContainerFactor.cpp @@ -104,8 +104,10 @@ size_t LinearContainerFactor::dim() const { /* ************************************************************************* */ GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) const { + // Clone factor and update as necessary + GaussianFactor::shared_ptr linFactor = factor_->clone(); if (!hasLinearizationPoint()) - return factor_; + return linFactor; // Extract subset of values Values subsetC; @@ -115,8 +117,7 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) con // Determine delta between linearization points using new ordering VectorValues delta = linearizationPoint_->localCoordinates(subsetC); - // clone and reorder linear factor to final ordering - GaussianFactor::shared_ptr linFactor = factor_->clone(); + // Apply changes due to relinearization if (isJacobian()) { JacobianFactor::shared_ptr jacFactor = boost::dynamic_pointer_cast(linFactor); jacFactor->getb() = -jacFactor->unweighted_error(delta); diff --git a/gtsam/nonlinear/LinearContainerFactor.h b/gtsam/nonlinear/LinearContainerFactor.h index 379821cfe..d1420d029 100644 --- a/gtsam/nonlinear/LinearContainerFactor.h +++ b/gtsam/nonlinear/LinearContainerFactor.h @@ -79,9 +79,8 @@ public: /** * Linearize to a GaussianFactor, with method depending on the presence of a linearizationPoint - * - With no linearization point, returns a reordered, but numerically identical, - * version of the existing stored linear factor - * - With a linearization point provided, returns a reordered and relinearized version of + * - With no linearization point, returns a cloned version of the stored linear factor. + * - With a linearization point provided, returns a relinearized version of * the linearized factor. * * The relinearization approach used computes a linear delta between the original linearization @@ -97,13 +96,12 @@ public: GaussianFactor::shared_ptr linearize(const Values& c) const; /** - * Creates an anti-factor directly and performs rekeying due to ordering + * Creates an anti-factor directly */ GaussianFactor::shared_ptr negateToGaussian() const; /** - * Creates the equivalent anti-factor as another LinearContainerFactor, - * so it remains independent of ordering. + * Creates the equivalent anti-factor as another LinearContainerFactor. */ NonlinearFactor::shared_ptr negateToNonlinear() const;