12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

[with Alex and Richard] Fixed major bug when constraints are present, but it was never encountered because of the global useQR flag. Re-arranged some other things.

release/4.3a0
Frank Dellaert 2012-01-20 20:47:30 +00:00
parent 62afde62f3
commit 9bad4f67eb
1 changed files with 328 additions and 341 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

669
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -43,7 +43,7 @@ namespace gtsam {
GaussianFactorGraph::Keys GaussianFactorGraph::keys() const { GaussianFactorGraph::Keys GaussianFactorGraph::keys() const {
FastSet<Index> keys; FastSet<Index> keys;
BOOST_FOREACH(const sharedFactor& factor, *this) BOOST_FOREACH(const sharedFactor& factor, *this)
if (factor) keys.insert(factor->begin(), factor->end()); if (factor) keys.insert(factor->begin(), factor->end());
return keys; return keys;
} }
@ -51,15 +51,15 @@ namespace gtsam {
void GaussianFactorGraph::permuteWithInverse( void GaussianFactorGraph::permuteWithInverse(
const Permutation& inversePermutation) { const Permutation& inversePermutation) {
BOOST_FOREACH(const sharedFactor& factor, factors_) BOOST_FOREACH(const sharedFactor& factor, factors_)
{ {
factor->permuteWithInverse(inversePermutation); factor->permuteWithInverse(inversePermutation);
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
void GaussianFactorGraph::combine(const GaussianFactorGraph &lfg) { void GaussianFactorGraph::combine(const GaussianFactorGraph &lfg) {
for (const_iterator factor = lfg.factors_.begin(); factor for (const_iterator factor = lfg.factors_.begin(); factor
!= lfg.factors_.end(); factor++) { != lfg.factors_.end(); factor++) {
push_back(*factor); push_back(*factor);
} }
} }
@ -73,7 +73,7 @@ namespace gtsam {
// add the second factors_ in the graph // add the second factors_ in the graph
for (const_iterator factor = lfg2.factors_.begin(); factor for (const_iterator factor = lfg2.factors_.begin(); factor
!= lfg2.factors_.end(); factor++) { != lfg2.factors_.end(); factor++) {
fg.push_back(*factor); fg.push_back(*factor);
} }
return fg; return fg;
@ -81,22 +81,22 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
std::vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian() const { std::vector<boost::tuple<size_t, size_t, double> > GaussianFactorGraph::sparseJacobian() const {
// First find dimensions of each variable // First find dimensions of each variable
FastVector<size_t> dims; FastVector<size_t> dims;
BOOST_FOREACH(const sharedFactor& factor, *this) { BOOST_FOREACH(const sharedFactor& factor, *this) {
for(GaussianFactor::const_iterator pos = factor->begin(); pos != factor->end(); ++pos) { for(GaussianFactor::const_iterator pos = factor->begin(); pos != factor->end(); ++pos) {
if(dims.size() <= *pos) if(dims.size() <= *pos)
dims.resize(*pos + 1, 0); dims.resize(*pos + 1, 0);
dims[*pos] = factor->getDim(pos); dims[*pos] = factor->getDim(pos);
} }
} }
// Compute first scalar column of each variable // Compute first scalar column of each variable
vector<size_t> columnIndices(dims.size()+1, 0); vector<size_t> columnIndices(dims.size()+1, 0);
for(size_t j=1; j<dims.size()+1; ++j) for(size_t j=1; j<dims.size()+1; ++j)
columnIndices[j] = columnIndices[j-1] + dims[j-1]; columnIndices[j] = columnIndices[j-1] + dims[j-1];
// Iterate over all factors, adding sparse scalar entries // Iterate over all factors, adding sparse scalar entries
typedef boost::tuple<size_t, size_t, double> triplet; typedef boost::tuple<size_t, size_t, double> triplet;
FastVector<triplet> entries; FastVector<triplet> entries;
size_t row = 0; size_t row = 0;
@ -105,7 +105,7 @@ namespace gtsam {
JacobianFactor::shared_ptr jacobianFactor( JacobianFactor::shared_ptr jacobianFactor(
boost::dynamic_pointer_cast<JacobianFactor>(factor)); boost::dynamic_pointer_cast<JacobianFactor>(factor));
if (!jacobianFactor) { if (!jacobianFactor) {
HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast<HessianFactor>(factor)); HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast<HessianFactor>(factor));
if (hessian) if (hessian)
jacobianFactor.reset(new JacobianFactor(*hessian)); jacobianFactor.reset(new JacobianFactor(*hessian));
else else
@ -117,21 +117,21 @@ namespace gtsam {
// iterate over all variables in the factor // iterate over all variables in the factor
const JacobianFactor whitened(jacobianFactor->whiten()); const JacobianFactor whitened(jacobianFactor->whiten());
for(JacobianFactor::const_iterator pos=whitened.begin(); pos<whitened.end(); ++pos) { for(JacobianFactor::const_iterator pos=whitened.begin(); pos<whitened.end(); ++pos) {
JacobianFactor::constABlock whitenedA = whitened.getA(pos); JacobianFactor::constABlock whitenedA = whitened.getA(pos);
// find first column index for this key // find first column index for this key
size_t column_start = columnIndices[*pos]; size_t column_start = columnIndices[*pos];
for (size_t i = 0; i < (size_t) whitenedA.rows(); i++) for (size_t i = 0; i < (size_t) whitenedA.rows(); i++)
for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) { for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) {
double s = whitenedA(i,j); double s = whitenedA(i,j);
if (std::abs(s) > 1e-12) entries.push_back( if (std::abs(s) > 1e-12) entries.push_back(
boost::make_tuple(row+i, column_start+j, s)); boost::make_tuple(row+i, column_start+j, s));
} }
} }
JacobianFactor::constBVector whitenedb(whitened.getb()); JacobianFactor::constBVector whitenedb(whitened.getb());
size_t bcolumn = columnIndices.back(); size_t bcolumn = columnIndices.back();
for (size_t i = 0; i < (size_t) whitenedb.size(); i++) for (size_t i = 0; i < (size_t) whitenedb.size(); i++)
entries.push_back(boost::make_tuple(row+i, bcolumn, whitenedb(i))); entries.push_back(boost::make_tuple(row+i, bcolumn, whitenedb(i)));
// Increment row index // Increment row index
row += jacobianFactor->rows(); row += jacobianFactor->rows();
@ -161,54 +161,54 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Matrix GaussianFactorGraph::denseJacobian() const { Matrix GaussianFactorGraph::denseJacobian() const {
// combine all factors // combine all factors
JacobianFactor combined(*CombineJacobians(*convertCastFactors<FactorGraph< JacobianFactor combined(*CombineJacobians(*convertCastFactors<FactorGraph<
JacobianFactor> > (), VariableSlots(*this))); JacobianFactor> > (), VariableSlots(*this)));
return combined.matrix_augmented(); return combined.matrix_augmented();
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Helper functions for Combine // Helper functions for Combine
static boost::tuple<vector<size_t>, size_t, size_t> countDims(const std::vector<JacobianFactor::shared_ptr>& factors, const VariableSlots& variableSlots) { static boost::tuple<vector<size_t>, size_t, size_t> countDims(const std::vector<JacobianFactor::shared_ptr>& factors, const VariableSlots& variableSlots) {
#ifndef NDEBUG #ifndef NDEBUG
vector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max()); vector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
#else #else
vector<size_t> varDims(variableSlots.size()); vector<size_t> varDims(variableSlots.size());
#endif #endif
size_t m = 0; size_t m = 0;
size_t n = 0; size_t n = 0;
{ {
Index jointVarpos = 0; Index jointVarpos = 0;
BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) { BOOST_FOREACH(const VariableSlots::value_type& slots, variableSlots) {
assert(slots.second.size() == factors.size()); assert(slots.second.size() == factors.size());
Index sourceFactorI = 0; Index sourceFactorI = 0;
BOOST_FOREACH(const Index sourceVarpos, slots.second) { BOOST_FOREACH(const Index sourceVarpos, slots.second) {
if(sourceVarpos < numeric_limits<Index>::max()) { if(sourceVarpos < numeric_limits<Index>::max()) {
const JacobianFactor& sourceFactor = *factors[sourceFactorI]; const JacobianFactor& sourceFactor = *factors[sourceFactorI];
size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos); size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
#ifndef NDEBUG #ifndef NDEBUG
if(varDims[jointVarpos] == numeric_limits<size_t>::max()) { if(varDims[jointVarpos] == numeric_limits<size_t>::max()) {
varDims[jointVarpos] = vardim; varDims[jointVarpos] = vardim;
n += vardim; n += vardim;
} else } else
assert(varDims[jointVarpos] == vardim); assert(varDims[jointVarpos] == vardim);
#else #else
varDims[jointVarpos] = vardim; varDims[jointVarpos] = vardim;
n += vardim; n += vardim;
break; break;
#endif #endif
} }
++ sourceFactorI; ++ sourceFactorI;
} }
++ jointVarpos; ++ jointVarpos;
} }
BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) { BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, factors) {
m += factor->rows(); m += factor->rows();
} }
} }
return boost::make_tuple(varDims, m, n); return boost::make_tuple(varDims, m, n);
} }
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::shared_ptr CombineJacobians( JacobianFactor::shared_ptr CombineJacobians(
@ -261,7 +261,7 @@ namespace gtsam {
const JacobianFactor& source(*factors[info.factorI]); const JacobianFactor& source(*factors[info.factorI]);
size_t sourceRow = info.factorRowI; size_t sourceRow = info.factorRowI;
Index sourceSlot = varslot.second[info.factorI]; Index sourceSlot = varslot.second[info.factorI];
combined->copyRow(source, sourceRow, sourceSlot, row, combinedSlot); combined->copyRow(source, sourceRow, sourceSlot, row, combinedSlot);
} }
++combinedSlot; ++combinedSlot;
} }
@ -303,56 +303,56 @@ namespace gtsam {
return make_pair(gbn, jointFactor); return make_pair(gbn, jointFactor);
} }
/* ************************************************************************* */ /* ************************************************************************* */
static static
FastMap<Index, SlotEntry> findScatterAndDims FastMap<Index, SlotEntry> findScatterAndDims
(const FactorGraph<GaussianFactor>& factors) { (const FactorGraph<GaussianFactor>& factors) {
const bool debug = ISDEBUG("findScatterAndDims"); const bool debug = ISDEBUG("findScatterAndDims");
// The "scatter" is a map from global variable indices to slot indices in the // The "scatter" is a map from global variable indices to slot indices in the
// union of involved variables. We also include the dimensionality of the // union of involved variables. We also include the dimensionality of the
// variable. // variable.
Scatter scatter; Scatter scatter;
// First do the set union. // First do the set union.
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) { BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
for(GaussianFactor::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) { for(GaussianFactor::const_iterator variable = factor->begin(); variable != factor->end(); ++variable) {
scatter.insert(make_pair(*variable, SlotEntry(0, factor->getDim(variable)))); scatter.insert(make_pair(*variable, SlotEntry(0, factor->getDim(variable))));
} }
} }
// Next fill in the slot indices (we can only get these after doing the set // Next fill in the slot indices (we can only get these after doing the set
// union. // union.
size_t slot = 0; size_t slot = 0;
BOOST_FOREACH(Scatter::value_type& var_slot, scatter) { BOOST_FOREACH(Scatter::value_type& var_slot, scatter) {
var_slot.second.slot = (slot ++); var_slot.second.slot = (slot ++);
if(debug) if(debug)
cout << "scatter[" << var_slot.first << "] = (slot " << var_slot.second.slot << ", dim " << var_slot.second.dimension << ")" << endl; cout << "scatter[" << var_slot.first << "] = (slot " << var_slot.second.slot << ", dim " << var_slot.second.dimension << ")" << endl;
} }
return scatter; return scatter;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix GaussianFactorGraph::denseHessian() const { Matrix GaussianFactorGraph::denseHessian() const {
Scatter scatter = findScatterAndDims(*this); Scatter scatter = findScatterAndDims(*this);
vector<size_t> dims; dims.reserve(scatter.size() + 1); vector<size_t> dims; dims.reserve(scatter.size() + 1);
BOOST_FOREACH(const Scatter::value_type& index_entry, scatter) { BOOST_FOREACH(const Scatter::value_type& index_entry, scatter) {
dims.push_back(index_entry.second.dimension); dims.push_back(index_entry.second.dimension);
} }
dims.push_back(1); dims.push_back(1);
// combine all factors and get upper-triangular part of Hessian // combine all factors and get upper-triangular part of Hessian
HessianFactor combined(*this, dims, scatter); HessianFactor combined(*this, dims, scatter);
Matrix result = combined.info(); Matrix result = combined.info();
// Fill in lower-triangular part of Hessian // Fill in lower-triangular part of Hessian
result.triangularView<Eigen::StrictlyLower>() = result.transpose(); result.triangularView<Eigen::StrictlyLower>() = result.transpose();
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph< GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph<
@ -399,7 +399,7 @@ namespace gtsam {
// Extract conditional and fill in details of the remaining factor // Extract conditional and fill in details of the remaining factor
tic(5, "split"); tic(5, "split");
GaussianConditional::shared_ptr conditional = GaussianConditional::shared_ptr conditional =
combinedFactor->splitEliminatedFactor(nrFrontals); combinedFactor->splitEliminatedFactor(nrFrontals);
if (debug) { if (debug) {
conditional->print("Extracted conditional: "); conditional->print("Extracted conditional: ");
combinedFactor->print("Eliminated factor (L piece): "); combinedFactor->print("Eliminated factor (L piece): ");
@ -422,26 +422,26 @@ namespace gtsam {
FactorGraph<J> jacobians; FactorGraph<J> jacobians;
jacobians.reserve(factors.size()); jacobians.reserve(factors.size());
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors)
if (factor) { if (factor) {
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor)); J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
if (jacobian) { if (jacobian) {
jacobians.push_back(jacobian); jacobians.push_back(jacobian);
if (debug) jacobian->print("Existing JacobianFactor: "); if (debug) jacobian->print("Existing JacobianFactor: ");
} else { } else {
H::shared_ptr hessian(boost::dynamic_pointer_cast<H>(factor)); H::shared_ptr hessian(boost::dynamic_pointer_cast<H>(factor));
if (!hessian) throw std::invalid_argument( if (!hessian) throw std::invalid_argument(
"convertToJacobians: factor is neither a JacobianFactor nor a HessianFactor."); "convertToJacobians: factor is neither a JacobianFactor nor a HessianFactor.");
J::shared_ptr converted(new J(*hessian)); J::shared_ptr converted(new J(*hessian));
if (debug) { if (debug) {
cout << "Converted HessianFactor to JacobianFactor:\n"; cout << "Converted HessianFactor to JacobianFactor:\n";
hessian->print("HessianFactor: "); hessian->print("HessianFactor: ");
converted->print("JacobianFactor: "); converted->print("JacobianFactor: ");
if (!assert_equal(*hessian, HessianFactor(*converted), 1e-3)) throw runtime_error( if (!assert_equal(*hessian, HessianFactor(*converted), 1e-3)) throw runtime_error(
"convertToJacobians: Conversion between Jacobian and Hessian incorrect"); "convertToJacobians: Conversion between Jacobian and Hessian incorrect");
}
jacobians.push_back(converted);
} }
jacobians.push_back(converted);
} }
}
return jacobians; return jacobians;
} }
@ -452,7 +452,7 @@ namespace gtsam {
const bool debug = ISDEBUG("EliminateQR"); const bool debug = ISDEBUG("EliminateQR");
// Convert all factors to the appropriate type and call the type-specific EliminateGaussian. // Convert all factors to the appropriate type and call the type-specific EliminateGaussian.
if (debug) cout << "Using QR:"; if (debug) cout << "Using QR" << endl;
tic(1, "convert to Jacobian"); tic(1, "convert to Jacobian");
FactorGraph<JacobianFactor> jacobians = convertToJacobians(factors); FactorGraph<JacobianFactor> jacobians = convertToJacobians(factors);
@ -467,155 +467,6 @@ namespace gtsam {
return make_pair(conditional, factor); return make_pair(conditional, factor);
} // \EliminateQR } // \EliminateQR
/* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminatePreferCholesky(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals) {
typedef JacobianFactor J;
typedef HessianFactor H;
// If any JacobianFactors have constrained noise models, we have to convert
// all factors to JacobianFactors. Otherwise, we can convert all factors
// to HessianFactors. This is because QR can handle constrained noise
// models but Cholesky cannot.
// Decide whether to use QR or Cholesky
// Check if any JacobianFactors have constrained noise models.
bool useQR = false;
useQR = false;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
if (jacobian && jacobian->get_model()->isConstrained()) {
useQR = true;
break;
}
}
// Convert all factors to the appropriate type
// and call the type-specific EliminateGaussian.
if (useQR) return EliminateQR(factors, nrFrontals);
GaussianFactorGraph::EliminationResult ret;
#ifdef NDEBUG
static const bool diag = false;
#else
static const bool diag = !ISDEBUG("NoCholeskyDiagnostics");
#endif
if(!diag) {
tic(2, "EliminateCholesky");
ret = EliminateCholesky(factors, nrFrontals);
toc(2, "EliminateCholesky");
} else {
try {
tic(2, "EliminateCholesky");
ret = EliminateCholesky(factors, nrFrontals);
toc(2, "EliminateCholesky");
} catch (const exception& e) {
cout << "Exception in EliminateCholesky: " << e.what() << endl;
SETDEBUG("EliminateCholesky", true);
SETDEBUG("updateATA", true);
SETDEBUG("JacobianFactor::eliminate", true);
SETDEBUG("JacobianFactor::Combine", true);
SETDEBUG("choleskyPartial", true);
factors.print("Combining factors: ");
EliminateCholesky(factors, nrFrontals);
throw;
}
}
const bool checkCholesky = ISDEBUG("EliminateGaussian Check Cholesky");
if (checkCholesky) {
GaussianFactorGraph::EliminationResult expected;
FactorGraph<J> jacobians = convertToJacobians(factors);
try {
// Compare with QR
expected = EliminateJacobians(jacobians, nrFrontals);
} catch (...) {
cout << "Exception in QR" << endl;
throw;
}
H actual_factor(*ret.second);
H expected_factor(*expected.second);
if (!assert_equal(*expected.first, *ret.first, 100.0) || !assert_equal(
expected_factor, actual_factor, 1.0)) {
cout << "Cholesky and QR do not agree" << endl;
SETDEBUG("EliminateCholesky", true);
SETDEBUG("updateATA", true);
SETDEBUG("JacobianFactor::eliminate", true);
SETDEBUG("JacobianFactor::Combine", true);
jacobians.print("Jacobian Factors: ");
EliminateJacobians(jacobians, nrFrontals);
EliminateCholesky(factors, nrFrontals);
factors.print("Combining factors: ");
throw runtime_error("Cholesky and QR do not agree");
}
}
return ret;
} // \EliminatePreferCholesky
/* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminateLDL(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals) {
const bool debug = ISDEBUG("EliminateLDL");
// Find the scatter and variable dimensions
tic(1, "find scatter");
Scatter scatter(findScatterAndDims(factors));
toc(1, "find scatter");
// Pull out keys and dimensions
tic(2, "keys");
vector<size_t> dimensions(scatter.size() + 1);
BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) {
dimensions[var_slot.second.slot] = var_slot.second.dimension;
}
// This is for the r.h.s. vector
dimensions.back() = 1;
toc(2, "keys");
// Form Ab' * Ab
tic(3, "combine");
if (debug) {
// print out everything before combine
factors.print("Factors to be combined into hessian");
cout << "Dimensions (" << dimensions.size() << "): ";
BOOST_FOREACH(size_t d, dimensions) cout << d << " ";
cout << "\nScatter:" << endl;
BOOST_FOREACH(const Scatter::value_type& p, scatter)
cout << " Index: " << p.first << ", " << p.second.toString() << endl;
}
HessianFactor::shared_ptr //
combinedFactor(new HessianFactor(factors, dimensions, scatter));
toc(3, "combine");
// Do LDL, note that after this, the lower triangle still contains
// some untouched non-zeros that should be zero. We zero them while
// extracting submatrices next.
tic(4, "partial LDL");
Eigen::LDLT<Matrix>::TranspositionType permutation = combinedFactor->partialLDL(nrFrontals);
toc(4, "partial LDL");
// Extract conditional and fill in details of the remaining factor
tic(5, "split");
GaussianConditional::shared_ptr conditional =
combinedFactor->splitEliminatedFactor(nrFrontals, permutation);
if (debug) {
conditional->print("Extracted conditional: ");
combinedFactor->print("Eliminated factor (L piece): ");
}
toc(5, "split");
combinedFactor->assertInvariants();
return make_pair(conditional, combinedFactor);
} // \EliminateLDL
/* ************************************************************************* */ /* ************************************************************************* */
bool hasConstraints(const FactorGraph<GaussianFactor>& factors) { bool hasConstraints(const FactorGraph<GaussianFactor>& factors) {
typedef JacobianFactor J; typedef JacobianFactor J;
@ -628,6 +479,142 @@ namespace gtsam {
return false; return false;
} }
/* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminatePreferCholesky(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals) {
typedef JacobianFactor J;
typedef HessianFactor H;
// If any JacobianFactors have constrained noise models, we have to convert
// all factors to JacobianFactors. Otherwise, we can convert all factors
// to HessianFactors. This is because QR can handle constrained noise
// models but Cholesky cannot.
if (hasConstraints(factors))
return EliminateQR(factors, nrFrontals);
else {
GaussianFactorGraph::EliminationResult ret;
#ifdef NDEBUG
static const bool diag = false;
#else
static const bool diag = !ISDEBUG("NoCholeskyDiagnostics");
#endif
if (!diag) {
tic(2, "EliminateCholesky");
ret = EliminateCholesky(factors, nrFrontals);
toc(2, "EliminateCholesky");
} else {
try {
tic(2, "EliminateCholesky");
ret = EliminateCholesky(factors, nrFrontals);
toc(2, "EliminateCholesky");
} catch (const exception& e) {
cout << "Exception in EliminateCholesky: " << e.what() << endl;
SETDEBUG("EliminateCholesky", true);
SETDEBUG("updateATA", true);
SETDEBUG("JacobianFactor::eliminate", true);
SETDEBUG("JacobianFactor::Combine", true);
SETDEBUG("choleskyPartial", true);
factors.print("Combining factors: ");
EliminateCholesky(factors, nrFrontals);
throw;
}
}
const bool checkCholesky = ISDEBUG("EliminateGaussian Check Cholesky");
if (checkCholesky) {
GaussianFactorGraph::EliminationResult expected;
FactorGraph<J> jacobians = convertToJacobians(factors);
try {
// Compare with QR
expected = EliminateJacobians(jacobians, nrFrontals);
} catch (...) {
cout << "Exception in QR" << endl;
throw;
}
H actual_factor(*ret.second);
H expected_factor(*expected.second);
if (!assert_equal(*expected.first, *ret.first, 100.0)
|| !assert_equal(expected_factor, actual_factor, 1.0)) {
cout << "Cholesky and QR do not agree" << endl;
SETDEBUG("EliminateCholesky", true);
SETDEBUG("updateATA", true);
SETDEBUG("JacobianFactor::eliminate", true);
SETDEBUG("JacobianFactor::Combine", true);
jacobians.print("Jacobian Factors: ");
EliminateJacobians(jacobians, nrFrontals);
EliminateCholesky(factors, nrFrontals);
factors.print("Combining factors: ");
throw runtime_error("Cholesky and QR do not agree");
}
}
return ret;
}
} // \EliminatePreferCholesky
/* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminateLDL(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals) {
const bool debug = ISDEBUG("EliminateLDL");
// Find the scatter and variable dimensions
tic(1, "find scatter");
Scatter scatter(findScatterAndDims(factors));
toc(1, "find scatter");
// Pull out keys and dimensions
tic(2, "keys");
vector<size_t> dimensions(scatter.size() + 1);
BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) {
dimensions[var_slot.second.slot] = var_slot.second.dimension;
}
// This is for the r.h.s. vector
dimensions.back() = 1;
toc(2, "keys");
// Form Ab' * Ab
tic(3, "combine");
if (debug) {
// print out everything before combine
factors.print("Factors to be combined into hessian");
cout << "Dimensions (" << dimensions.size() << "): ";
BOOST_FOREACH(size_t d, dimensions) cout << d << " ";
cout << "\nScatter:" << endl;
BOOST_FOREACH(const Scatter::value_type& p, scatter)
cout << " Index: " << p.first << ", " << p.second.toString() << endl;
}
HessianFactor::shared_ptr //
combinedFactor(new HessianFactor(factors, dimensions, scatter));
toc(3, "combine");
// Do LDL, note that after this, the lower triangle still contains
// some untouched non-zeros that should be zero. We zero them while
// extracting submatrices next.
tic(4, "partial LDL");
Eigen::LDLT<Matrix>::TranspositionType permutation = combinedFactor->partialLDL(nrFrontals);
toc(4, "partial LDL");
// Extract conditional and fill in details of the remaining factor
tic(5, "split");
GaussianConditional::shared_ptr conditional =
combinedFactor->splitEliminatedFactor(nrFrontals, permutation);
if (debug) {
conditional->print("Extracted conditional: ");
combinedFactor->print("Eliminated factor (L piece): ");
}
toc(5, "split");
combinedFactor->assertInvariants();
return make_pair(conditional, combinedFactor);
} // \EliminateLDL
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminatePreferLDL( GaussianFactorGraph::EliminationResult EliminatePreferLDL(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals) { const FactorGraph<GaussianFactor>& factors, size_t nrFrontals) {
@ -643,72 +630,72 @@ namespace gtsam {
// Decide whether to use QR or LDL // Decide whether to use QR or LDL
// Check if any JacobianFactors have constrained noise models. // Check if any JacobianFactors have constrained noise models.
if (hasConstraints(factors)) if (hasConstraints(factors))
EliminateQR(factors, nrFrontals); return EliminateQR(factors, nrFrontals);
else {
GaussianFactorGraph::EliminationResult ret; GaussianFactorGraph::EliminationResult ret;
#ifdef NDEBUG #ifdef NDEBUG
static const bool diag = false; static const bool diag = false;
#else #else
static const bool diag = !ISDEBUG("NoLDLDiagnostics"); static const bool diag = !ISDEBUG("NoLDLDiagnostics");
#endif #endif
if(!diag) { if(!diag) {
tic(2, "EliminateLDL"); tic(2, "EliminateLDL");
ret = EliminateLDL(factors, nrFrontals); ret = EliminateLDL(factors, nrFrontals);
toc(2, "EliminateLDL"); toc(2, "EliminateLDL");
} else { } else {
try { try {
tic(2, "EliminateLDL"); tic(2, "EliminateLDL");
ret = EliminateLDL(factors, nrFrontals); ret = EliminateLDL(factors, nrFrontals);
toc(2, "EliminateLDL"); toc(2, "EliminateLDL");
} catch (const NegativeMatrixException& e) { } catch (const NegativeMatrixException& e) {
throw; throw;
} catch (const exception& e) { } catch (const exception& e) {
cout << "Exception in EliminateLDL: " << e.what() << endl; cout << "Exception in EliminateLDL: " << e.what() << endl;
SETDEBUG("EliminateLDL", true); SETDEBUG("EliminateLDL", true);
SETDEBUG("updateATA", true); SETDEBUG("updateATA", true);
SETDEBUG("JacobianFactor::eliminate", true); SETDEBUG("JacobianFactor::eliminate", true);
SETDEBUG("JacobianFactor::Combine", true); SETDEBUG("JacobianFactor::Combine", true);
SETDEBUG("ldlPartial", true); SETDEBUG("ldlPartial", true);
SETDEBUG("findScatterAndDims", true); SETDEBUG("findScatterAndDims", true);
factors.print("Combining factors: "); factors.print("Combining factors: ");
EliminateLDL(factors, nrFrontals); EliminateLDL(factors, nrFrontals);
throw; throw;
} }
}
const bool checkLDL = ISDEBUG("EliminateGaussian Check LDL");
if (checkLDL) {
GaussianFactorGraph::EliminationResult expected;
FactorGraph<J> jacobians = convertToJacobians(factors);
try {
// Compare with QR
expected = EliminateJacobians(jacobians, nrFrontals);
} catch (...) {
cout << "Exception in QR" << endl;
throw;
} }
H actual_factor(*ret.second); const bool checkLDL = ISDEBUG("EliminateGaussian Check LDL");
H expected_factor(*expected.second); if (checkLDL) {
if (!assert_equal(*expected.first, *ret.first, 100.0) || !assert_equal( GaussianFactorGraph::EliminationResult expected;
expected_factor, actual_factor, 1.0)) { FactorGraph<J> jacobians = convertToJacobians(factors);
cout << "LDL and QR do not agree" << endl; try {
// Compare with QR
expected = EliminateJacobians(jacobians, nrFrontals);
} catch (...) {
cout << "Exception in QR" << endl;
throw;
}
SETDEBUG("EliminateLDL", true); H actual_factor(*ret.second);
SETDEBUG("updateATA", true); H expected_factor(*expected.second);
SETDEBUG("JacobianFactor::eliminate", true); if (!assert_equal(*expected.first, *ret.first, 100.0) || !assert_equal(
SETDEBUG("JacobianFactor::Combine", true); expected_factor, actual_factor, 1.0)) {
jacobians.print("Jacobian Factors: "); cout << "LDL and QR do not agree" << endl;
EliminateJacobians(jacobians, nrFrontals);
EliminateLDL(factors, nrFrontals);
factors.print("Combining factors: ");
throw runtime_error("LDL and QR do not agree"); SETDEBUG("EliminateLDL", true);
SETDEBUG("updateATA", true);
SETDEBUG("JacobianFactor::eliminate", true);
SETDEBUG("JacobianFactor::Combine", true);
jacobians.print("Jacobian Factors: ");
EliminateJacobians(jacobians, nrFrontals);
EliminateLDL(factors, nrFrontals);
factors.print("Combining factors: ");
throw runtime_error("LDL and QR do not agree");
}
} }
return ret;
} }
return ret;
} // \EliminatePreferLDL } // \EliminatePreferLDL
} // namespace gtsam } // namespace gtsam