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

79
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -169,11 +169,11 @@ namespace gtsam {
/* ************************************************************************* */
// 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) {
#ifndef NDEBUG
#ifndef NDEBUG
vector<size_t> varDims(variableSlots.size(), numeric_limits<size_t>::max());
#else
#else
vector<size_t> varDims(variableSlots.size());
#endif
#endif
size_t m = 0;
size_t n = 0;
{
@ -187,17 +187,17 @@ namespace gtsam {
if(sourceVarpos < numeric_limits<Index>::max()) {
const JacobianFactor& sourceFactor = *factors[sourceFactorI];
size_t vardim = sourceFactor.getDim(sourceFactor.begin() + sourceVarpos);
#ifndef NDEBUG
if(varDims[jointVarpos] == numeric_limits<size_t>::max()) {
#ifndef NDEBUG
if(varDims[jointVarpos] == numeric_limits<size_t>::max()) {
varDims[jointVarpos] = vardim;
n += vardim;
} else
} else
assert(varDims[jointVarpos] == vardim);
#else
#else
varDims[jointVarpos] = vardim;
n += vardim;
break;
#endif
n += vardim;
break;
#endif
}
++ sourceFactorI;
}
@ -452,7 +452,7 @@ namespace gtsam {
const bool debug = ISDEBUG("EliminateQR");
// 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");
FactorGraph<JacobianFactor> jacobians = convertToJacobians(factors);
@ -467,6 +467,18 @@ namespace gtsam {
return make_pair(conditional, factor);
} // \EliminateQR
/* ************************************************************************* */
bool hasConstraints(const FactorGraph<GaussianFactor>& factors) {
typedef JacobianFactor J;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
if (jacobian && jacobian->get_model()->isConstrained()) {
return true;
}
}
return false;
}
/* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminatePreferCholesky(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals) {
@ -478,30 +490,16 @@ namespace gtsam {
// 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);
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) {
if (!diag) {
tic(2, "EliminateCholesky");
ret = EliminateCholesky(factors, nrFrontals);
toc(2, "EliminateCholesky");
@ -537,8 +535,8 @@ namespace gtsam {
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)) {
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);
@ -555,6 +553,7 @@ namespace gtsam {
}
return ret;
}
} // \EliminatePreferCholesky
@ -616,18 +615,6 @@ namespace gtsam {
return make_pair(conditional, combinedFactor);
} // \EliminateLDL
/* ************************************************************************* */
bool hasConstraints(const FactorGraph<GaussianFactor>& factors) {
typedef JacobianFactor J;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors) {
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
if (jacobian && jacobian->get_model()->isConstrained()) {
return true;
}
}
return false;
}
/* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminatePreferLDL(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals) {
@ -643,8 +630,8 @@ namespace gtsam {
// Decide whether to use QR or LDL
// Check if any JacobianFactors have constrained noise models.
if (hasConstraints(factors))
EliminateQR(factors, nrFrontals);
return EliminateQR(factors, nrFrontals);
else {
GaussianFactorGraph::EliminationResult ret;
#ifdef NDEBUG
static const bool diag = false;
@ -708,7 +695,7 @@ namespace gtsam {
}
return ret;
}
} // \EliminatePreferLDL
} // namespace gtsam