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