diff --git a/gtsam/linear/GaussianFactor.cpp b/gtsam/linear/GaussianFactor.cpp index fc7da09a6..8a306d73a 100644 --- a/gtsam/linear/GaussianFactor.cpp +++ b/gtsam/linear/GaussianFactor.cpp @@ -96,43 +96,43 @@ namespace gtsam { const bool checkCholesky = ISDEBUG("GaussianFactor::CombineAndEliminate Check Cholesky"); - FactorGraph hessianFactors; - tic(1, "convert to Hessian"); - hessianFactors.reserve(factors.size()); - BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { - if(factor) { - HessianFactor::shared_ptr hessianFactor(boost::dynamic_pointer_cast(factor)); - if(hessianFactor) - hessianFactors.push_back(hessianFactor); - else { - JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast(factor)); - if(!jacobianFactor) throw std::invalid_argument( - "In GaussianFactor::CombineAndEliminate, factor is neither a JacobianFactor nor a HessianFactor."); - HessianFactor::shared_ptr convertedHessianFactor; - try { - convertedHessianFactor.reset(new HessianFactor(*jacobianFactor)); - if(checkCholesky) - if(!assert_equal(HessianFactor(*jacobianFactor), HessianFactor(JacobianFactor(*convertedHessianFactor)), 1e-3)) - throw runtime_error("Conversion between Jacobian and Hessian incorrect"); - } catch(const exception& e) { - cout << "Exception converting to Hessian: " << e.what() << endl; - jacobianFactor->print("jacobianFactor: "); - convertedHessianFactor->print("convertedHessianFactor: "); - SETDEBUG("choleskyPartial", true); - SETDEBUG("choleskyCareful", true); - HessianFactor(JacobianFactor(*convertedHessianFactor)).print("Jacobian->Hessian->Jacobian->Hessian: "); - throw; - } - hessianFactors.push_back(convertedHessianFactor); - } - } - } - toc(1, "convert to Hessian"); +// FactorGraph hessianFactors; +// tic(1, "convert to Hessian"); +// hessianFactors.reserve(factors.size()); +// BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { +// if(factor) { +// HessianFactor::shared_ptr hessianFactor(boost::dynamic_pointer_cast(factor)); +// if(hessianFactor) +// hessianFactors.push_back(hessianFactor); +// else { +// JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast(factor)); +// if(!jacobianFactor) throw std::invalid_argument( +// "In GaussianFactor::CombineAndEliminate, factor is neither a JacobianFactor nor a HessianFactor."); +// HessianFactor::shared_ptr convertedHessianFactor; +// try { +// convertedHessianFactor.reset(new HessianFactor(*jacobianFactor)); +// if(checkCholesky) +// if(!assert_equal(HessianFactor(*jacobianFactor), HessianFactor(JacobianFactor(*convertedHessianFactor)), 1e-3)) +// throw runtime_error("Conversion between Jacobian and Hessian incorrect"); +// } catch(const exception& e) { +// cout << "Exception converting to Hessian: " << e.what() << endl; +// jacobianFactor->print("jacobianFactor: "); +// convertedHessianFactor->print("convertedHessianFactor: "); +// SETDEBUG("choleskyPartial", true); +// SETDEBUG("choleskyCareful", true); +// HessianFactor(JacobianFactor(*convertedHessianFactor)).print("Jacobian->Hessian->Jacobian->Hessian: "); +// throw; +// } +// hessianFactors.push_back(convertedHessianFactor); +// } +// } +// } +// toc(1, "convert to Hessian"); pair ret; try { tic(2, "Hessian CombineAndEliminate"); - ret = HessianFactor::CombineAndEliminate(hessianFactors, nrFrontals); + ret = HessianFactor::CombineAndEliminate(factors, nrFrontals); toc(2, "Hessian CombineAndEliminate"); } catch(const exception& e) { cout << "Exception in HessianFactor::CombineAndEliminate: " << e.what() << endl; @@ -142,7 +142,7 @@ namespace gtsam { SETDEBUG("JacobianFactor::Combine", true); SETDEBUG("choleskyPartial", true); factors.print("Combining factors: "); - HessianFactor::CombineAndEliminate(hessianFactors, nrFrontals); + HessianFactor::CombineAndEliminate(factors, nrFrontals); throw; } @@ -185,7 +185,7 @@ namespace gtsam { SETDEBUG("JacobianFactor::Combine", true); jacobianFactors.print("Jacobian Factors: "); JacobianFactor::CombineAndEliminate(jacobianFactors, nrFrontals); - HessianFactor::CombineAndEliminate(hessianFactors, nrFrontals); + HessianFactor::CombineAndEliminate(factors, nrFrontals); factors.print("Combining factors: "); throw runtime_error("Cholesky and QR do not agree"); diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 9f23bf4b8..6c4aaf031 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -202,7 +202,7 @@ namespace gtsam { } /* ************************************************************************* */ - static FastMap findScatterAndDims(const FactorGraph& factors) { + static FastMap findScatterAndDims(const FactorGraph& factors) { static const bool debug = false; @@ -213,8 +213,8 @@ namespace gtsam { Scatter scatter; // First do the set union. - BOOST_FOREACH(const HessianFactor::shared_ptr& factor, factors) { - for(HessianFactor::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) { + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { + for(GaussianFactor::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) { scatter.insert(make_pair(*variable, SlotEntry(0, factor->getDim(variable)))); } } @@ -254,8 +254,11 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte update.print("with: "); } + Eigen::Map information(&matrix_(0,0), matrix_.size1(), matrix_.size2()); + Eigen::Map updateInform(&update.matrix_(0,0), update.matrix_.size1(), update.matrix_.size2()); + // Apply updates to the upper triangle - tic(2, "update"); + tic(3, "update"); assert(this->info_.nBlocks() - 1 == scatter.size()); for(size_t j2=0; j2info_.nBlocks()-1 : slots[j2]; @@ -264,36 +267,169 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte if(slot2 > slot1) { if(debug) cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; -// ublas::noalias(this->info_(slot1, slot2)) += update.info_(j1,j2); - Eigen::Map(&matrix_(0,0), matrix_.size1(), matrix_.size2()).block( - info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).size1(), info_(slot1,slot2).size2()) += - Eigen::Map(&update.matrix_(0,0), update.matrix_.size1(), update.matrix_.size2()).block( - update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).size1(), update.info_(j1,j2).size2()); + information.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).size1(), info_(slot1,slot2).size2()).noalias() += + updateInform.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).size1(), update.info_(j1,j2).size2()); } else if(slot1 > slot2) { if(debug) cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl; -// ublas::noalias(this->info_(slot2, slot1)) += ublas::trans(update.info_(j1,j2)); - Eigen::Map(&matrix_(0,0), matrix_.size1(), matrix_.size2()).block( - info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).size1(), info_(slot2,slot1).size2()) += - Eigen::Map(&update.matrix_(0,0), update.matrix_.size1(), update.matrix_.size2()).block( - update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).size1(), update.info_(j1,j2).size2()).transpose(); + information.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).size1(), info_(slot2,slot1).size2()).noalias() += + updateInform.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).size1(), update.info_(j1,j2).size2()).transpose(); } else { if(debug) cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; -// Block thisBlock(this->info_(slot1, slot2)); -// constBlock updateBlock(update.info_(j1,j2)); -// ublas::noalias(ublas::symmetric_adaptor(thisBlock)) += -// ublas::symmetric_adaptor(updateBlock); - Eigen::Map(&matrix_(0,0), matrix_.size1(), matrix_.size2()).block( - info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).size1(), info_(slot1,slot2).size2()).triangularView() += - Eigen::Map(&update.matrix_(0,0), update.matrix_.size1(), update.matrix_.size2()).block( - update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).size1(), update.info_(j1,j2).size2()); + information.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).size1(), info_(slot1,slot2).size2()).triangularView() += + updateInform.block(update.info_.offset(j1), update.info_.offset(j2), update.info_(j1,j2).size1(), update.info_(j1,j2).size2()); } if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n"; if(debug) this->print(); } } - toc(2, "update"); + toc(3, "update"); +} + +void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatter) { + + // This function updates 'combined' with the information in 'update'. + // 'scatter' maps variables in the update factor to slots in the combined + // factor. + + const bool debug = ISDEBUG("updateATA"); + + // First build an array of slots + tic(1, "slots"); + size_t slots[update.size()]; + size_t slot = 0; + BOOST_FOREACH(Index j, update) { + slots[slot] = scatter.find(j)->second.slot; + ++ slot; + } + toc(1, "slots"); + + tic(2, "form A^T*A"); + if(update.model_->isConstrained()) + throw invalid_argument("Cannot update HessianFactor from JacobianFactor with constrained noise model"); + + if(debug) { + this->print("Updating this: "); + update.print("with: "); + } + + Eigen::Map information(&matrix_(0,0), matrix_.size1(), matrix_.size2()); + Eigen::Map updateAf(&update.matrix_(0,0), update.matrix_.size1(), update.matrix_.size2()); + Eigen::Block updateA(updateAf.block( + update.Ab_.rowStart(),update.Ab_.offset(0), update.Ab_.full().size1(), update.Ab_.full().size2())); + + Eigen::MatrixXd updateInform; + if(boost::dynamic_pointer_cast(update.model_)) { + updateInform.noalias() = updateA.transpose() * updateA; + } else { + noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast(update.model_)); + if(diagonal) { + typeof(Eigen::Map(&update.model_->invsigmas()(0),0).asDiagonal()) R( + Eigen::Map(&update.model_->invsigmas()(0),update.model_->dim()).asDiagonal()); + updateInform.noalias() = updateA.transpose() * R * R * updateA; + } else + throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal"); + } + toc(2, "form A^T*A"); + + // 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) { + size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1]; + size_t off0 = update.Ab_.offset(0); + if(slot2 > slot1) { + if(debug) + cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; + information.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).size1(), info_(slot1,slot2).size2()).noalias() += + updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).size2(), update.Ab_(j2).size2()); + } else if(slot1 > slot2) { + if(debug) + cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl; + information.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).size1(), info_(slot2,slot1).size2()).noalias() += + updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).size2(), update.Ab_(j2).size2()).transpose(); + } else { + if(debug) + cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; + information.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).size1(), info_(slot1,slot2).size2()).triangularView() += + updateInform.block(update.Ab_.offset(j1)-off0, update.Ab_.offset(j2)-off0, update.Ab_(j1).size2(), update.Ab_(j2).size2()); + } + if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n"; + if(debug) this->print(); + } + } + toc(3, "update"); + +// Eigen::Map information(&matrix_(0,0), matrix_.size1(), matrix_.size2()); +// Eigen::Map updateA(&update.matrix_(0,0), update.matrix_.size1(), update.matrix_.size2()); +// +// // Apply updates to the upper triangle +// tic(2, "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) { +// size_t slot1 = (j1 == update.size()) ? this->info_.nBlocks()-1 : slots[j1]; +// typedef typeof(updateA.block(0,0,0,0)) ABlock; +// ABlock A1(updateA.block(update.Ab_.rowStart(),update.Ab_.offset(j1), update.Ab_(j1).size1(),update.Ab_(j1).size2())); +// ABlock A2(updateA.block(update.Ab_.rowStart(),update.Ab_.offset(j2), update.Ab_(j2).size1(),update.Ab_(j2).size2())); +// if(slot2 > slot1) { +// if(debug) +// cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; +// if(boost::dynamic_pointer_cast(update.model_)) { +// information.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).size1(), info_(slot1,slot2).size2()) += +// A1.transpose() * A2; +// } else { +// noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast(update.model_)); +// if(diagonal) { +// typeof(Eigen::Map(&update.model_->invsigmas()(0),0).asDiagonal()) R( +// Eigen::Map(&update.model_->invsigmas()(0),update.model_->dim()).asDiagonal()); +// information.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).size1(), info_(slot1,slot2).size2()).noalias() += +// A1.transpose() * R * R * A2; +// } else +// throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal"); +// } +// } else if(slot1 > slot2) { +// if(debug) +// cout << "Updating (" << slot2 << "," << slot1 << ") from (" << j1 << "," << j2 << ")" << endl; +// if(boost::dynamic_pointer_cast(update.model_)) { +// information.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).size1(), info_(slot2,slot1).size2()).noalias() += +// A2.transpose() * A1; +// } else { +// noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast(update.model_)); +// if(diagonal) { +// typeof(Eigen::Map(&update.model_->invsigmas()(0),0).asDiagonal()) R( +// Eigen::Map(&update.model_->invsigmas()(0),update.model_->dim()).asDiagonal()); +// information.block(info_.offset(slot1), info_.offset(slot2), info_(slot1,slot2).size1(), info_(slot1,slot2).size2()).noalias() += +// A2.transpose() * R * R * A1; +// } else +// throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal"); +// } +// } else { +// if(debug) +// cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; +// if(boost::dynamic_pointer_cast(update.model_)) { +// information.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).size1(), info_(slot2,slot1).size2()).triangularView() += +// A1.transpose() * A1; +// } else { +// noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast(update.model_)); +// if(diagonal) { +// typeof(Eigen::Map(&update.model_->invsigmas()(0),0).asDiagonal()) R( +// Eigen::Map(&update.model_->invsigmas()(0),update.model_->dim()).asDiagonal()); +// information.block(info_.offset(slot2), info_.offset(slot1), info_(slot2,slot1).size1(), info_(slot2,slot1).size2()).triangularView() += +// A1.transpose() * R * R * A1; +// } else +// throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal"); +// } +// } +// if(debug) cout << "Updating block " << slot1 << "," << slot2 << " from block " << j1 << "," << j2 << "\n"; +// if(debug) this->print(); +// } +// } +// toc(2, "update"); } GaussianBayesNet::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFrontals, const vector& keys) { @@ -347,7 +483,7 @@ GaussianBayesNet::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFront /* ************************************************************************* */ pair HessianFactor::CombineAndEliminate( - const FactorGraph& factors, size_t nrFrontals) { + const FactorGraph& factors, size_t nrFrontals) { const bool debug = ISDEBUG("HessianFactor::CombineAndEliminate"); @@ -378,8 +514,17 @@ pair HessianFactor::Com ublas::noalias(combinedFactor->matrix_) = ublas::zero_matrix(combinedFactor->matrix_.size1(), combinedFactor->matrix_.size2()); toc(2, "zero"); tic(3, "update"); - BOOST_FOREACH(const HessianFactor::shared_ptr& factor, factors) { - combinedFactor->updateATA(*factor, scatter); + BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { + HessianFactor::shared_ptr hessianFactor(boost::dynamic_pointer_cast(factor)); + if(hessianFactor) + combinedFactor->updateATA(*hessianFactor, scatter); + else { + JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast(factor)); + if(jacobianFactor) + combinedFactor->updateATA(*jacobianFactor, scatter); + else + throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian"); + } } toc(3, "update"); if(debug) gtsam::print(combinedFactor->matrix_, "Ab' * Ab: "); diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 1de9a2f64..b86612aec 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -48,6 +48,7 @@ namespace gtsam { void assertInvariants() const; GaussianBayesNet::shared_ptr splitEliminatedFactor(size_t nrFrontals, const std::vector& keys); + void updateATA(const JacobianFactor& update, const Scatter& scatter); void updateATA(const HessianFactor& update, const Scatter& scatter); public: @@ -123,7 +124,7 @@ namespace gtsam { * Combine and eliminate several factors. */ static std::pair CombineAndEliminate( - const FactorGraph& factors, size_t nrFrontals=1); + const FactorGraph& factors, size_t nrFrontals=1); // Friend unit test classes friend class ::ConversionConstructorHessianFactorTest; diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 2bce79f2c..fa9f0713b 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -179,7 +179,6 @@ namespace gtsam { Ab_.rowEnd() = maxrank; model_ = noiseModel::Unit::Create(maxrank); - size_t varpos = 0; firstNonzeroBlocks_.resize(this->size1(), 0); // Sort keys diff --git a/gtsam/linear/tests/testGaussianJunctionTree.cpp b/gtsam/linear/tests/testGaussianJunctionTree.cpp index 7d68a3682..ae42c472c 100644 --- a/gtsam/linear/tests/testGaussianJunctionTree.cpp +++ b/gtsam/linear/tests/testGaussianJunctionTree.cpp @@ -67,9 +67,6 @@ GaussianFactorGraph createChain() { */ TEST( GaussianJunctionTree, eliminate ) { - - SETDEBUG("updateATA",true); - GaussianFactorGraph fg = createChain(); GaussianJunctionTree junctionTree(fg); BayesTree::sharedClique rootClique = junctionTree.eliminate(); diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 9206d300a..3a330d82a 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -88,7 +88,7 @@ TEST(HessianFactor, ConversionConstructor) { } /* ************************************************************************* */ -TEST_UNSAFE(GaussianFactor, CombineAndEliminate) +TEST(GaussianFactor, CombineAndEliminate) { Matrix A01 = Matrix_(3,3, 1.0, 0.0, 0.0, @@ -207,7 +207,7 @@ TEST(GaussianFactor, eliminate2 ) } /* ************************************************************************* */ -TEST_UNSAFE(GaussianFactor, eliminateUnsorted) { +TEST(GaussianFactor, eliminateUnsorted) { JacobianFactor::shared_ptr factor1( new JacobianFactor(0,