diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index 4081c4d16..79c69e5ce 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -31,7 +31,10 @@ QPSolver::QPSolver(const QP& qp) : qp_(qp) { VectorValues QPSolver::solveWithCurrentWorkingSet( const LinearInequalityFactorGraph& workingSet) const { GaussianFactorGraph workingGraph = baseGraph_; - workingGraph.push_back(workingSet); + BOOST_FOREACH(const LinearInequality::shared_ptr& factor, workingSet) { + if (factor->active()) + workingGraph.push_back(factor); + } return workingGraph.optimize(); } @@ -48,30 +51,19 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key, AtermsInequalities.end()); // Collect the gradients of unconstrained cost factors to the b vector - Vector b = zero(delta.at(key).size()); - if (costVariableIndex_.find(key) != costVariableIndex_.end()) { - BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) { - GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx); - b += factor->gradient(key, delta); + if (Aterms.size() > 0) { + Vector b = zero(delta.at(key).size()); + if (costVariableIndex_.find(key) != costVariableIndex_.end()) { + BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) { + GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx); + b += factor->gradient(key, delta); + } } + return boost::make_shared(Aterms, b, noiseModel::Constrained::All(b.rows())); } - - return boost::make_shared(Aterms, b, noiseModel::Constrained::All(b.rows())); -} - -//****************************************************************************** -GaussianFactor::shared_ptr QPSolver::createDualPrior( - const LinearInequality::shared_ptr& factor) const { - Vector sigmas = factor->get_model()->sigmas(); - size_t n = sigmas.rows(); - Matrix A = eye(n); - for (size_t i = 0; i(); } - - Vector b = zero(n); - return boost::make_shared(factor->dualKey(), A, b); } //****************************************************************************** @@ -80,50 +72,32 @@ GaussianFactorGraph::shared_ptr QPSolver::buildDualGraph( GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); BOOST_FOREACH(Key key, constrainedKeys_) { // Each constrained key becomes a factor in the dual graph - dualGraph->push_back(createDualFactor(key, workingSet, delta)); - } - - // Add prior for inactive dual variables - BOOST_FOREACH(const LinearInequality::shared_ptr& factor, workingSet) { - dualGraph->push_back(createDualPrior(factor)); + JacobianFactor::shared_ptr dualFactor = createDualFactor(key, workingSet, delta); + if (!dualFactor->empty()) + dualGraph->push_back(dualFactor); } return dualGraph; } //****************************************************************************** -pair QPSolver::identifyLeavingConstraint( +int QPSolver::identifyLeavingConstraint( const LinearInequalityFactorGraph& workingSet, const VectorValues& lambdas) const { - int worstFactorIx = -1, worstSigmaIx = -1; + int worstFactorIx = -1; // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either // inactive or a good inequality constraint, so we don't care! double maxLambda = 0.0; for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); - Vector lambda = lambdas.at(factor->dualKey()); - Vector sigmas = factor->get_model()->sigmas(); - for (size_t j = 0; j < sigmas.size(); ++j) - // If it is an active constraint, and lambda is larger than the current max - if (sigmas[j] == ACTIVE && lambda[j] > maxLambda) { + if (factor->active()) { + double lambda = lambdas.at(factor->dualKey())[0]; + if (lambda > maxLambda) { worstFactorIx = factorIx; - worstSigmaIx = j; - maxLambda = lambda[j]; + maxLambda = lambda; } + } } - return make_pair(worstFactorIx, worstSigmaIx); -} - -//****************************************************************************** -LinearInequalityFactorGraph QPSolver::updateWorkingSet( - const LinearInequalityFactorGraph& workingSet, int factorIx, int sigmaIx, - double state) const { - LinearInequalityFactorGraph newWorkingSet = workingSet; - if (factorIx < 0 || sigmaIx < 0) - return newWorkingSet; - Vector sigmas = newWorkingSet.at(factorIx)->get_model()->sigmas(); - sigmas[sigmaIx] = state; - newWorkingSet.at(factorIx)->setModel(true, sigmas); - return newWorkingSet; + return worstFactorIx; } //****************************************************************************** @@ -142,45 +116,43 @@ LinearInequalityFactorGraph QPSolver::updateWorkingSet( * * We want the minimum of all those alphas among all inactive inequality. */ -boost::tuple QPSolver::computeStepSize( +boost::tuple QPSolver::computeStepSize( const LinearInequalityFactorGraph& workingSet, const VectorValues& xk, const VectorValues& p) const { static bool debug = false; double minAlpha = 1.0; - int closestFactorIx = -1, closestSigmaIx = -1; + int closestFactorIx = -1; for(size_t factorIx = 0; factorIxget_model()->sigmas(); - Vector b = factor->getb(); - for (size_t s = 0; s < sigmas.size(); ++s) { - // If it is an inactive inequality, compute alpha and update min - if (sigmas[s] == INACTIVE) { - // Compute aj'*p - double ajTp = factor->dotProductRow(s, p); + double b = factor->getb()[0]; + // only check inactive factors + if (!factor->active()) { + // Compute a'*p + double aTp = factor->dotProductRow(p); - // Check if aj'*p >0. Don't care if it's not. - if (ajTp <= 0) - continue; + // Check if a'*p >0. Don't care if it's not. + if (aTp <= 0) + continue; - // Compute aj'*xk - double ajTx = factor->dotProductRow(s, xk); + // Compute a'*xk + double aTx = factor->dotProductRow(xk); - // alpha = (bj - aj'*xk) / (aj'*p) - double alpha = (b[s] - ajTx) / ajTp; - if (debug) - cout << "alpha: " << alpha << endl; + // alpha = (b - a'*xk) / (a'*p) + double alpha = (b - aTx) / aTp; + if (debug) + cout << "alpha: " << alpha << endl; - // We want the minimum of all those max alphas - if (alpha < minAlpha) { - closestFactorIx = factorIx; - closestSigmaIx = s; - minAlpha = alpha; - } + // We want the minimum of all those max alphas + if (alpha < minAlpha) { + closestFactorIx = factorIx; + minAlpha = alpha; } } + } - return boost::make_tuple(minAlpha, closestFactorIx, closestSigmaIx); + + return boost::make_tuple(minAlpha, closestFactorIx); } //****************************************************************************** @@ -204,21 +176,18 @@ QPState QPSolver::iterate(const QPState& state) const { if (debug) duals.print("Duals :"); - int leavingFactor, leavingSigmaIx; - boost::tie(leavingFactor, leavingSigmaIx) = // - identifyLeavingConstraint(state.workingSet, duals); + int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); if (debug) - cout << "violated active inequality - factorIx, sigmaIx: " << leavingFactor - << " " << leavingSigmaIx << endl; + cout << "leavingFactor: " << leavingFactor << endl; // If all inequality constraints are satisfied: We have the solution!! - if (leavingFactor < 0 || leavingSigmaIx < 0) { + if (leavingFactor < 0) { return QPState(newValues, duals, state.workingSet, true); } else { // Inactivate the leaving constraint - LinearInequalityFactorGraph newWorkingSet = updateWorkingSet( - state.workingSet, leavingFactor, leavingSigmaIx, INACTIVE); + LinearInequalityFactorGraph newWorkingSet = state.workingSet; + newWorkingSet.at(leavingFactor)->inactivate(); return QPState(newValues, duals, newWorkingSet, false); } } @@ -226,16 +195,18 @@ QPState QPSolver::iterate(const QPState& state) const { // If we CAN make some progress // Adapt stepsize if some inactive constraints complain about this move double alpha; - int factorIx, sigmaIx; + int factorIx; VectorValues p = newValues - state.values; - boost::tie(alpha, factorIx, sigmaIx) = // + boost::tie(alpha, factorIx) = // computeStepSize(state.workingSet, state.values, p); if (debug) - cout << "alpha, factorIx, sigmaIx: " << alpha << " " << factorIx << " " - << sigmaIx << endl; + cout << "alpha, factorIx: " << alpha << " " << factorIx << " " + << endl; + // also add to the working set the one that complains the most - LinearInequalityFactorGraph newWorkingSet = // - updateWorkingSet(state.workingSet, factorIx, sigmaIx, ACTIVE); + LinearInequalityFactorGraph newWorkingSet = state.workingSet; + if (factorIx >= 0) + newWorkingSet.at(factorIx)->activate(); // step! newValues = state.values + alpha * p; @@ -251,14 +222,12 @@ LinearInequalityFactorGraph QPSolver::identifyActiveConstraints( LinearInequalityFactorGraph workingSet; BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities){ LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); - Vector e = workingFactor->error_vector(initialValues); - Vector sigmas = zero(e.rows()); - for (int i = 0; i < e.rows(); ++i){ - if (fabs(e[i])>1e-7){ - sigmas[i] = INACTIVE; - } + double error = workingFactor->error(initialValues); + if (fabs(error)>1e-7){ + workingFactor->inactivate(); + } else { + workingFactor->activate(); } - workingFactor->setModel(true, sigmas); workingSet.push_back(workingFactor); } return workingSet; @@ -410,13 +379,12 @@ boost::tuplerows(); - terms.push_back(make_pair(slackKey, -eye(dim))); - inequalities->push_back(LinearInequality(terms, factor->getb(), + terms.push_back(make_pair(slackKey, -eye(1))); + inequalities->push_back(LinearInequality(terms, factor->getb()[0], factor->dualKey())); // Add lower bound for this slack key - slackLowerBounds.insert(slackKey, zero(dim)); + slackLowerBounds.insert(slackKey, zero(1)); // Increase slackKey for the next slack variable slackKey++; } diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index 8d51322ae..e7148cbaa 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -15,9 +15,6 @@ namespace gtsam { -static const double ACTIVE = 0.0; -static const double INACTIVE = std::numeric_limits::infinity(); - /// This struct holds the state of QPSolver at each iteration struct QPState { VectorValues values; @@ -73,6 +70,7 @@ public: if (variableIndex.find(key) != variableIndex.end()) { BOOST_FOREACH(size_t factorIx, variableIndex[key]){ typename FACTOR::shared_ptr factor = graph.at(factorIx); + if (!factor->active()) continue; Matrix Ai = factor->getA(factor->find(key)).transpose(); Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); } @@ -85,15 +83,6 @@ public: const LinearInequalityFactorGraph& workingSet, const VectorValues& delta) const; - /** - * Create dummy prior for inactive dual variables - * TODO: This might be inefficient but I don't know how to avoid - * the Indeterminant exception due to no information for the duals - * on inactive components of the constraints. - */ - GaussianFactor::shared_ptr createDualPrior( - const LinearInequality::shared_ptr& factor) const; - /** * Build the dual graph to solve for the Lagrange multipliers. * @@ -161,17 +150,10 @@ public: * And we want to remove the worst one with the largest lambda from the active set. * */ - std::pair identifyLeavingConstraint( + int identifyLeavingConstraint( const LinearInequalityFactorGraph& workingSet, const VectorValues& lambdas) const; - /** - * Inactivate or activate an inequality constraint - */ - LinearInequalityFactorGraph updateWorkingSet( - const LinearInequalityFactorGraph& workingSet, int factorIx, int sigmaIx, - double state) const; - /** * Compute step size alpha for the new solution x' = xk + alpha*p, where alpha \in [0,1] * @@ -180,7 +162,7 @@ public: * This constraint will be added to the working set and become active * in the next iteration */ - boost::tuple computeStepSize( + boost::tuple computeStepSize( const LinearInequalityFactorGraph& workingSet, const VectorValues& xk, const VectorValues& p) const; diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 6ac329688..fef60ef56 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -26,6 +26,8 @@ using namespace std; using namespace gtsam; using namespace gtsam::symbol_shorthand; +const Matrix One = ones(1,1); + /* ************************************************************************* */ // Create test graph according to Forst10book_pg171Ex5 QP createTestCase() { @@ -40,12 +42,10 @@ QP createTestCase() { 2.0 * ones(1, 1), zero(1), 10.0)); // Inequality constraints - // Jacobian factors represent Ax-b, hence - // 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); - qp.inequalities.push_back(LinearInequality(X(1), A1, X(2), A2, b, 0)); + qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), X(2), ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 + qp.inequalities.push_back(LinearInequality(X(1), -ones(1,1), 0, 1)); // -x1 <= 0 + qp.inequalities.push_back(LinearInequality(X(2), -ones(1,1), 0, 2)); // -x2 <= 0 + qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), 1.5, 3)); // x1 <= 3/2 return qp; } @@ -66,20 +66,20 @@ TEST(QPSolver, constraintsAux) { QPSolver solver(qp); VectorValues lambdas; - lambdas.insert(0, (Vector(4) << -0.5, 0.0, 0.3, 0.1)); - int factorIx, lambdaIx; - boost::tie(factorIx, lambdaIx) = solver.identifyLeavingConstraint( - qp.inequalities, lambdas); - LONGS_EQUAL(0, factorIx); - LONGS_EQUAL(2, lambdaIx); + lambdas.insert(0, (Vector(1) << -0.5)); + lambdas.insert(1, (Vector(1) << 0.0)); + lambdas.insert(2, (Vector(1) << 0.3)); + lambdas.insert(3, (Vector(1) << 0.1)); + int factorIx = solver.identifyLeavingConstraint(qp.inequalities, lambdas); + LONGS_EQUAL(2, factorIx); VectorValues lambdas2; - lambdas2.insert(0, (Vector(4) << -0.5, 0.0, -0.3, -0.1)); - int factorIx2, lambdaIx2; - boost::tie(factorIx2, lambdaIx2) = solver.identifyLeavingConstraint( - qp.inequalities, lambdas2); + lambdas2.insert(0, (Vector(1) << -0.5)); + lambdas2.insert(1, (Vector(1) << 0.0)); + lambdas2.insert(2, (Vector(1) << -0.3)); + lambdas2.insert(3, (Vector(1) << -0.1)); + int factorIx2 = solver.identifyLeavingConstraint(qp.inequalities, lambdas2); LONGS_EQUAL(-1, factorIx2); - LONGS_EQUAL(-1, lambdaIx2); } /* ************************************************************************* */ @@ -135,9 +135,10 @@ TEST(QPSolver, indentifyActiveConstraints) { LinearInequalityFactorGraph workingSet = solver.identifyActiveConstraints(qp.inequalities, currentSolution); - Vector actualSigmas = workingSet.at(0)->get_model()->sigmas(); - Vector expectedSigmas = (Vector(4) << INACTIVE, ACTIVE, ACTIVE, INACTIVE); - CHECK(assert_equal(expectedSigmas, actualSigmas, 1e-100)); + CHECK(!workingSet.at(0)->active()); // inactive + CHECK(workingSet.at(1)->active()); // active + CHECK(workingSet.at(2)->active()); // active + CHECK(!workingSet.at(3)->active()); // inactive VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet); VectorValues expectedSolution; @@ -159,19 +160,18 @@ TEST(QPSolver, iterate) { std::vector expectedSolutions(4), expectedDuals(4); expectedSolutions[0].insert(X(1), (Vector(1) << 0.0)); expectedSolutions[0].insert(X(2), (Vector(1) << 0.0)); - expectedDuals[0].insert(0, (Vector(4) << 0, 3, 0, 0)); + expectedDuals[0].insert(1, (Vector(1) << 3)); + expectedDuals[0].insert(2, (Vector(1) << 0)); expectedSolutions[1].insert(X(1), (Vector(1) << 1.5)); expectedSolutions[1].insert(X(2), (Vector(1) << 0.0)); - expectedDuals[1].insert(0, (Vector(4) << 0, 0, 1.5, 0)); + expectedDuals[1].insert(3, (Vector(1) << 1.5)); expectedSolutions[2].insert(X(1), (Vector(1) << 1.5)); expectedSolutions[2].insert(X(2), (Vector(1) << 0.75)); - expectedDuals[2].insert(0, (Vector(4) << 0, 0, 1.5, 0)); expectedSolutions[3].insert(X(1), (Vector(1) << 1.5)); expectedSolutions[3].insert(X(2), (Vector(1) << 0.5)); - expectedDuals[3].insert(0, (Vector(4) << -0.5, 0, 0, 0)); LinearInequalityFactorGraph workingSet = solver.identifyActiveConstraints(qp.inequalities, currentSolution); @@ -223,12 +223,11 @@ QP createTestMatlabQPEx() { 2.0 * ones(1, 1), 6 * ones(1), 1000.0)); // Inequality constraints - // Jacobian factors represent Ax-b, hence - // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 - Matrix A1 = (Matrix(5, 1) << 1, -1, 2, -1, 0); - Matrix A2 = (Matrix(5, 1) << 1, 2, 1, 0, -1); - Vector b = (Vector(5) << 2, 2, 3, 0, 0); - qp.inequalities.push_back(LinearInequality(X(1), A1, X(2), A2, b, 0)); + qp.inequalities.push_back(LinearInequality(X(1), One, X(2), One, 2, 0)); // x1 + x2 <= 2 + qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2*One, 2, 1)); //-x1 + 2*x2 <=2 + qp.inequalities.push_back(LinearInequality(X(1), 2*One, X(2), One, 3, 2)); // 2*x1 + x2 <=3 + qp.inequalities.push_back(LinearInequality(X(1), -One, 0, 3)); // -x1 <= 0 + qp.inequalities.push_back(LinearInequality(X(2), -One, 0, 4)); // -x2 <= 0 return qp; } @@ -256,16 +255,11 @@ QP createTestNocedal06bookEx16_4() { qp.cost.push_back(JacobianFactor(X(2), ones(1, 1), 2.5 * ones(1))); // Inequality constraints - qp.inequalities.push_back( - LinearInequality(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), 2 * ones(1), - 0)); - qp.inequalities.push_back( - LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), 6 * ones(1), 1)); - qp.inequalities.push_back( - LinearInequality(X(1), ones(1, 1), X(2), -2 * ones(1, 1), 2 * ones(1), - 2)); - qp.inequalities.push_back(LinearInequality(X(1), -ones(1, 1), zero(1), 3)); - qp.inequalities.push_back(LinearInequality(X(2), -ones(1, 1), zero(1), 4)); + qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, 2, 0)); + qp.inequalities.push_back(LinearInequality(X(1), One, X(2), 2 * One, 6, 1)); + qp.inequalities.push_back(LinearInequality(X(1), One, X(2), -2 * One, 2, 2)); + qp.inequalities.push_back(LinearInequality(X(1), -One, 0.0, 3)); + qp.inequalities.push_back(LinearInequality(X(2), -One, 0.0, 4)); return qp; } @@ -317,16 +311,11 @@ QP modifyNocedal06bookEx16_4() { // Inequality constraints noiseModel::Constrained::shared_ptr noise = noiseModel::Constrained::MixedSigmas((Vector(1) << -1)); - qp.inequalities.push_back( - LinearInequality(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), -1 * ones(1), - 0)); - qp.inequalities.push_back( - LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), 6 * ones(1), 1)); - qp.inequalities.push_back( - LinearInequality(X(1), ones(1, 1), X(2), -2 * ones(1, 1), 2 * ones(1), - 2)); - qp.inequalities.push_back(LinearInequality(X(1), -ones(1, 1), zero(1), 3)); - qp.inequalities.push_back(LinearInequality(X(2), -ones(1, 1), zero(1), 4)); + qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, -1, 0)); + qp.inequalities.push_back(LinearInequality(X(1), One, X(2), 2 * One, 6, 1)); + qp.inequalities.push_back(LinearInequality(X(1), One, X(2), -2 * One, 2, 2)); + qp.inequalities.push_back(LinearInequality(X(1), -One, 0.0, 3)); + qp.inequalities.push_back(LinearInequality(X(2), -One, 0.0, 4)); return qp; } @@ -360,18 +349,15 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4_findInitialPoint) { LinearInequalityFactorGraph expectedInequalities; expectedInequalities.push_back( - LinearInequality(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), X(3), - -ones(1, 1), -1 * ones(1), 0)); + LinearInequality(X(1), -One, X(2), 2 * One, X(3), -One, -1, 0)); expectedInequalities.push_back( - LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), X(4), - -ones(1, 1), 6 * ones(1), 1)); + LinearInequality(X(1), One, X(2), 2 * One, X(4), -One, 6, 1)); expectedInequalities.push_back( - LinearInequality(X(1), ones(1, 1), X(2), -2 * ones(1, 1), X(5), - -ones(1, 1), 2 * ones(1), 2)); + LinearInequality(X(1), One, X(2), -2 * One, X(5), -One, 2, 2)); expectedInequalities.push_back( - LinearInequality(X(1), -ones(1, 1), X(6), -ones(1, 1), zero(1), 3)); + LinearInequality(X(1), -One, X(6), -One, 0, 3)); expectedInequalities.push_back( - LinearInequality(X(2), -ones(1, 1), X(7), -ones(1, 1), zero(1), 4)); + LinearInequality(X(2), -One, X(7), -One, 0, 4)); EXPECT(assert_equal(expectedInequalities, *inequalities)); bool isFeasible; @@ -414,7 +400,7 @@ TEST(QPSolver, failedSubproblem) { qp.cost.push_back(JacobianFactor(X(1), eye(2), zero(2))); qp.cost.push_back(HessianFactor(X(1), zeros(2, 2), zero(2), 100.0)); qp.inequalities.push_back( - LinearInequality(X(1), (Matrix(1, 2) << -1.0, 0.0), -ones(1), 0)); + LinearInequality(X(1), (Matrix(1,2) << -1.0, 0.0), -1.0, 0)); VectorValues expected; expected.insert(X(1), (Vector(2) << 1.0, 0.0));