remove support for special EliminatePreferCholesky to deal with Indeterminant exception arising from multiplied Hessian terms of nonlinear equality constraints.
thduynguyen
parent
da318184ae
commit
0576aac69b
|
|
@ -393,29 +393,6 @@ namespace gtsam {
|
||||||
return false;
|
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
|
// x += alpha*A'*e
|
||||||
void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
|
void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,
|
||||||
|
|
|
||||||
|
|
@ -682,110 +682,10 @@ EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys
|
||||||
// all factors to JacobianFactors. Otherwise, we can convert all factors
|
// all factors to JacobianFactors. Otherwise, we can convert all factors
|
||||||
// to HessianFactors. This is because QR can handle constrained noise
|
// to HessianFactors. This is because QR can handle constrained noise
|
||||||
// models but Cholesky cannot.
|
// models but Cholesky cannot.
|
||||||
|
if (hasConstraints(factors))
|
||||||
/* Currently, when eliminating a constrained variable, EliminatePreferCholesky
|
return EliminateQR(factors, keys);
|
||||||
* converts every other factors to JacobianFactor before doing the special QR
|
else
|
||||||
* 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 {
|
|
||||||
return EliminateCholesky(factors, keys);
|
return EliminateCholesky(factors, keys);
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // gtsam
|
} // gtsam
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue