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;