diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index bff931050..d406fdc2c 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -16,6 +16,7 @@ * @created Dec 8, 2010 */ +#include #include #include #include @@ -50,19 +51,32 @@ using namespace boost::lambda; namespace gtsam { /* ************************************************************************* */ - HessianFactor::HessianFactor(const HessianFactor& gf) : - GaussianFactor(gf), info_(matrix_) { - info_.assignNoalias(gf.info_); + void HessianFactor::assertInvariants() const { + // Check for non-finite values + for(size_t i=0; i(matrix_)) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_); + assertInvariants(); } /* ************************************************************************* */ @@ -133,6 +153,7 @@ namespace gtsam { info_.assignNoalias(hf.info_); } else throw std::invalid_argument("In HessianFactor(const GaussianFactor& gf), gf is neither a JacobianFactor nor a HessianFactor"); + assertInvariants(); } /* ************************************************************************* */ @@ -195,82 +216,13 @@ namespace gtsam { return scatter; } -///* ************************************************************************* */ -//static MatrixColMajor formAbTAb(const FactorGraph& factors, const Scatter& scatter) { -// -// static const bool debug = false; -// -// tic("CombineAndEliminate: 3.1 varStarts"); -// // Determine scalar indices of each variable -// vector varStarts; -// varStarts.reserve(scatter.size() + 2); -// varStarts.push_back(0); -// BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) { -// varStarts.push_back(varStarts.back() + var_slot.second.dimension); -// } -// // This is for the r.h.s. vector -// varStarts.push_back(varStarts.back() + 1); -// toc("CombineAndEliminate: 3.1 varStarts"); -// -// // Allocate and zero matrix for Ab' * Ab -// MatrixColMajor ATA(ublas::zero_matrix(varStarts.back(), varStarts.back())); -// -// tic("CombineAndEliminate: 3.2 updates"); -// // Do blockwise low-rank updates to Ab' * Ab for each factor. Here, we -// // only update the upper triangle because this is all that Cholesky uses. -// BOOST_FOREACH(const HessianFactor::shared_ptr& factor, factors) { -// -// // Whiten the factor first so it has a unit diagonal noise model -// HessianFactor whitenedFactor(factor->whiten()); -// -// if(debug) whitenedFactor.print("whitened factor: "); -// -// for(HessianFactor::const_iterator var2 = whitenedFactor.begin(); var2 != whitenedFactor.end(); ++var2) { -// assert(scatter.find(*var2) != scatter.end()); -// size_t vj = scatter.find(*var2)->second.slot; -// for(HessianFactor::const_iterator var1 = whitenedFactor.begin(); var1 <= var2; ++var1) { -// assert(scatter.find(*var1) != scatter.end()); -// size_t vi = scatter.find(*var1)->second.slot; -// if(debug) cout << "Updating block " << vi << ", " << vj << endl; -// if(debug) cout << "Updating (" << varStarts[vi] << ":" << varStarts[vi+1] << ", " << -// varStarts[vj] << ":" << varStarts[vj+1] << ") from A" << *var1 << "' * A" << *var2 << endl; -// ublas::noalias(ublas::project(ATA, -// ublas::range(varStarts[vi], varStarts[vi+1]), ublas::range(varStarts[vj], varStarts[vj+1]))) += -// ublas::prod(ublas::trans(whitenedFactor.getA(var1)), whitenedFactor.getA(var2)); -// } -// } -// -// // Update r.h.s. vector -// size_t vj = scatter.size(); -// for(HessianFactor::const_iterator var1 = whitenedFactor.begin(); var1 < whitenedFactor.end(); ++var1) { -// assert(scatter.find(*var1) != scatter.end()); -// size_t vi = scatter.find(*var1)->second.slot; -// if(debug) cout << "Updating block " << vi << ", " << vj << endl; -// if(debug) cout << "Updating (" << varStarts[vi] << ":" << varStarts[vi+1] << ", " << -// varStarts[vj] << ":" << varStarts[vj+1] << ") from A" << *var1 << "' * b" << endl; -// ublas::matrix_column col(ATA, varStarts[vj]); -// ublas::noalias(ublas::subrange(col, varStarts[vi], varStarts[vi+1])) += -// ublas::prod(ublas::trans(whitenedFactor.getA(var1)), whitenedFactor.getb()); -// } -// -// size_t vi = scatter.size(); -// if(debug) cout << "Updating block " << vi << ", " << vj << endl; -// if(debug) cout << "Updating (" << varStarts[vi] << ":" << varStarts[vi+1] << ", " << -// varStarts[vj] << ":" << varStarts[vj+1] << ") from b" << "' * b" << endl; -// ublas::noalias(ATA(varStarts[vi], varStarts[vj])) += ublas::inner_prod(whitenedFactor.getb(), whitenedFactor.getb()); -// } -// toc("CombineAndEliminate: 3.2 updates"); -// -// return ATA; -//} - void HessianFactor::updateATA(const HessianFactor& 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. - static const bool debug = false; + const bool debug = ISDEBUG("updateATA"); // First build an array of slots tic(1, "slots"); @@ -300,7 +252,7 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte } else { if(debug) cout << "Updating (" << slot1 << "," << slot2 << ") from (" << j1 << "," << j2 << ")" << endl; - Block thisBlock(this->info_(slot2, slot1)); + Block thisBlock(this->info_(slot1, slot2)); constBlock updateBlock(update.info_(j1,j2)); ublas::noalias(ublas::symmetric_adaptor(thisBlock)) += ublas::symmetric_adaptor(updateBlock); @@ -331,8 +283,10 @@ GaussianBayesNet::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFront tic(1, "zero"); BlockAb::Block remainingMatrix(Ab.range(0, Ab.nBlocks())); if(remainingMatrix.size1() > 1) - for(size_t j = 0; j < remainingMatrix.size1() - 1; ++j) - memset(&remainingMatrix(j+1, j), 0, sizeof(remainingMatrix(0,0)) * (remainingMatrix.size1() - j - 1)); + for(size_t j = 0; j < remainingMatrix.size1() - 1; ++j) { + ublas::matrix_column col(ublas::column(remainingMatrix, j)); + std::fill(col.begin() + j+1, col.end(), 0.0); + } toc(1, "zero"); } @@ -361,7 +315,7 @@ GaussianBayesNet::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFront pair HessianFactor::CombineAndEliminate( const FactorGraph& factors, size_t nrFrontals) { - static const bool debug = false; + const bool debug = ISDEBUG("HessianFactor::CombineAndEliminate"); // Find the scatter and variable dimensions tic(1, "find scatter"); @@ -414,6 +368,8 @@ pair HessianFactor::Com } toc(5, "split"); + combinedFactor->assertInvariants(); + return make_pair(conditionals, combinedFactor); } diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 3ebb1dfbd..1de9a2f64 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -46,6 +46,7 @@ namespace gtsam { InfoMatrix matrix_; // The full information matrix [A b]^T * [A b] BlockInfo info_; // The block view of the full information matrix. + void assertInvariants() const; GaussianBayesNet::shared_ptr splitEliminatedFactor(size_t nrFrontals, const std::vector& keys); void updateATA(const HessianFactor& update, const Scatter& scatter); @@ -104,6 +105,9 @@ namespace gtsam { */ virtual size_t getDim(const_iterator variable) const { return info_(variable-this->begin(), 0).size1(); } + /** Return the number of columns and rows of the Hessian matrix */ + size_t size1() const { return info_.size1(); } + /** Return a view of a block of the information matrix */ constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); }