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 {
// 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<JacobianFactor>(linFactor);
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
* - 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;