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

move detailed comments to the cpp file. An important comment about an Eigen's exception when converting a jacobian to a hessian factor, probably due to a bug in clang compiler.

release/4.3a0
thduynguyen 2014-05-01 14:41:55 -04:00
parent fc63f540db
commit cb7153a9d2
2 changed files with 34 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
gtsam/linear/QPSolver.cpp
View File

@ -16,7 +16,6 @@ namespace gtsam {
/* ************************************************************************* */
QPSolver::QPSolver(const GaussianFactorGraph& graph) :
graph_(graph), fullFactorIndices_(graph) {
// Split the original graph into unconstrained and constrained part
// and collect indices of constrained factors
for (size_t i = 0; i < graph.nrFactors(); ++i) {
@ -29,9 +28,9 @@ QPSolver::QPSolver(const GaussianFactorGraph& graph) :
}
// Collect constrained variable keys
KeySet constrainedVars;
std::set<size_t> constrainedVars;
BOOST_FOREACH(size_t index, constraintIndices_) {
KeyVector keys = graph[index]->keys();
KeyVector keys = graph.at(index)->keys();
constrainedVars.insert(keys.begin(), keys.end());
}
@ -43,10 +42,9 @@ QPSolver::QPSolver(const GaussianFactorGraph& graph) :
/* ************************************************************************* */
GaussianFactorGraph::shared_ptr QPSolver::unconstrainedHessiansOfConstrainedVars(
const GaussianFactorGraph& graph, const KeySet& constrainedVars) const {
const GaussianFactorGraph& graph, const std::set<Key>& constrainedVars) const {
VariableIndex variableIndex(graph);
GaussianFactorGraph::shared_ptr hfg = boost::make_shared<GaussianFactorGraph>();
GaussianFactorGraph::shared_ptr hfg(new GaussianFactorGraph());
// Collect all factors involving constrained vars
FastSet<size_t> factors;
BOOST_FOREACH(Key key, constrainedVars) {
@ -83,13 +81,19 @@ GaussianFactorGraph::shared_ptr QPSolver::unconstrainedHessiansOfConstrainedVars
}
else { // unconstrained Jacobian
// Convert the original linear factor to Hessian factor
hfg->push_back(HessianFactor(*jf));
// TODO: This may fail and throw the following exception
// Assertion failed: (((!PanelMode) && stride==0 && offset==0) ||
// (PanelMode && stride>=depth && offset<=stride)), function operator(),
// file Eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h, line 1133.
// because of a weird error which might be related to clang
// See this: https://groups.google.com/forum/#!topic/ceres-solver/DYhqOLPquHU
// My current way to fix this is to compile both gtsam and my library in Release mode
hfg->add(HessianFactor(*jf));
}
}
else { // If it's not a Jacobian, it should be a hessian factor. Just add!
hfg->push_back(graph[factorIndex]);
}
}
return hfg;
}
@ -97,7 +101,7 @@ GaussianFactorGraph::shared_ptr QPSolver::unconstrainedHessiansOfConstrainedVars
/* ************************************************************************* */
GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph,
const VectorValues& x0, bool useLeastSquare) const {
static const bool debug = true;
static const bool debug = false;
// The dual graph to return
GaussianFactorGraph dualGraph;
@ -248,9 +252,24 @@ bool QPSolver::updateWorkingSetInplace(GaussianFactorGraph& workingGraph,
}
/* ************************************************************************* */
/* We have to make sure the new solution with alpha satisfies all INACTIVE ineq constraints
* If some inactive ineq constraints complain about the full step (alpha = 1),
* we have to adjust alpha to stay within the ineq constraints' feasible regions.
*
* For each inactive ineq j:
* - We already have: aj'*xk - bj <= 0, since xk satisfies all ineq constraints
* - We want: aj'*(xk + alpha*p) - bj <= 0
* - If aj'*p <= 0, we have: aj'*(xk + alpha*p) <= aj'*xk <= bj, for all alpha>0
* it's good!
* - We only care when aj'*p > 0. In this case, we need to choose alpha so that
* aj'*xk + alpha*aj'*p - bj <= 0 --> alpha <= (bj - aj'*xk) / (aj'*p)
* We want to step as far as possible, so we should choose alpha = (bj - aj'*xk) / (aj'*p)
*
* We want the minimum of all those alphas among all inactive ineq.
*/
boost::tuple<double, int, int> QPSolver::computeStepSize(const GaussianFactorGraph& workingGraph,
const VectorValues& xk, const VectorValues& p) const {
static bool debug = false;
static bool debug = true;
double minAlpha = 1.0;
int closestFactorIx = -1, closestSigmaIx = -1;
@ -268,9 +287,11 @@ boost::tuple<double, int, int> QPSolver::computeStepSize(const GaussianFactorGra
Vector aj = jacobian->getA(xj).row(s);
ajTp += aj.dot(pj);
}
if (debug) cout << "s, ajTp: " << s << " " << ajTp << endl;
if (debug) cout << "s, ajTp, b[s]: " << s << " " << ajTp << " " << b[s] << endl;
// Check if aj'*p >0. Don't care if it's not.
// if (ajTp - b[s]>0)
// throw std::runtime_error("Infeasible point detected. Please choose a feasible initial values!");
if (ajTp<=0) continue;
// Compute aj'*xk
@ -344,6 +365,7 @@ bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& c
/* ************************************************************************* */
VectorValues QPSolver::optimize(const VectorValues& initials,
boost::optional<VectorValues&> lambdas) const {
cout << "QP optimize.." << endl;
GaussianFactorGraph workingGraph = graph_.clone();
VectorValues currentSolution = initials;
VectorValues workingLambdas;

16
gtsam/linear/QPSolver.h
View File

@ -107,20 +107,6 @@ public:
/**
* Compute step size alpha for the new solution x' = xk + alpha*p, where alpha \in [0,1]
* We have to make sure the new solution with alpha satisfies all INACTIVE ineq constraints
* If some inactive ineq constraints complain about the full step (alpha = 1),
* we have to adjust alpha to stay within the ineq constraints' feasible regions.
*
* For each inactive ineq j:
* - We already have: aj'*xk - bj <= 0, since xk satisfies all ineq constraints
* - We want: aj'*(xk + alpha*p) - bj <= 0
* - If aj'*p <= 0, we have: aj'*(xk + alpha*p) <= aj'*xk <= bj, for all alpha>0
* it's good!
* - We only care when aj'*p > 0. In this case, we need to choose alpha so that
* aj'*xk + alpha*aj'*p - bj <= 0 --> alpha <= (bj - aj'*xk) / (aj'*p)
* We want to step as far as possible, so we should choose alpha = (bj - aj'*xk) / (aj'*p)
*
* We want the minimum of all those alphas among all inactive ineq.
*
* @return a tuple of (alpha, factorIndex, sigmaIndex) where (factorIndex, sigmaIndex)
* is the constraint that has minimum alpha, or (-1,-1) if alpha = 1.
@ -156,7 +142,7 @@ public:
private:
/// Collect all free Hessians involving constrained variables into a graph
GaussianFactorGraph::shared_ptr unconstrainedHessiansOfConstrainedVars(
const GaussianFactorGraph& graph, const KeySet& constrainedVars) const;
const GaussianFactorGraph& graph, const std::set<Key>& constrainedVars) const;
};