diff --git a/gtsam_unstable/linear/LPSolver.cpp b/gtsam_unstable/linear/LPSolver.cpp index 2a7764a7e..e12fa477e 100644 --- a/gtsam_unstable/linear/LPSolver.cpp +++ b/gtsam_unstable/linear/LPSolver.cpp @@ -5,10 +5,12 @@ * @author: Duy-Nguyen Ta */ +#include +#include + +#include #include #include -#include -#include using namespace std; using namespace gtsam; @@ -21,7 +23,7 @@ void LPSolver::buildMetaInformation() { // Collect variables in objective function first BOOST_FOREACH(Key key, objectiveCoeffs_ | boost::adaptors::map_keys) { variableColumnNo_.insert(make_pair(key, firstVarIndex)); - variableDims_.insert(make_pair(key,objectiveCoeffs_.dim(key))); + variableDims_.insert(make_pair(key, objectiveCoeffs_.dim(key))); firstVarIndex += variableDims_[key]; freeVars_.insert(key); } @@ -29,32 +31,32 @@ void LPSolver::buildMetaInformation() { VariableIndex factorIndex(*constraints_); BOOST_FOREACH(Key key, factorIndex | boost::adaptors::map_keys) { if (!variableColumnNo_.count(key)) { - JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast - (constraints_->at(*factorIndex[key].begin())); + JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast< + JacobianFactor>(constraints_->at(*factorIndex[key].begin())); if (!jacobian || !jacobian->isConstrained()) { - throw std::runtime_error("Invalid constrained graph!"); + throw runtime_error("Invalid constrained graph!"); } size_t dim = jacobian->getDim(jacobian->find(key)); variableColumnNo_.insert(make_pair(key, firstVarIndex)); - variableDims_.insert(make_pair(key,dim)); + variableDims_.insert(make_pair(key, dim)); firstVarIndex += variableDims_[key]; freeVars_.insert(key); } } // Collect the remaining keys in lowerBounds - BOOST_FOREACH(Key key, lowerBounds_ | boost::adaptors::map_keys){ + BOOST_FOREACH(Key key, lowerBounds_ | boost::adaptors::map_keys) { if (!variableColumnNo_.count(key)) { variableColumnNo_.insert(make_pair(key, firstVarIndex)); - variableDims_.insert(make_pair(key,lowerBounds_.dim(key))); + variableDims_.insert(make_pair(key, lowerBounds_.dim(key))); firstVarIndex += variableDims_[key]; } freeVars_.erase(key); } // Collect the remaining keys in upperBounds - BOOST_FOREACH(Key key, upperBounds_ | boost::adaptors::map_keys){ + BOOST_FOREACH(Key key, upperBounds_ | boost::adaptors::map_keys) { if (!variableColumnNo_.count(key)) { variableColumnNo_.insert(make_pair(key, firstVarIndex)); - variableDims_.insert(make_pair(key,upperBounds_.dim(key))); + variableDims_.insert(make_pair(key, upperBounds_.dim(key))); firstVarIndex += variableDims_[key]; } freeVars_.erase(key); @@ -67,7 +69,7 @@ void LPSolver::buildMetaInformation() { void LPSolver::addConstraints(const boost::shared_ptr& lp, const JacobianFactor::shared_ptr& jacobian) const { if (!jacobian || !jacobian->isConstrained()) - throw std::runtime_error("LP only accepts constrained factors!"); + throw runtime_error("LP only accepts constrained factors!"); // Build column number from keys KeyVector keys = jacobian->keys(); @@ -77,7 +79,7 @@ void LPSolver::addConstraints(const boost::shared_ptr& lp, Vector sigmas = jacobian->get_model()->sigmas(); Matrix A = jacobian->getA(); Vector b = jacobian->getb(); - for (int i = 0; i& lp, // so we have to make a new copy for every row!!!!! vector columnNoCopy(columnNo); - if (sigmas[i]>0) { - cout << "Warning: Ignore Gaussian noise (sigma>0) in LP constraints!" << endl; + if (sigmas[i] > 0) { + cout << "Warning: Ignore Gaussian noise (sigma>0) in LP constraints!" + << endl; } - int constraintType = (sigmas[i]<0)?LE:EQ; - if(!add_constraintex(lp.get(), columnNoCopy.size(), r.data(), columnNoCopy.data(), - constraintType, b[i])) + int constraintType = (sigmas[i] < 0) ? LE : EQ; + if (!add_constraintex(lp.get(), columnNoCopy.size(), r.data(), + columnNoCopy.data(), constraintType, b[i])) throw runtime_error("LP can't accept Gaussian noise!"); } } - /* ************************************************************************* */ void LPSolver::addBounds(const boost::shared_ptr& lp) const { // Set lower bounds - BOOST_FOREACH(Key key, lowerBounds_ | boost::adaptors::map_keys){ + BOOST_FOREACH(Key key, lowerBounds_ | boost::adaptors::map_keys) { Vector lb = lowerBounds_.at(key); - for (size_t i = 0; i LPSolver::buildModel() const { // Add constraints BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, *constraints_) { - JacobianFactor::shared_ptr jacobian = - boost::dynamic_pointer_cast(factor); + JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast< + JacobianFactor>(factor); addConstraints(lp, jacobian); } @@ -152,8 +154,8 @@ boost::shared_ptr LPSolver::buildModel() const { Vector f = objectiveCoeffs_.vector(keys); vector columnNo = buildColumnNo(keys); - if(!set_obj_fnex(lp.get(), f.size(), f.data(), columnNo.data())) - throw std::runtime_error("lpsolve cannot set obj function!"); + if (!set_obj_fnex(lp.get(), f.size(), f.data(), columnNo.data())) + throw runtime_error("lpsolve cannot set obj function!"); // Set the object direction to minimize set_minim(lp.get()); @@ -169,7 +171,8 @@ VectorValues LPSolver::convertToVectorValues(REAL* row) const { VectorValues values; BOOST_FOREACH(Key key, variableColumnNo_ | boost::adaptors::map_keys) { // Warning: the columnNo starts from 1, but C's array index starts from 0!! - Vector v = Eigen::Map(&row[variableColumnNo_.at(key)-1], variableDims_.at(key)); + Vector v = Eigen::Map(&row[variableColumnNo_.at(key) - 1], + variableDims_.at(key)); values.insert(key, v); } return values; @@ -183,13 +186,15 @@ VectorValues LPSolver::solve() const { /* just out of curioucity, now show the model in lp format on screen */ /* this only works if this is a console application. If not, use write_lp and a filename */ - if (debug) write_LP(lp.get(), stdout); + if (debug) + write_LP(lp.get(), stdout); int ret = ::solve(lp.get()); if (ret != 0) { - throw std::runtime_error( - (boost::format( "lpsolve cannot find the optimal solution and terminates with %d error. "\ - "See lpsolve's solve() documentation for details.") % ret).str()); + throw runtime_error( + (boost::format( + "lpsolve cannot find the optimal solution and terminates with %d error. " + "See lpsolve's solve() documentation for details.") % ret).str()); } REAL* row = NULL; get_ptr_variables(lp.get(), &row); diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index 652f0d430..73382fe3f 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -7,13 +7,14 @@ #pragma once -#include - -#include #include #include #include +#include + +#include + namespace gtsam { /** @@ -36,25 +37,35 @@ public: * We do NOT adopt this convention here. If no lower/upper bounds are specified, the variable will be * set as unbounded, i.e. -inf <= x <= inf. */ - LPSolver(const VectorValues& objectiveCoeffs, const GaussianFactorGraph::shared_ptr& constraints, - const VectorValues& lowerBounds = VectorValues(), const VectorValues& upperBounds = VectorValues()) : - objectiveCoeffs_(objectiveCoeffs), constraints_(constraints), lowerBounds_( - lowerBounds), upperBounds_(upperBounds) { + LPSolver(const VectorValues& objectiveCoeffs, + const GaussianFactorGraph::shared_ptr& constraints, + const VectorValues& lowerBounds = VectorValues(), + const VectorValues& upperBounds = VectorValues()) : + objectiveCoeffs_(objectiveCoeffs), constraints_(constraints), lowerBounds_( + lowerBounds), upperBounds_(upperBounds) { buildMetaInformation(); } - /** - * Build data structures to support converting between gtsam and lpsolve - * TODO: consider lp as a class variable and support setConstraints - * to avoid rebuild this meta data - */ + /** + * Build data structures to support converting between gtsam and lpsolve + * TODO: consider lp as a class variable and support setConstraints + * to avoid rebuild this meta data + */ void buildMetaInformation(); /// Get functions for unittest checking - const std::map& variableColumnNo() const { return variableColumnNo_; } - const std::map& variableDims() const { return variableDims_; } - size_t nrColumns() const {return nrColumns_;} - const KeySet& freeVars() const { return freeVars_; } + const std::map& variableColumnNo() const { + return variableColumnNo_; + } + const std::map& variableDims() const { + return variableDims_; + } + size_t nrColumns() const { + return nrColumns_; + } + const KeySet& freeVars() const { + return freeVars_; + } /** * Build lpsolve's column number for a list of keys @@ -64,7 +75,8 @@ public: std::vector columnNo; BOOST_FOREACH(Key key, keyList) { std::vector varIndices = boost::copy_range >( - boost::irange(variableColumnNo_.at(key), variableColumnNo_.at(key) + variableDims_.at(key))); + boost::irange(variableColumnNo_.at(key), + variableColumnNo_.at(key) + variableDims_.at(key))); columnNo.insert(columnNo.end(), varIndices.begin(), varIndices.end()); } return columnNo; @@ -82,7 +94,6 @@ public: */ void addBounds(const boost::shared_ptr& lp) const; - /** * Main function to build lpsolve model * TODO: consider lp as a class variable and support setConstraints diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index d4eb76f7e..e945391e0 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -5,19 +5,20 @@ * @author: thduynguyen */ -#include -#include #include #include #include +#include +#include + using namespace std; namespace gtsam { /* ************************************************************************* */ QPSolver::QPSolver(const GaussianFactorGraph& graph) : - graph_(graph), fullFactorIndices_(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) { @@ -30,21 +31,21 @@ QPSolver::QPSolver(const GaussianFactorGraph& graph) : } // Collect constrained variable keys - std::set constrainedVars; + set constrainedVars; BOOST_FOREACH(size_t index, constraintIndices_) { KeyVector keys = graph.at(index)->keys(); constrainedVars.insert(keys.begin(), keys.end()); } // Collect unconstrained hessians of constrained vars to build dual graph - freeHessians_ = unconstrainedHessiansOfConstrainedVars(graph, constrainedVars); + freeHessians_ = unconstrainedHessiansOfConstrainedVars(graph, + constrainedVars); freeHessianFactorIndex_ = VariableIndex(*freeHessians_); } - /* ************************************************************************* */ GaussianFactorGraph::shared_ptr QPSolver::unconstrainedHessiansOfConstrainedVars( - const GaussianFactorGraph& graph, const std::set& constrainedVars) const { + const GaussianFactorGraph& graph, const set& constrainedVars) const { VariableIndex variableIndex(graph); GaussianFactorGraph::shared_ptr hfg(new GaussianFactorGraph()); // Collect all factors involving constrained vars @@ -58,7 +59,8 @@ GaussianFactorGraph::shared_ptr QPSolver::unconstrainedHessiansOfConstrainedVars // Convert each factor into Hessian BOOST_FOREACH(size_t factorIndex, factors) { - if (!graph[factorIndex]) continue; + if (!graph[factorIndex]) + continue; // See if this is a Jacobian factor JacobianFactor::shared_ptr jf = toJacobian(graph[factorIndex]); if (jf) { @@ -68,20 +70,21 @@ GaussianFactorGraph::shared_ptr QPSolver::unconstrainedHessiansOfConstrainedVars Vector sigmas = jf->get_model()->sigmas(); Vector newPrecisions(sigmas.size()); bool mixed = false; - for (size_t s=0; sclone()); - newJacobian->setModel(noiseModel::Diagonal::Precisions(newPrecisions)); + newJacobian->setModel( + noiseModel::Diagonal::Precisions(newPrecisions)); hfg->push_back(HessianFactor(*newJacobian)); } - } - else { // unconstrained Jacobian + } else { // unconstrained Jacobian // Convert the original linear factor to Hessian factor // TODO: This may fail and throw the following exception // Assertion failed: (((!PanelMode) && stride==0 && offset==0) || @@ -92,8 +95,7 @@ GaussianFactorGraph::shared_ptr QPSolver::unconstrainedHessiansOfConstrainedVars // 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! + } else { // If it's not a Jacobian, it should be a hessian factor. Just add! hfg->push_back(graph[factorIndex]); } } @@ -111,17 +113,23 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, // 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 - if (debug) freeHessianFactorIndex_.print("freeHessianFactorIndex_: "); + if (debug) + freeHessianFactorIndex_.print("freeHessianFactorIndex_: "); BOOST_FOREACH(const VariableIndex::value_type& xiKey_factors, freeHessianFactorIndex_) { Key xiKey = xiKey_factors.first; VariableIndex::Factors xiFactors = xiKey_factors.second; // Find xi's dim from the first factor on xi - if (xiFactors.size() == 0) continue; - GaussianFactor::shared_ptr xiFactor0 = freeHessians_->at(*xiFactors.begin()); + if (xiFactors.size() == 0) + continue; + GaussianFactor::shared_ptr xiFactor0 = freeHessians_->at( + *xiFactors.begin()); size_t xiDim = xiFactor0->getDim(xiFactor0->find(xiKey)); - if (debug) xiFactor0->print("xiFactor0: "); - if (debug) cout << "xiKey: " << string(Symbol(xiKey)) << ", xiDim: " << xiDim << endl; + if (debug) + xiFactor0->print("xiFactor0: "); + if (debug) + cout << "xiKey: " << string(Symbol(xiKey)) << ", xiDim: " << xiDim + << endl; //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// // Compute the b-vector for the dual factor Ax-b @@ -139,8 +147,7 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, if (xi > xj) { Matrix Gji = factor->info(xj, xi); Gij = Gji.transpose(); - } - else { + } else { Gij = factor->info(xi, xj); } // Accumulate Gij*xj to gradf @@ -155,20 +162,22 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, // Compute the Jacobian A for the dual factor Ax-b // Obtain the jacobians for lambda variables from their corresponding constraints // A = gradc_k(xi) = \frac{\partial c_k}{\partial xi}' - std::vector > lambdaTerms; // collection of lambda_k, and gradc_k - typedef std::pair FactorIx_SigmaIx; - std::vector unconstrainedIndex; // pairs of factorIx,sigmaIx of unconstrained rows + vector > lambdaTerms; // collection of lambda_k, and gradc_k + typedef pair FactorIx_SigmaIx; + vector unconstrainedIndex; // pairs of factorIx,sigmaIx of unconstrained rows BOOST_FOREACH(size_t factorIndex, fullFactorIndices_[xiKey]) { 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}' // Each column for each lambda_k corresponds to [the transpose of] each constrained row factor Matrix A_k = factor->getA(factor->find(xiKey)).transpose(); - if (debug) gtsam::print(A_k, "A_k = "); + if (debug) + gtsam::print(A_k, "A_k = "); // Deal with mixed sigmas: no information if sigma != 0 Vector sigmas = factor->get_model()->sigmas(); - for (size_t sigmaIx = 0; sigmaIx0) // we have no information about it if (fabs(sigmas[sigmaIx]) > 1e-9) { @@ -185,31 +194,37 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++// // Create and add factors to the dual graph // If least square approximation is desired, use unit noise model. - if (debug) cout << "Create dual factor" << endl; + if (debug) + cout << "Create dual factor" << endl; if (useLeastSquare) { - if (debug) cout << "use least square!" << endl; - dualGraph.push_back(JacobianFactor(lambdaTerms, gradf_xi, - noiseModel::Unit::Create(gradf_xi.size()))); - } - else { + if (debug) + cout << "use least square!" << endl; + dualGraph.push_back( + JacobianFactor(lambdaTerms, gradf_xi, + noiseModel::Unit::Create(gradf_xi.size()))); + } else { // Enforce constrained noise model so lambdas are solved with QR // and should exactly satisfy all the equations - if (debug) cout << gradf_xi << endl; - dualGraph.push_back(JacobianFactor(lambdaTerms, gradf_xi, - noiseModel::Constrained::All(gradf_xi.size()))); + if (debug) + cout << gradf_xi << endl; + dualGraph.push_back( + JacobianFactor(lambdaTerms, gradf_xi, + noiseModel::Constrained::All(gradf_xi.size()))); } // Add 0 priors on all lambdas of the unconstrained rows to make sure the graph is solvable - if (debug) cout << "Create priors" << endl; + if (debug) + cout << "Create priors" << endl; BOOST_FOREACH(FactorIx_SigmaIx factorIx_sigmaIx, unconstrainedIndex) { size_t factorIx = factorIx_sigmaIx.first; JacobianFactor::shared_ptr factor = toJacobian(graph.at(factorIx)); - size_t dim= factor->get_model()->dim(); + size_t dim = factor->get_model()->dim(); Matrix J = zeros(dim, dim); size_t sigmaIx = factorIx_sigmaIx.second; - J(sigmaIx,sigmaIx) = 1.0; + J(sigmaIx, sigmaIx) = 1.0; // Use factorIndex as the lambda's key. - if (debug) cout << "prior for factor " << factorIx << endl; + if (debug) + cout << "prior for factor " << factorIx << endl; dualGraph.push_back(JacobianFactor(factorIx, J, zero(dim))); } } @@ -218,7 +233,8 @@ GaussianFactorGraph QPSolver::buildDualGraph(const GaussianFactorGraph& graph, } /* ************************************************************************* */ -std::pair QPSolver::findWorstViolatedActiveIneq(const VectorValues& lambdas) const { +pair QPSolver::findWorstViolatedActiveIneq( + const VectorValues& lambdas) const { int worstFactorIx = -1, worstSigmaIx = -1; // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either // inactive or a good ineq constraint, so we don't care! @@ -226,9 +242,9 @@ std::pair QPSolver::findWorstViolatedActiveIneq(const VectorValues& la BOOST_FOREACH(size_t factorIx, constraintIndices_) { Vector lambda = lambdas.at(factorIx); Vector orgSigmas = toJacobian(graph_.at(factorIx))->get_model()->sigmas(); - for (size_t j = 0; j maxLambda) { + if (orgSigmas[j] < 0 && lambda[j] > maxLambda) { worstFactorIx = factorIx; worstSigmaIx = j; maxLambda = lambda[j]; @@ -237,9 +253,9 @@ std::pair QPSolver::findWorstViolatedActiveIneq(const VectorValues& la return make_pair(worstFactorIx, worstSigmaIx); } -/* ************************************************************************* */ -bool QPSolver::updateWorkingSetInplace(GaussianFactorGraph& workingGraph, - int factorIx, int sigmaIx, double newSigma) const { +/* ************************************************************************* */bool QPSolver::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(); @@ -264,8 +280,9 @@ bool QPSolver::updateWorkingSetInplace(GaussianFactorGraph& workingGraph, * * We want the minimum of all those alphas among all inactive ineq. */ -boost::tuple QPSolver::computeStepSize(const GaussianFactorGraph& workingGraph, - const VectorValues& xk, const VectorValues& p) const { +boost::tuple QPSolver::computeStepSize( + const GaussianFactorGraph& workingGraph, const VectorValues& xk, + const VectorValues& p) const { static bool debug = false; double minAlpha = 1.0; @@ -274,33 +291,39 @@ boost::tuple QPSolver::computeStepSize(const GaussianFactorGra JacobianFactor::shared_ptr jacobian = toJacobian(workingGraph.at(factorIx)); Vector sigmas = jacobian->get_model()->sigmas(); Vector b = jacobian->getb(); - for (size_t s = 0; sbegin(); xj != jacobian->end(); ++xj) { + 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, b[s]: " << s << " " << ajTp << " " << b[s] << 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<=0) continue; + if (ajTp <= 0) + continue; // Compute aj'*xk double ajTx = 0.0; - for (Factor::const_iterator xj = jacobian->begin(); xj != jacobian->end(); ++xj) { + 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; + if (debug) + cout << "b[s], ajTx: " << b[s] << " " << ajTx << " " << ajTp << endl; // alpha = (bj - aj'*xk) / (aj'*p) - double alpha = (b[s] - ajTx)/ajTp; - if (debug) cout << "alpha: " << alpha << endl; + double alpha = (b[s] - ajTx) / ajTp; + if (debug) + cout << "alpha: " << alpha << endl; // We want the minimum of all those max alphas if (alpha < minAlpha) { @@ -314,41 +337,52 @@ boost::tuple QPSolver::computeStepSize(const GaussianFactorGra return boost::make_tuple(minAlpha, closestFactorIx, closestSigmaIx); } -/* ************************************************************************* */ -bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& currentSolution, VectorValues& lambdas) const { +/* ************************************************************************* */bool QPSolver::iterateInPlace( + GaussianFactorGraph& workingGraph, VectorValues& currentSolution, + VectorValues& lambdas) const { static bool debug = false; - if (debug) workingGraph.print("workingGraph: "); + if (debug) + workingGraph.print("workingGraph: "); // Obtain the solution from the current working graph VectorValues newSolution = workingGraph.optimize(); - if (debug) newSolution.print("New solution:"); + if (debug) + newSolution.print("New solution:"); // If we CAN'T move further if (newSolution.equals(currentSolution, 1e-5)) { // Compute lambda from the dual graph - if (debug) cout << "Building dual graph..." << endl; + if (debug) + cout << "Building dual graph..." << endl; GaussianFactorGraph dualGraph = buildDualGraph(workingGraph, newSolution); - if (debug) dualGraph.print("Dual graph: "); + if (debug) + dualGraph.print("Dual graph: "); lambdas = dualGraph.optimize(); - if (debug) lambdas.print("lambdas :"); + if (debug) + lambdas.print("lambdas :"); int factorIx, sigmaIx; boost::tie(factorIx, sigmaIx) = findWorstViolatedActiveIneq(lambdas); - if (debug) cout << "violated active ineq - factorIx, sigmaIx: " << factorIx << " " << sigmaIx << endl; + if (debug) + cout << "violated active ineq - factorIx, sigmaIx: " << factorIx << " " + << sigmaIx << endl; // 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 { + } else { // If we CAN make some progress // Adapt stepsize if some inactive inequality constraints complain about this move - if (debug) cout << "Computing stepsize..." << endl; + if (debug) + cout << "Computing stepsize..." << endl; 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; + 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! @@ -367,7 +401,8 @@ bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& c } /* ************************************************************************* */ -std::pair QPSolver::optimize(const VectorValues& initials) const { +pair QPSolver::optimize( + const VectorValues& initials) const { GaussianFactorGraph workingGraph = graph_.clone(); VectorValues currentSolution = initials; VectorValues lambdas; @@ -379,10 +414,11 @@ std::pair QPSolver::optimize(const VectorValues& ini } /* ************************************************************************* */ -std::pair QPSolver::initialValuesLP() const { +pair QPSolver::initialValuesLP() const { size_t firstSlackKey = 0; BOOST_FOREACH(Key key, fullFactorIndices_ | boost::adaptors::map_keys) { - if (firstSlackKey < key) firstSlackKey = key; + if (firstSlackKey < key) + firstSlackKey = key; } firstSlackKey += 1; @@ -406,9 +442,9 @@ std::pair QPSolver::initialValuesLP() const { Vector errorAtZero = jacobian->getb(); Vector slackInit = zero(errorAtZero.size()); Vector sigmas = jacobian->get_model()->sigmas(); - for (size_t i = 0; i0 sigma, i.e. normal Gaussian noise, initialize it at 0 @@ -439,18 +475,19 @@ VectorValues QPSolver::objectiveCoeffsLP(Key firstSlackKey) const { } /* ************************************************************************* */ -std::pair QPSolver::constraintsLP( +pair QPSolver::constraintsLP( Key firstSlackKey) const { // Create constraints and 0 lower bounds (zi>=0) GaussianFactorGraph::shared_ptr constraints(new GaussianFactorGraph()); VectorValues slackLowerBounds; - for (size_t key = firstSlackKey; key > terms; + vector > terms; for (Factor::iterator it = jacobian->begin(); it != jacobian->end(); ++it) { terms.push_back(make_pair(*it, jacobian->getA(it))); } @@ -459,7 +496,8 @@ std::pair QPSolver::constraintsLP // LE constraints ax <= b for sigma < 0. size_t dim = jacobian->rows(); terms.push_back(make_pair(key, -eye(dim))); - constraints->push_back(JacobianFactor(terms, jacobian->getb(), jacobian->get_model())); + constraints->push_back( + JacobianFactor(terms, jacobian->getb(), jacobian->get_model())); // Add lower bound for this slack key slackLowerBounds.insert(key, zero(dim)); } @@ -467,7 +505,7 @@ std::pair QPSolver::constraintsLP } /* ************************************************************************* */ -std::pair QPSolver::findFeasibleInitialValues() const { +pair QPSolver::findFeasibleInitialValues() const { static const bool debug = false; // Initial values with slack variables for the LP subproblem, Nocedal06book, pg.473 VectorValues initials; @@ -486,14 +524,19 @@ std::pair QPSolver::findFeasibleInitialValues() const { LPSolver lpSolver(objectiveLP, constraints, slackLowerBounds); VectorValues solution = lpSolver.solve(); - if (debug) initials.print("Initials LP: "); - if (debug) objectiveLP.print("Objective LP: "); - if (debug) constraints->print("Constraints LP: "); - if (debug) solution.print("LP solution: "); + if (debug) + initials.print("Initials LP: "); + if (debug) + objectiveLP.print("Objective LP: "); + if (debug) + constraints->print("Constraints LP: "); + if (debug) + solution.print("LP solution: "); // Remove slack variables from solution double slackSum = 0.0; - for (Key key = firstSlackKey; key < firstSlackKey+constraintIndices_.size(); ++key) { + for (Key key = firstSlackKey; key < firstSlackKey + constraintIndices_.size(); + ++key) { slackSum += solution.at(key).cwiseAbs().sum(); solution.erase(key); } @@ -501,22 +544,23 @@ std::pair QPSolver::findFeasibleInitialValues() const { // Insert zero vectors for free variables that are not in the constraints BOOST_FOREACH(Key key, fullFactorIndices_ | boost::adaptors::map_keys) { if (!solution.exists(key)) { - GaussianFactor::shared_ptr factor = graph_.at(*fullFactorIndices_[key].begin()); + GaussianFactor::shared_ptr factor = graph_.at( + *fullFactorIndices_[key].begin()); size_t dim = factor->getDim(factor->find(key)); solution.insert(key, zero(dim)); } } - return make_pair(slackSum<1e-5, solution); + return make_pair(slackSum < 1e-5, solution); } /* ************************************************************************* */ -std::pair QPSolver::optimize() const { +pair QPSolver::optimize() const { bool isFeasible; VectorValues initials; boost::tie(isFeasible, initials) = findFeasibleInitialValues(); if (!isFeasible) { - throw std::runtime_error("LP subproblem is infeasible!"); + throw runtime_error("LP subproblem is infeasible!"); } return optimize(initials); } diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index 00ec8df72..049dd0a28 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -10,6 +10,9 @@ #include #include +#include +#include + namespace gtsam { /** @@ -20,12 +23,12 @@ namespace gtsam { * and a positive sigma denotes a normal Gaussian noise model. */ class QPSolver { - const GaussianFactorGraph& graph_; //!< the original graph, can't be modified! + const GaussianFactorGraph& graph_; //!< the original graph, can't be modified! FastVector constraintIndices_; //!< Indices of constrained factors in the original graph 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. + // 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. @@ -35,8 +38,9 @@ public: QPSolver(const GaussianFactorGraph& graph); /// Return indices of all constrained factors - FastVector constraintIndices() const { return constraintIndices_; } - + FastVector constraintIndices() const { + return constraintIndices_; + } /// Return the Hessian factor graph of constrained variables GaussianFactorGraph::shared_ptr freeHessiansOfConstrainedVars() const { @@ -73,37 +77,37 @@ public: GaussianFactorGraph buildDualGraph(const GaussianFactorGraph& graph, const VectorValues& x0, bool useLeastSquare = false) const; - /** - * Find the BAD active ineq that pulls x strongest to the wrong direction of its constraint - * (i.e. it is pulling towards >0, while its feasible region is <=0) - * - * For active ineq constraints (those that are enforced as eq constraints now - * in the working set), we want lambda < 0. - * This is because: - * - From the Lagrangian L = f - lambda*c, we know that the constraint force is - * (lambda * \grad c) = \grad f, because it cancels out the unconstrained - * unconstrained force (-\grad f), which is pulling x in the opposite direction - * of \grad f towards the unconstrained minimum point - * - We also know that at the constraint surface \grad c points toward + (>= 0), - * while we are solving for - (<=0) constraint - * - So, we want the constraint force (lambda * \grad c) to to pull x - * towards the opposite direction of \grad c, i.e. towards the area - * where the ineq constraint <=0 is satisfied. - * - Hence, we want lambda < 0 - * - * So active ineqs with lambda > 0 are BAD. And we want the worst one with the largest lambda. - * - */ - std::pair findWorstViolatedActiveIneq(const VectorValues& lambdas) const; + * Find the BAD active ineq that pulls x strongest to the wrong direction of its constraint + * (i.e. it is pulling towards >0, while its feasible region is <=0) + * + * For active ineq constraints (those that are enforced as eq constraints now + * in the working set), we want lambda < 0. + * This is because: + * - From the Lagrangian L = f - lambda*c, we know that the constraint force is + * (lambda * \grad c) = \grad f, because it cancels out the unconstrained + * unconstrained force (-\grad f), which is pulling x in the opposite direction + * of \grad f towards the unconstrained minimum point + * - We also know that at the constraint surface \grad c points toward + (>= 0), + * while we are solving for - (<=0) constraint + * - So, we want the constraint force (lambda * \grad c) to to pull x + * towards the opposite direction of \grad c, i.e. towards the area + * where the ineq constraint <=0 is satisfied. + * - Hence, we want lambda < 0 + * + * So active ineqs with lambda > 0 are BAD. And we want the worst one with the largest lambda. + * + */ + std::pair findWorstViolatedActiveIneq( + const VectorValues& lambdas) const; /** * 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; + bool updateWorkingSetInplace(GaussianFactorGraph& workingGraph, int factorIx, + int sigmaIx, double newSigma) const; /** * Compute step size alpha for the new solution x' = xk + alpha*p, where alpha \in [0,1] @@ -113,12 +117,13 @@ public: * This constraint will be added to the working set and become active * in the next iteration */ - boost::tuple computeStepSize(const GaussianFactorGraph& workingGraph, - const VectorValues& xk, const VectorValues& p) const; + boost::tuple computeStepSize( + const GaussianFactorGraph& workingGraph, const VectorValues& xk, + const VectorValues& p) const; /** Iterate 1 step, modify workingGraph and currentSolution *IN PLACE* !!! */ - bool iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& currentSolution, - VectorValues& lambdas) const; + bool iterateInPlace(GaussianFactorGraph& workingGraph, + VectorValues& currentSolution, VectorValues& lambdas) const; /** Optimize with a provided initial values * For this version, it is the responsibility of the caller to provide @@ -127,7 +132,8 @@ public: * of optimize(). * @return a pair of solutions */ - std::pair optimize(const VectorValues& initials) const; + std::pair optimize( + const VectorValues& initials) const; /** Optimize without an initial value. * This version of optimize will try to find a feasible initial value by solving @@ -137,7 +143,6 @@ public: */ std::pair optimize() const; - /** * Create initial values for the LP subproblem * @return initial values and the key for the first slack variable @@ -148,14 +153,16 @@ public: VectorValues objectiveCoeffsLP(Key firstSlackKey) const; /// Build constraints and slacks' lower bounds for the LP subproblem - std::pair constraintsLP(Key firstSlackKey) const; + std::pair constraintsLP( + Key firstSlackKey) const; /// Find a feasible initial point std::pair findFeasibleInitialValues() const; /// Convert a Gaussian factor to a jacobian. return empty shared ptr if failed /// TODO: Move to GaussianFactor? - static JacobianFactor::shared_ptr toJacobian(const GaussianFactor::shared_ptr& factor) { + static JacobianFactor::shared_ptr toJacobian( + const GaussianFactor::shared_ptr& factor) { JacobianFactor::shared_ptr jacobian( boost::dynamic_pointer_cast(factor)); return jacobian; @@ -163,17 +170,19 @@ public: /// Convert a Gaussian factor to a Hessian. Return empty shared ptr if failed /// TODO: Move to GaussianFactor? - static HessianFactor::shared_ptr toHessian(const GaussianFactor::shared_ptr factor) { - HessianFactor::shared_ptr hessian(boost::dynamic_pointer_cast(factor)); + static HessianFactor::shared_ptr toHessian( + const GaussianFactor::shared_ptr factor) { + HessianFactor::shared_ptr hessian( + boost::dynamic_pointer_cast(factor)); return hessian; } private: /// Collect all free Hessians involving constrained variables into a graph GaussianFactorGraph::shared_ptr unconstrainedHessiansOfConstrainedVars( - const GaussianFactorGraph& graph, const std::set& constrainedVars) const; + const GaussianFactorGraph& graph, + const std::set& constrainedVars) const; }; - } /* namespace gtsam */ diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 3f357d1d6..550ec88fc 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -16,11 +16,12 @@ * @author Duy-Nguyen Ta */ -#include #include #include #include +#include + using namespace std; using namespace gtsam; using namespace gtsam::symbol_shorthand; @@ -38,18 +39,18 @@ GaussianFactorGraph createTestCase() { // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // 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), 10.0)); + HessianFactor(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), 3.0 * ones(1), + 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); + 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); // Special constrained noise model denoting <= inequalities with negative sigmas 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)); return graph; @@ -63,20 +64,22 @@ TEST(QPSolver, constraintsAux) { LONGS_EQUAL(1, constraintIx[0]); 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; boost::tie(factorIx, lambdaIx) = solver.findWorstViolatedActiveIneq(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)); + lambdas2.insert(constraintIx[0], (Vector(4) << -0.5, 0.0, -0.3, -0.1)); int factorIx2, lambdaIx2; - boost::tie(factorIx2, lambdaIx2) = solver.findWorstViolatedActiveIneq(lambdas2); + boost::tie(factorIx2, lambdaIx2) = solver.findWorstViolatedActiveIneq( + lambdas2); LONGS_EQUAL(-1, factorIx2); LONGS_EQUAL(-1, lambdaIx2); - GaussianFactorGraph::shared_ptr freeHessian = solver.freeHessiansOfConstrainedVars(); + GaussianFactorGraph::shared_ptr freeHessian = + solver.freeHessiansOfConstrainedVars(); GaussianFactorGraph expectedFreeHessian; expectedFreeHessian.push_back( HessianFactor(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), 3.0 * ones(1), @@ -94,17 +97,17 @@ GaussianFactorGraph createEqualityConstrainedTest() { // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0 graph.push_back( - HessianFactor(X(1), X(2), 2.0*ones(1, 1), zeros(1, 1), zero(1), - 2.0*ones(1, 1), zero(1), 0.0)); + HessianFactor(X(1), X(2), 2.0 * ones(1, 1), zeros(1, 1), zero(1), + 2.0 * ones(1, 1), zero(1), 0.0)); // Equality constraints // x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector - Matrix A1 = (Matrix(1, 1)<<1); - Matrix A2 = (Matrix(1, 1)<<1); - Vector b = -(Vector(1)<<1); + Matrix A1 = (Matrix(1, 1) << 1); + Matrix A2 = (Matrix(1, 1) << 1); + Vector b = -(Vector(1) << 1); // Special constrained noise model denoting <= inequalities with negative sigmas noiseModel::Constrained::shared_ptr noise = - 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)); return graph; @@ -123,7 +126,7 @@ TEST(QPSolver, dual) { GaussianFactorGraph dualGraph = solver.buildDualGraph(graph, initials); VectorValues dual = dualGraph.optimize(); VectorValues expectedDual; - expectedDual.insert(1, (Vector(1)<<2.0)); + expectedDual.insert(1, (Vector(1) << 2.0)); CHECK(assert_equal(expectedDual, dual, 1e-10)); } @@ -140,8 +143,8 @@ TEST(QPSolver, iterate) { currentSolution.insert(X(2), zero(1)); std::vector expectedSolutions(3); - expectedSolutions[0].insert(X(1), (Vector(1) << 4.0/3.0)); - expectedSolutions[0].insert(X(2), (Vector(1) << 2.0/3.0)); + expectedSolutions[0].insert(X(1), (Vector(1) << 4.0 / 3.0)); + expectedSolutions[0].insert(X(2), (Vector(1) << 2.0 / 3.0)); expectedSolutions[1].insert(X(1), (Vector(1) << 1.5)); expectedSolutions[1].insert(X(2), (Vector(1) << 0.5)); expectedSolutions[2].insert(X(1), (Vector(1) << 1.5)); @@ -168,8 +171,8 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) { VectorValues solution; boost::tie(solution, boost::tuples::ignore) = solver.optimize(initials); VectorValues expectedSolution; - expectedSolution.insert(X(1), (Vector(1)<< 1.5)); - expectedSolution.insert(X(2), (Vector(1)<< 0.5)); + expectedSolution.insert(X(1), (Vector(1) << 1.5)); + expectedSolution.insert(X(2), (Vector(1) << 0.5)); CHECK(assert_equal(expectedSolution, solution, 1e-100)); } @@ -183,18 +186,18 @@ GaussianFactorGraph createTestMatlabQPEx() { // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0 graph.push_back( - HessianFactor(X(1), X(2), 1.0*ones(1, 1), -ones(1, 1), 2.0*ones(1), - 2.0*ones(1, 1), 6*ones(1), 1000.0)); + HessianFactor(X(1), X(2), 1.0 * ones(1, 1), -ones(1, 1), 2.0 * ones(1), + 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); + 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); // Special constrained noise model denoting <= inequalities with negative sigmas noiseModel::Constrained::shared_ptr noise = - noiseModel::Constrained::MixedSigmas((Vector(5)<<-1, -1, -1, -1, -1)); + noiseModel::Constrained::MixedSigmas((Vector(5) << -1, -1, -1, -1, -1)); graph.push_back(JacobianFactor(X(1), A1, X(2), A2, b, noise)); return graph; @@ -209,8 +212,8 @@ TEST(QPSolver, optimizeMatlabEx) { VectorValues solution; boost::tie(solution, boost::tuples::ignore) = solver.optimize(initials); VectorValues expectedSolution; - expectedSolution.insert(X(1), (Vector(1)<< 2.0/3.0)); - expectedSolution.insert(X(2), (Vector(1)<< 4.0/3.0)); + expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0)); + expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0)); CHECK(assert_equal(expectedSolution, solution, 1e-7)); } @@ -219,17 +222,23 @@ TEST(QPSolver, optimizeMatlabEx) { GaussianFactorGraph createTestNocedal06bookEx16_4() { GaussianFactorGraph graph; - graph.push_back(JacobianFactor(X(1), ones(1,1), ones(1))); - graph.push_back(JacobianFactor(X(2), ones(1,1), 2.5*ones(1))); + graph.push_back(JacobianFactor(X(1), ones(1, 1), ones(1))); + graph.push_back(JacobianFactor(X(2), ones(1, 1), 2.5 * ones(1))); // Inequality constraints - noiseModel::Constrained::shared_ptr noise = noiseModel::Constrained::MixedSigmas( - (Vector(1) << -1)); - graph.push_back(JacobianFactor(X(1), -ones(1,1), X(2), 2*ones(1,1), 2*ones(1), noise)); - graph.push_back(JacobianFactor(X(1), ones(1,1), X(2), 2*ones(1,1), 6*ones(1), noise)); - graph.push_back(JacobianFactor(X(1), ones(1,1), X(2),-2*ones(1,1), 2*ones(1), noise)); - graph.push_back(JacobianFactor(X(1), -ones(1,1), zero(1), noise)); - graph.push_back(JacobianFactor(X(2), -ones(1,1), zero(1), noise)); + noiseModel::Constrained::shared_ptr noise = + noiseModel::Constrained::MixedSigmas((Vector(1) << -1)); + graph.push_back( + JacobianFactor(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), 2 * ones(1), + noise)); + graph.push_back( + JacobianFactor(X(1), ones(1, 1), X(2), 2 * ones(1, 1), 6 * ones(1), + noise)); + graph.push_back( + JacobianFactor(X(1), ones(1, 1), X(2), -2 * ones(1, 1), 2 * ones(1), + noise)); + graph.push_back(JacobianFactor(X(1), -ones(1, 1), zero(1), noise)); + graph.push_back(JacobianFactor(X(2), -ones(1, 1), zero(1), noise)); return graph; } @@ -238,54 +247,60 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) { GaussianFactorGraph graph = createTestNocedal06bookEx16_4(); QPSolver solver(graph); VectorValues initials; - initials.insert(X(1), (Vector(1)<<2.0)); + initials.insert(X(1), (Vector(1) << 2.0)); initials.insert(X(2), zero(1)); VectorValues solution; boost::tie(solution, boost::tuples::ignore) = solver.optimize(initials); VectorValues expectedSolution; - expectedSolution.insert(X(1), (Vector(1)<< 1.4)); - expectedSolution.insert(X(2), (Vector(1)<< 1.7)); + expectedSolution.insert(X(1), (Vector(1) << 1.4)); + expectedSolution.insert(X(2), (Vector(1) << 1.7)); CHECK(assert_equal(expectedSolution, solution, 1e-7)); } /* ************************************************************************* */ /* Create test graph as in Nocedal06book, Ex 16.4, pg. 475 with the first constraint (16.49b) is replaced by - x1 - 2 x2 - 1 >=0 -so that the trivial initial point (0,0) is infeasible -==== + x1 - 2 x2 - 1 >=0 + so that the trivial initial point (0,0) is infeasible + ==== H = [2 0; 0 2]; -f = [-2; -5]; + f = [-2; -5]; A =[-1 2; - 1 2 - 1 -2]; -b = [-1; 6; 2]; -lb = zeros(2,1); + 1 2 + 1 -2]; + b = [-1; 6; 2]; + lb = zeros(2,1); -opts = optimoptions('quadprog','Algorithm','active-set','Display','off'); + opts = optimoptions('quadprog','Algorithm','active-set','Display','off'); -[x,fval,exitflag,output,lambda] = ... - quadprog(H,f,A,b,[],[],lb,[],[],opts); -==== -x = - 2.0000 - 0.5000 -*/ + [x,fval,exitflag,output,lambda] = ... + quadprog(H,f,A,b,[],[],lb,[],[],opts); + ==== + x = + 2.0000 + 0.5000 + */ GaussianFactorGraph modifyNocedal06bookEx16_4() { GaussianFactorGraph graph; - graph.push_back(JacobianFactor(X(1), ones(1,1), ones(1))); - graph.push_back(JacobianFactor(X(2), ones(1,1), 2.5*ones(1))); + graph.push_back(JacobianFactor(X(1), ones(1, 1), ones(1))); + graph.push_back(JacobianFactor(X(2), ones(1, 1), 2.5 * ones(1))); // Inequality constraints - noiseModel::Constrained::shared_ptr noise = noiseModel::Constrained::MixedSigmas( - (Vector(1) << -1)); - graph.push_back(JacobianFactor(X(1), -ones(1,1), X(2), 2*ones(1,1), -1*ones(1), noise)); - graph.push_back(JacobianFactor(X(1), ones(1,1), X(2), 2*ones(1,1), 6*ones(1), noise)); - graph.push_back(JacobianFactor(X(1), ones(1,1), X(2),-2*ones(1,1), 2*ones(1), noise)); - graph.push_back(JacobianFactor(X(1), -ones(1,1), zero(1), noise)); - graph.push_back(JacobianFactor(X(2), -ones(1,1), zero(1), noise)); + noiseModel::Constrained::shared_ptr noise = + noiseModel::Constrained::MixedSigmas((Vector(1) << -1)); + graph.push_back( + JacobianFactor(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), -1 * ones(1), + noise)); + graph.push_back( + JacobianFactor(X(1), ones(1, 1), X(2), 2 * ones(1, 1), 6 * ones(1), + noise)); + graph.push_back( + JacobianFactor(X(1), ones(1, 1), X(2), -2 * ones(1, 1), 2 * ones(1), + noise)); + graph.push_back(JacobianFactor(X(1), -ones(1, 1), zero(1), noise)); + graph.push_back(JacobianFactor(X(2), -ones(1, 1), zero(1), noise)); return graph; } @@ -306,23 +321,31 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4_findInitialPoint) { EXPECT(assert_equal(zero(1), initialsLP.at(firstSlackKey+4))); VectorValues objCoeffs = solver.objectiveCoeffsLP(firstSlackKey); - for (size_t i = 0; i<5; ++i) + for (size_t i = 0; i < 5; ++i) EXPECT(assert_equal(ones(1), objCoeffs.at(firstSlackKey+i))); GaussianFactorGraph::shared_ptr constraints; VectorValues lowerBounds; boost::tie(constraints, lowerBounds) = solver.constraintsLP(firstSlackKey); - for (size_t i = 0; i<5; ++i) + for (size_t i = 0; i < 5; ++i) EXPECT(assert_equal(zero(1), lowerBounds.at(firstSlackKey+i))); GaussianFactorGraph expectedConstraints; - noiseModel::Constrained::shared_ptr noise = noiseModel::Constrained::MixedSigmas( - (Vector(1) << -1)); - expectedConstraints.push_back(JacobianFactor(X(1), -ones(1,1), X(2), 2*ones(1,1), X(3), -ones(1,1),-1*ones(1), noise)); - expectedConstraints.push_back(JacobianFactor(X(1), ones(1,1), X(2), 2*ones(1,1), X(4), -ones(1,1), 6*ones(1), noise)); - expectedConstraints.push_back(JacobianFactor(X(1), ones(1,1), X(2),-2*ones(1,1), X(5), -ones(1,1), 2*ones(1), noise)); - expectedConstraints.push_back(JacobianFactor(X(1), -ones(1,1), X(6), -ones(1,1), zero(1), noise)); - expectedConstraints.push_back(JacobianFactor(X(2), -ones(1,1), X(7), -ones(1,1), zero(1), noise)); + noiseModel::Constrained::shared_ptr noise = + noiseModel::Constrained::MixedSigmas((Vector(1) << -1)); + expectedConstraints.push_back( + JacobianFactor(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), X(3), -ones(1, 1), + -1 * ones(1), noise)); + expectedConstraints.push_back( + JacobianFactor(X(1), ones(1, 1), X(2), 2 * ones(1, 1), X(4), -ones(1, 1), + 6 * ones(1), noise)); + expectedConstraints.push_back( + JacobianFactor(X(1), ones(1, 1), X(2), -2 * ones(1, 1), X(5), -ones(1, 1), + 2 * ones(1), noise)); + expectedConstraints.push_back( + JacobianFactor(X(1), -ones(1, 1), X(6), -ones(1, 1), zero(1), noise)); + expectedConstraints.push_back( + JacobianFactor(X(2), -ones(1, 1), X(7), -ones(1, 1), zero(1), noise)); EXPECT(assert_equal(expectedConstraints, *constraints)); bool isFeasible; @@ -341,12 +364,12 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4_2) { GaussianFactorGraph graph = createTestNocedal06bookEx16_4(); QPSolver solver(graph); VectorValues initials; - initials.insert(X(1), (Vector(1)<<0.0)); - initials.insert(X(2), (Vector(1)<<100.0)); + initials.insert(X(1), (Vector(1) << 0.0)); + initials.insert(X(2), (Vector(1) << 100.0)); VectorValues expectedSolution; - expectedSolution.insert(X(1), (Vector(1)<< 1.4)); - expectedSolution.insert(X(2), (Vector(1)<< 1.7)); + expectedSolution.insert(X(1), (Vector(1) << 1.4)); + expectedSolution.insert(X(2), (Vector(1) << 1.7)); VectorValues solution; boost::tie(solution, boost::tuples::ignore) = solver.optimize(initials); @@ -363,12 +386,13 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4_2) { TEST(QPSolver, failedSubproblem) { GaussianFactorGraph graph; graph.push_back(JacobianFactor(X(1), eye(2), zero(2))); - graph.push_back(HessianFactor(X(1), zeros(2,2), zero(2), 100.0)); - graph.push_back(JacobianFactor(X(1), (Matrix(1,2)<<-1.0, 0.0), -ones(1), - noiseModel::Constrained::MixedSigmas(-ones(1)))); + graph.push_back(HessianFactor(X(1), zeros(2, 2), zero(2), 100.0)); + graph.push_back( + JacobianFactor(X(1), (Matrix(1, 2) << -1.0, 0.0), -ones(1), + noiseModel::Constrained::MixedSigmas(-ones(1)))); VectorValues expected; - expected.insert(X(1), (Vector(2)<< 1.0, 0.0)); + expected.insert(X(1), (Vector(2) << 1.0, 0.0)); QPSolver solver(graph); VectorValues solution;