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

first ineq QP test passed!

release/4.3a0
thduynguyen 2014-04-15 13:55:24 -04:00
parent 22cbe326e3
commit b07b431ac0
1 changed files with 203 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

290
gtsam/linear/tests/testQPSolver.cpp
View File

@ -27,22 +27,24 @@ using namespace std;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;
#define TEST_DISABLED(testGroup, testName)\
void testGroup##testName##Test(TestResult& result_, const std::string& name_)
class QPSolver {
const GaussianFactorGraph& graph_; //!< the original graph, can't be modified!
GaussianFactorGraph workingGraph_; //!< working set
VectorValues currentSolution_;
FastVector<size_t> constraintIndices_; //!< Indices of constrained factors in the original graph
GaussianFactorGraph::shared_ptr freeHessians_;
VariableIndex freeHessianVarIndex_;
GaussianFactorGraph::shared_ptr freeHessians_; //!< unconstrained Hessians of constrained variables
VariableIndex freeHessianFactorIndex_; //!< indices of unconstrained Hessian factors of constrained variables
// gtsam calls it "VariableIndex", but I think FactorIndex
// makes more sense, because it really stores factor indices.
VariableIndex fullFactorIndices_; //!< indices of factors involving each variable.
// gtsam calls it "VariableIndex", but I think FactorIndex
// makes more sense, because it really stores factor indices.
public:
/// Constructor
QPSolver(const GaussianFactorGraph& graph, const VectorValues& initials) :
graph_(graph), workingGraph_(graph.clone()), currentSolution_(initials),
fullFactorIndices_(graph) {
QPSolver(const GaussianFactorGraph& graph) :
graph_(graph), fullFactorIndices_(graph) {
// Split the original graph into unconstrained and constrained part
// and collect indices of constrained factors
@ -64,7 +66,7 @@ public:
// Collect unconstrained hessians of constrained vars to build dual graph
freeHessians_ = unconstrainedHessiansOfConstrainedVars(graph, constrainedVars);
freeHessianVarIndex_ = VariableIndex(*freeHessians_);
freeHessianFactorIndex_ = VariableIndex(*freeHessians_);
}
/// Return indices of all constrained factors
@ -169,14 +171,15 @@ public:
* - The constant term b is \grad f(xi), which can be computed from all unconstrained
* Hessian factors connecting to xi: \grad f(xi) = \sum_j G_ij*xj - gi
*/
GaussianFactorGraph::shared_ptr buildDualGraph(const VectorValues& x0) const {
GaussianFactorGraph buildDualGraph(const GaussianFactorGraph& graph,
const VectorValues& x0) const {
// The dual graph to return
GaussianFactorGraph::shared_ptr dualGraph = boost::make_shared<GaussianFactorGraph>();
GaussianFactorGraph dualGraph;
// For each variable xi involving in some constraint, compute the unconstrained gradient
// wrt xi from the prebuilt freeHessian graph
// \grad f(xi) = \frac{\partial f}{\partial xi}' = \sum_j G_ij*xj - gi
BOOST_FOREACH(const VariableIndex::value_type& xiKey_factors, freeHessianVarIndex_) {
BOOST_FOREACH(const VariableIndex::value_type& xiKey_factors, freeHessianFactorIndex_) {
Key xiKey = xiKey_factors.first;
VariableIndex::Factors xiFactors = xiKey_factors.second;
@ -215,7 +218,7 @@ public:
// gradc_k(xi) = \frac{\partial c_k}{\partial xi}'
std::vector<std::pair<Key, Matrix> > lambdaTerms; // collection of lambda_k, and gradc_k
BOOST_FOREACH(size_t factorIndex, fullFactorIndices_[xiKey]) {
JacobianFactor::shared_ptr factor = toJacobian(workingGraph_.at(factorIndex));
JacobianFactor::shared_ptr factor = toJacobian(graph.at(factorIndex));
if (!factor || !factor->isConstrained()) continue;
// Gradient is the transpose of the Jacobian: A_k = gradc_k(xi) = \frac{\partial c_k}{\partial xi}'
// Each column for each lambda_k corresponds to [the transpose of] each constrained row factor
@ -223,7 +226,9 @@ public:
// Deal with mixed sigmas: no information if sigma != 0
Vector sigmas = factor->get_model()->sigmas();
for (size_t sigmaIx = 0; sigmaIx<sigmas.size(); ++sigmaIx) {
if (fabs(sigmas[sigmaIx]) > 1e-9) { // if it's either ineq (sigma<0) or unconstrained (sigma>0)
// if it's either ineq (sigma<0) or unconstrained (sigma>0)
// we have no information about it
if (fabs(sigmas[sigmaIx]) > 1e-9) {
A_k.col(sigmaIx) = zero(A_k.rows());
}
}
@ -232,14 +237,24 @@ public:
}
// Enforce constrained noise model so lambdas are solved with QR
// and should exactly satisfy all the equations
dualGraph->push_back(JacobianFactor(lambdaTerms, gradf_xi,
dualGraph.push_back(JacobianFactor(lambdaTerms, gradf_xi,
noiseModel::Constrained::All(gradf_xi.size())));
// Add 0 priors on all lambdas to make sure the graph is solvable
// TODO: Can we do for all lambdas like this, or only for those with no information?
BOOST_FOREACH(size_t factorIndex, fullFactorIndices_[xiKey]) {
JacobianFactor::shared_ptr factor = toJacobian(graph.at(factorIndex));
if (!factor || !factor->isConstrained()) continue;
size_t dim= factor->get_model()->dim();
// Use factorIndex as the lambda's key.
dualGraph.push_back(JacobianFactor(factorIndex, eye(dim), zero(dim)));
}
}
return dualGraph;
}
/// Find max lambda element
std::pair<int, int> findMostViolatedIneqConstraint(const VectorValues& lambdas) {
std::pair<int, int> findWeakestViolationIneq(const VectorValues& lambdas) const {
int worstFactorIx = -1, worstSigmaIx = -1;
double maxLambda = 0.0;
BOOST_FOREACH(size_t factorIx, constraintIndices_) {
@ -256,54 +271,127 @@ public:
return make_pair(worstFactorIx, worstSigmaIx);
}
/** Iterate 1 step */
// bool iterate() {
// // Obtain the solution from the current working graph
// VectorValues newSolution = workingGraph_.optimize();
//
// // If we CAN'T move further
// if (newSolution == currentSolution) {
// // Compute lambda from the dual graph
// GaussianFactorGraph dualGraph = buildDualGraph(graph, newSolution);
// VectorValues lambdas = dualGraph.optimize();
//
// int factorIx, sigmaIx;
// boost::tie(factorIx, sigmaIx) = findMostViolatedIneqConstraint(lambdas);
// // If all constraints are satisfied: We have found the solution!!
// if (factorIx < 0) {
// return true;
// }
// else {
// // If some constraints are violated!
// Vector sigmas = toJacobian(workingGraph.at(factorIx))->get_model()->sigmas();
// sigmas[sigmaIx] = 0.0;
// toJacobian()->setModel(true, sigmas);
// // No need to update the currentSolution, since we haven't moved anywhere
// }
// }
// else {
// // If we CAN make some progress
// // Adapt stepsize if some inactive inequality constraints complain about this move
// // also add to the working set the one that complains the most
// VectorValues alpha = updateWorkingSetInplace();
// currentSolution_ = (1 - alpha) * currentSolution_ + alpha * newSolution;
// }
//
// return false;
// }
//
// VectorValues optimize() const {
// bool converged = false;
// while (!converged) {
// converged = iterate();
// }
// }
/**
* Deactivate or activate an ineq constraint in place
* Warning: modify in-place to avoid copy/clone
* @return true if update successful
*/
bool updateWorkingSetInplace(GaussianFactorGraph& workingGraph,
int factorIx, int sigmaIx, double newSigma) const {
if (factorIx < 0 || sigmaIx < 0)
return false;
Vector sigmas = toJacobian(workingGraph.at(factorIx))->get_model()->sigmas();
sigmas[sigmaIx] = newSigma; // removing it from the working set
toJacobian(workingGraph.at(factorIx))->setModel(true, sigmas);
return true;
}
/**
* Compute step size
*/
boost::tuple<double, int, int> computeStepSize(const GaussianFactorGraph& workingGraph,
const VectorValues& xk, const VectorValues& p) const {
static bool debug = true;
double minAlpha = 1.0;
int closestFactorIx = -1, closestSigmaIx = -1;
BOOST_FOREACH(size_t factorIx, constraintIndices_) {
JacobianFactor::shared_ptr jacobian = toJacobian(workingGraph.at(factorIx));
Vector sigmas = jacobian->get_model()->sigmas();
Vector b = jacobian->getb();
for (size_t s = 0; s<sigmas.size(); ++s)
// If it is an inactive inequality, compute alpha and update min
if (sigmas[s]<0) {
double ajTp = 0.0;
for (Factor::const_iterator xj = jacobian->begin(); xj != jacobian->end(); ++xj) {
Vector pj = p.at(*xj);
Vector aj = jacobian->getA(xj).row(s);
ajTp += aj.dot(pj);
}
if (debug) {
cout << "s, ajTp: " << s << " " << ajTp << endl;
}
if (ajTp<=0) continue;
double ajTx = 0.0;
for (Factor::const_iterator xj = jacobian->begin(); xj != jacobian->end(); ++xj) {
Vector xkj = xk.at(*xj);
Vector aj = jacobian->getA(xj).row(s);
ajTx += aj.dot(xkj);
}
if (debug) {
cout << "b[s], ajTx: " << b[s] << " " << ajTx << " " << ajTp << endl;
}
double alpha = (b[s] - ajTx)/ajTp; // TODO: compute me!
if (debug) {
cout << "alpha: " << alpha << endl;
}
if (alpha < minAlpha) {
closestFactorIx = factorIx;
closestSigmaIx = s;
minAlpha = alpha;
}
}
}
return boost::make_tuple(minAlpha, closestFactorIx, closestSigmaIx);
}
/** Iterate 1 step, modify workingGraph and currentSolution in place */
bool iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& currentSolution) const {
static bool debug = true;
// Obtain the solution from the current working graph
VectorValues newSolution = workingGraph.optimize();
if (debug) newSolution.print("New solution:");
// If we CAN'T move further
if (newSolution.equals(currentSolution, 1e-5)) {
// Compute lambda from the dual graph
GaussianFactorGraph dualGraph = buildDualGraph(workingGraph, newSolution);
if (debug) dualGraph.print("Dual graph: ");
VectorValues lambdas = dualGraph.optimize();
if (debug) lambdas.print("lambdas :");
int factorIx, sigmaIx;
boost::tie(factorIx, sigmaIx) = findWeakestViolationIneq(lambdas);
// Try to disactivate the weakest violated ineq constraints
// if not successful, i.e. all ineq constraints are satisfied: We have the solution!!
if (!updateWorkingSetInplace(workingGraph, factorIx, sigmaIx, -1.0))
return true;
}
else {
// If we CAN make some progress
// Adapt stepsize if some inactive inequality constraints complain about this move
double alpha;
int factorIx, sigmaIx;
VectorValues p = newSolution - currentSolution;
boost::tie(alpha, factorIx, sigmaIx) = computeStepSize(workingGraph, currentSolution, p);
if (debug) {
cout << "alpha, factorIx, sigmaIx: " << alpha << " " << factorIx << " " << sigmaIx << endl;
}
// also add to the working set the one that complains the most
updateWorkingSetInplace(workingGraph, factorIx, sigmaIx, 0.0);
// step!
currentSolution = currentSolution + alpha * p;
}
return false;
}
VectorValues optimize(const VectorValues& initials) const {
GaussianFactorGraph workingGraph = graph_.clone();
VectorValues currentSolution = initials;
bool converged = false;
while (!converged) {
converged = iterateInPlace(workingGraph, currentSolution);
}
return currentSolution;
}
};
/* ************************************************************************* */
// Create test graph according to Forst10book_pg171Ex5
std::pair<GaussianFactorGraph, VectorValues> createTestCase() {
GaussianFactorGraph createTestCase() {
GaussianFactorGraph graph;
// Objective functions x1^2 - x1*x2 + x2^2 - 3*x1
@ -312,31 +400,25 @@ std::pair<GaussianFactorGraph, VectorValues> createTestCase() {
// Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 0
graph.push_back(
HessianFactor(X(1), X(2), 2.0*ones(1, 1), -ones(1, 1), 3.0*ones(1),
2.0*ones(1, 1), zero(1), 1.0));
2.0*ones(1, 1), zero(1), 10.0));
// Inequality constraints
// x1 + x2 <= 2 --> x1 + x2 -2 <= 0, hence we negate the b vector
// Jacobian factors represent Ax-b, ehnce
// x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
Matrix A1 = (Matrix(4, 1)<<1, -1, 0, 1);
Matrix A2 = (Matrix(4, 1)<<1, 0, -1, 0);
Vector b = -(Vector(4)<<2, 0, 0, 1.5);
Vector b = (Vector(4)<<2, 0, 0, 1.5);
// Special constrained noise model denoting <= inequalities with negative sigmas
noiseModel::Constrained::shared_ptr noise =
noiseModel::Constrained::MixedSigmas((Vector(4)<<-1, -1, -1, -1));
graph.push_back(JacobianFactor(X(1), A1, X(2), A2, b, noise));
// Initials values
VectorValues initials;
initials.insert(X(1), ones(1));
initials.insert(X(2), ones(1));
return make_pair(graph, initials);
return graph;
}
TEST(QPSolver, constraintsAux) {
GaussianFactorGraph graph;
VectorValues initials;
boost::tie(graph, initials)= createTestCase();
QPSolver solver(graph, initials);
TEST_DISABLED(QPSolver, constraintsAux) {
GaussianFactorGraph graph = createTestCase();
QPSolver solver(graph);
FastVector<size_t> constraintIx = solver.constraintIndices();
LONGS_EQUAL(1, constraintIx.size());
LONGS_EQUAL(1, constraintIx[0]);
@ -344,14 +426,14 @@ TEST(QPSolver, constraintsAux) {
VectorValues lambdas;
lambdas.insert(constraintIx[0], (Vector(4)<< -0.5, 0.0, 0.3, 0.1));
int factorIx, lambdaIx;
boost::tie(factorIx, lambdaIx) = solver.findMostViolatedIneqConstraint(lambdas);
boost::tie(factorIx, lambdaIx) = solver.findWeakestViolationIneq(lambdas);
LONGS_EQUAL(1, factorIx);
LONGS_EQUAL(2, lambdaIx);
VectorValues lambdas2;
lambdas2.insert(constraintIx[0], (Vector(4)<< -0.5, 0.0, -0.3, -0.1));
int factorIx2, lambdaIx2;
boost::tie(factorIx2, lambdaIx2) = solver.findMostViolatedIneqConstraint(lambdas2);
boost::tie(factorIx2, lambdaIx2) = solver.findWeakestViolationIneq(lambdas2);
LONGS_EQUAL(-1, factorIx2);
LONGS_EQUAL(-1, lambdaIx2);
@ -364,8 +446,8 @@ TEST(QPSolver, constraintsAux) {
}
/* ************************************************************************* */
// Create test graph according to Forst10book_pg171Ex5
std::pair<GaussianFactorGraph, VectorValues> createEqualityConstrainedTest() {
// Create a simple test graph with one equality constraint
GaussianFactorGraph createEqualityConstrainedTest() {
GaussianFactorGraph graph;
// Objective functions x1^2 + x2^2
@ -386,28 +468,62 @@ std::pair<GaussianFactorGraph, VectorValues> createEqualityConstrainedTest() {
noiseModel::Constrained::MixedSigmas((Vector(1)<<0.0));
graph.push_back(JacobianFactor(X(1), A1, X(2), A2, b, noise));
return graph;
}
TEST(QPSolver, dual) {
GaussianFactorGraph graph = createEqualityConstrainedTest();
// Initials values
VectorValues initials;
initials.insert(X(1), ones(1));
initials.insert(X(2), ones(1));
return make_pair(graph, initials);
}
QPSolver solver(graph);
TEST(QPSolver, dual) {
GaussianFactorGraph graph;
VectorValues initials;
boost::tie(graph, initials)= createEqualityConstrainedTest();
QPSolver solver(graph, initials);
GaussianFactorGraph::shared_ptr dualGraph = solver.buildDualGraph(initials);
dualGraph->print("Dual graph: ");
VectorValues dual = dualGraph->optimize();
GaussianFactorGraph dualGraph = solver.buildDualGraph(graph, initials);
VectorValues dual = dualGraph.optimize();
VectorValues expectedDual;
expectedDual.insert(1, (Vector(1)<<2.0));
CHECK(assert_equal(expectedDual, dual, 1e-100));
}
/* ************************************************************************* */
TEST(QPSolver, iterate) {
GaussianFactorGraph graph = createTestCase();
QPSolver solver(graph);
GaussianFactorGraph workingGraph = graph.clone();
VectorValues currentSolution;
currentSolution.insert(X(1), zeros(1,1));
currentSolution.insert(X(2), zeros(1,1));
workingGraph.print("workingGraph: ");
bool converged = false;
int it = 0;
while (!converged) {
cout << "+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n";
cout << "Iteration " << it << " :" << endl;
converged = solver.iterateInPlace(workingGraph, currentSolution);
workingGraph.print("workingGraph: ");
currentSolution.print("currentSolution: ");
}
}
/* ************************************************************************* */
TEST(QPSolver, optimize) {
GaussianFactorGraph graph = createTestCase();
QPSolver solver(graph);
VectorValues initials;
initials.insert(X(1), zeros(1,1));
initials.insert(X(2), zeros(1,1));
VectorValues solution = solver.optimize(initials);
solution.print("Solution: ");
}
/* ************************************************************************* */
int main() {
TestResult tr;