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

remove support for special EliminatePreferCholesky to deal with Indeterminant exception arising from multiplied Hessian terms of nonlinear equality constraints.

release/4.3a0
thduynguyen 2014-12-12 22:08:09 -05:00
parent da318184ae
commit 0576aac69b
2 changed files with 3 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -393,29 +393,6 @@ namespace gtsam {
return false;
}
/* ************************************************************************* */
boost::tuple<GaussianFactorGraph, GaussianFactorGraph, GaussianFactorGraph> GaussianFactorGraph::splitConstraints() const {
typedef HessianFactor H;
typedef JacobianFactor J;
GaussianFactorGraph hessians, jacobians, constraints;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, *this) {
H::shared_ptr hessian(boost::dynamic_pointer_cast<H>(factor));
if (hessian)
hessians.push_back(factor);
else {
J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
if (jacobian && jacobian->get_model() && jacobian->get_model()->isConstrained()) {
constraints.push_back(jacobian);
}
else {
jacobians.push_back(factor);
}
}
}
return boost::make_tuple(hessians, jacobians, constraints);
}
/* ************************************************************************* */
// x += alpha*A'*e
void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,

106
gtsam/linear/HessianFactor.cpp
View File

@ -682,110 +682,10 @@ EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys
// all factors to JacobianFactors. Otherwise, we can convert all factors
// to HessianFactors. This is because QR can handle constrained noise
// models but Cholesky cannot.
/* Currently, when eliminating a constrained variable, EliminatePreferCholesky
* converts every other factors to JacobianFactor before doing the special QR
* factorization for constrained variables. Unfortunately, after a constrained
* nonlinear graph is linearized, new hessian factors from constraints, multiplied
* with the dual variable (-lambda*\hessian{c} terms in the Lagrangian objective
* function), might become negative definite, thus cannot be converted to JacobianFactors.
*
* Following EliminateCholesky, this version of EliminatePreferCholesky for
* constrained var gathers all unconstrained factors into a big joint HessianFactor
* before converting it into a JacobianFactor to be eliminiated by QR together with
* the other constrained factors.
*
* Of course, this might not solve the non-positive-definite problem entirely,
* because (1) the original hessian factors might be non-positive definite
* and (2) large strange value of lambdas might cause the joint factor non-positive
* definite [is this true?]. But at least, this will help in typical cases.
*/
GaussianFactorGraph hessians, jacobians, constraints;
// factors.print("factors: ");
boost::tie(hessians, jacobians, constraints) = factors.splitConstraints();
// keys.print("Frontal variables to eliminate: ");
// hessians.print("Hessians: ");
// jacobians.print("Jacobians: ");
// constraints.print("Constraints: ");
bool hasHessians = hessians.size() > 0;
// Add all jacobians to gather as much info as we can
hessians.push_back(jacobians);
if (constraints.size()>0) {
// // Build joint factor
// HessianFactor::shared_ptr jointFactor;
// try {
// jointFactor = boost::make_shared<HessianFactor>(hessians, Scatter(factors, keys));
// } catch(std::invalid_argument&) {
// throw InvalidDenseElimination(
// "EliminateCholesky was called with a request to eliminate variables that are not\n"
// "involved in the provided factors.");
// }
// constraints.push_back(jointFactor);
// return EliminateQR(constraints, keys);
// If there are hessian factors, turn them into conditional
// by doing partial elimination, then use those jacobians when eliminating the constraints
GaussianFactor::shared_ptr unconstrainedNewFactor;
if (hessians.size() > 0) {
bool hasSeparator = false;
GaussianFactorGraph::Keys unconstrainedKeys = hessians.keys();
BOOST_FOREACH(Key key, unconstrainedKeys) {
if (find(keys.begin(), keys.end(), key) == keys.end()) {
hasSeparator = true;
break;
}
}
if (hasSeparator) {
// find frontal keys in the unconstrained factor to eliminate
Ordering subkeys;
BOOST_FOREACH(Key key, keys) {
if (unconstrainedKeys.exists(key))
subkeys.push_back(key);
}
GaussianConditional::shared_ptr unconstrainedConditional;
boost::tie(unconstrainedConditional, unconstrainedNewFactor)
= EliminateCholesky(hessians, subkeys);
constraints.push_back(unconstrainedConditional);
}
else {
if (hasHessians) {
HessianFactor::shared_ptr jointFactor = boost::make_shared<
HessianFactor>(hessians, Scatter(factors, keys));
constraints.push_back(jointFactor);
}
else {
constraints.push_back(hessians);
}
}
}
// Now eliminate the constraints
GaussianConditional::shared_ptr constrainedConditional;
GaussianFactor::shared_ptr constrainedNewFactor;
boost::tie(constrainedConditional, constrainedNewFactor) = EliminateQR(
constraints, keys);
// constraints.print("constraints: ");
// constrainedConditional->print("constrainedConditional: ");
// constrainedNewFactor->print("constrainedNewFactor: ");
if (unconstrainedNewFactor) {
GaussianFactorGraph newFactors;
newFactors.push_back(unconstrainedNewFactor);
newFactors.push_back(constrainedNewFactor);
// newFactors.print("newFactors: ");
HessianFactor::shared_ptr newFactor(new HessianFactor(newFactors));
return make_pair(constrainedConditional, newFactor);
} else {
return make_pair(constrainedConditional, constrainedNewFactor);
}
}
else {
if (hasConstraints(factors))
return EliminateQR(factors, keys);
else
return EliminateCholesky(factors, keys);
}
}
} // gtsam