diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 6db6330b0..a44f5c682 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -261,12 +261,10 @@ HessianFactor::HessianFactor(const FactorGraph& factors, if (debug) cout << "Combining " << factors.size() << " factors" << endl; BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { - shared_ptr hessian(boost::dynamic_pointer_cast(factor)); - if (hessian) - updateATA(*hessian, scatter); - else { - JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast(factor)); - if (jacobianFactor) + if(factor) { + if(shared_ptr hessian = boost::dynamic_pointer_cast(factor)) + updateATA(*hessian, scatter); + else if(JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast(factor)) updateATA(*jacobianFactor, scatter); else throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian"); @@ -360,7 +358,6 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte // Apply updates to the upper triangle tic(3, "update"); - assert(this->info_.nBlocks() - 1 == scatter.size()); for(size_t j2=0; j2info_.nBlocks()-1 : slots[j2]; for(size_t j1=0; j1<=j2; ++j1) { @@ -437,7 +434,6 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt // Apply updates to the upper triangle tic(3, "update"); - assert(this->info_.nBlocks() - 1 == scatter.size()); for(size_t j2=0; j2info_.nBlocks()-1 : slots[j2]; for(size_t j1=0; j1<=j2; ++j1) { diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 74216f0f0..2e9b67c2a 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -120,20 +120,6 @@ namespace gtsam { InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1] BlockInfo info_; ///< The block view of the full information matrix. - /** Update the factor by adding the information from the JacobianFactor - * (used internally during elimination). - * @param update The JacobianFactor containing the new information to add - * @param scatter A mapping from variable index to slot index in this HessianFactor - */ - void updateATA(const JacobianFactor& update, const Scatter& scatter); - - /** Update the factor by adding the information from the HessianFactor - * (used internally during elimination). - * @param update The HessianFactor containing the new information to add - * @param scatter A mapping from variable index to slot index in this HessianFactor - */ - void updateATA(const HessianFactor& update, const Scatter& scatter); - public: typedef boost::shared_ptr shared_ptr; ///< A shared_ptr to this @@ -315,6 +301,20 @@ namespace gtsam { /** split partially eliminated factor */ boost::shared_ptr splitEliminatedFactor(size_t nrFrontals); + /** Update the factor by adding the information from the JacobianFactor + * (used internally during elimination). + * @param update The JacobianFactor containing the new information to add + * @param scatter A mapping from variable index to slot index in this HessianFactor + */ + void updateATA(const JacobianFactor& update, const Scatter& scatter); + + /** Update the factor by adding the information from the HessianFactor + * (used internally during elimination). + * @param update The HessianFactor containing the new information to add + * @param scatter A mapping from variable index to slot index in this HessianFactor + */ + void updateATA(const HessianFactor& update, const Scatter& scatter); + /** assert invariants */ void assertInvariants() const;