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

57
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -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,23 +490,9 @@ 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;
@ -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