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

Fixed bug with linear factor not getting cloned on linearization, updated comments to reflect lack of ordering

release/4.3a0
Alex Cunningham 2013-09-10 14:09:25 +00:00
parent bbb6ff90fd
commit 6d6ee8debc
2 changed files with 8 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/nonlinear/LinearContainerFactor.cpp
View File

@ -104,8 +104,10 @@ size_t LinearContainerFactor::dim() const {
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) const { GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) const {
// Clone factor and update as necessary
GaussianFactor::shared_ptr linFactor = factor_->clone();
if (!hasLinearizationPoint()) if (!hasLinearizationPoint())
return factor_; return linFactor;
// Extract subset of values // Extract subset of values
Values subsetC; Values subsetC;
@ -115,8 +117,7 @@ GaussianFactor::shared_ptr LinearContainerFactor::linearize(const Values& c) con
// Determine delta between linearization points using new ordering // Determine delta between linearization points using new ordering
VectorValues delta = linearizationPoint_->localCoordinates(subsetC); VectorValues delta = linearizationPoint_->localCoordinates(subsetC);
// clone and reorder linear factor to final ordering // Apply changes due to relinearization
GaussianFactor::shared_ptr linFactor = factor_->clone();
if (isJacobian()) { if (isJacobian()) {
JacobianFactor::shared_ptr jacFactor = boost::dynamic_pointer_cast<JacobianFactor>(linFactor); JacobianFactor::shared_ptr jacFactor = boost::dynamic_pointer_cast<JacobianFactor>(linFactor);
jacFactor->getb() = -jacFactor->unweighted_error(delta); jacFactor->getb() = -jacFactor->unweighted_error(delta);

10
gtsam/nonlinear/LinearContainerFactor.h
View File

@ -79,9 +79,8 @@ public:
/** /**
* Linearize to a GaussianFactor, with method depending on the presence of a linearizationPoint * Linearize to a GaussianFactor, with method depending on the presence of a linearizationPoint
* - With no linearization point, returns a reordered, but numerically identical, * - With no linearization point, returns a cloned version of the stored linear factor.
* version of the existing stored linear factor * - With a linearization point provided, returns a relinearized version of
* - With a linearization point provided, returns a reordered and relinearized version of
* the linearized factor. * the linearized factor.
* *
* The relinearization approach used computes a linear delta between the original linearization * 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; 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; GaussianFactor::shared_ptr negateToGaussian() const;
/** /**
* Creates the equivalent anti-factor as another LinearContainerFactor, * Creates the equivalent anti-factor as another LinearContainerFactor.
* so it remains independent of ordering.
*/ */
NonlinearFactor::shared_ptr negateToNonlinear() const; NonlinearFactor::shared_ptr negateToNonlinear() const;