diff --git a/gtsam/linear/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index a20c09a43..0d435ce17 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -115,6 +115,13 @@ namespace gtsam { IndexFactor::permuteWithInverse(inversePermutation); } + /** + * Construct the corresponding anti-factor to negate information + * stored stored in this factor. + * @return a HessianFactor with negated Hessian matrices + */ + virtual GaussianFactor::shared_ptr negate() const = 0; + private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 86938a9bf..cdf597411 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -511,4 +511,24 @@ GaussianConditional::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFr return conditional; } +/* ************************************************************************* */ +GaussianFactor::shared_ptr HessianFactor::negate() const { + // Copy Hessian Blocks from Hessian factor and invert + std::vector js; + std::vector Gs; + std::vector gs; + double f; + js.insert(js.end(), begin(), end()); + for(size_t i = 0; i < js.size(); ++i){ + for(size_t j = i; j < js.size(); ++j){ + Gs.push_back( -info(begin()+i, begin()+j) ); + } + gs.push_back( -linearTerm(begin()+i) ); + } + f = -constantTerm(); + + // Create the Anti-Hessian Factor from the negated blocks + return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f)); +} + } // gtsam diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index f90a33c5e..74216f0f0 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -237,6 +237,13 @@ namespace gtsam { /** Return the number of columns and rows of the Hessian matrix */ size_t rows() const { return info_.rows(); } + /** + * Construct the corresponding anti-factor to negate information + * stored stored in this factor. + * @return a HessianFactor with negated Hessian matrices + */ + virtual GaussianFactor::shared_ptr negate() const; + /** Return a view of the block at (j1,j2) of the upper-triangular part of the * information matrix \f$ H \f$, no data is copied. See HessianFactor class documentation * above to explain that only the upper-triangular part of the information matrix is stored diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 038f8c718..b241ff24a 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -385,6 +385,12 @@ namespace gtsam { return result; } + /* ************************************************************************* */ + GaussianFactor::shared_ptr JacobianFactor::negate() const { + HessianFactor hessian(*this); + return hessian.negate(); + } + /* ************************************************************************* */ GaussianConditional::shared_ptr JacobianFactor::eliminateFirst() { return this->eliminate(1); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 0ac085987..660eb25f7 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -169,6 +169,13 @@ namespace gtsam { */ virtual Matrix computeInformation() const; + /** + * Construct the corresponding anti-factor to negate information + * stored stored in this factor. + * @return a HessianFactor with negated Hessian matrices + */ + virtual GaussianFactor::shared_ptr negate() const; + /** Check if the factor contains no information, i.e. zero rows. This does * not necessarily mean that the factor involves no variables (to check for * involving no variables use keys().empty()). diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h index 1e0d64db7..19f73c8b9 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -99,30 +99,8 @@ namespace gtsam { // Generate the linearized factor from the contained nonlinear factor GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c, ordering); - // Cast the GaussianFactor to a Hessian - HessianFactor::shared_ptr hessianFactor = boost::dynamic_pointer_cast(gaussianFactor); - - // If the cast fails, convert it to a Hessian - if(!hessianFactor){ - hessianFactor = HessianFactor::shared_ptr(new HessianFactor(*gaussianFactor)); - } - - // Copy Hessian Blocks from Hessian factor and invert - std::vector js; - std::vector Gs; - std::vector gs; - double f; - js.insert(js.end(), hessianFactor->begin(), hessianFactor->end()); - for(size_t i = 0; i < js.size(); ++i){ - for(size_t j = i; j < js.size(); ++j){ - Gs.push_back( -hessianFactor->info(hessianFactor->begin()+i, hessianFactor->begin()+j) ); - } - gs.push_back( -hessianFactor->linearTerm(hessianFactor->begin()+i) ); - } - f = -hessianFactor->constantTerm(); - - // Create the Anti-Hessian Factor from the negated blocks - return HessianFactor::shared_ptr(new HessianFactor(js, Gs, gs, f)); + // return the negated version of the factor + return gaussianFactor->negate(); }