diff --git a/gtsam_unstable/linear/LPInitSolverMatlab.h b/gtsam_unstable/linear/LPInitSolverMatlab.h index e9b792a35..3e15f355f 100644 --- a/gtsam_unstable/linear/LPInitSolverMatlab.h +++ b/gtsam_unstable/linear/LPInitSolverMatlab.h @@ -5,12 +5,11 @@ * @date 1/24/16 */ - #pragma once #include #include - +#include namespace gtsam { /** @@ -39,13 +38,16 @@ namespace gtsam { * inequality constraint, we can't conclude that the problem is infeasible. * However, whether it is infeasible or unbounded, we don't have a unique solution anyway. */ -class LPInitSolverMatlab : public LPInitSolver { +class LPInitSolverMatlab: public LPInitSolver { typedef LPInitSolver Base; public: - LPInitSolverMatlab(const LPSolver& lpSolver) : Base(lpSolver) {} - virtual ~LPInitSolverMatlab() {} + LPInitSolverMatlab(const LPSolver& lpSolver) : + Base(lpSolver) { + } + virtual ~LPInitSolverMatlab() { + } - virtual VectorValues solve() const { + virtual VectorValues solve() const { // Build the graph to solve for the initial value of the initial problem GaussianFactorGraph::shared_ptr initOfInitGraph = buildInitOfInitGraph(); VectorValues x0 = initOfInitGraph->optimize(); @@ -62,7 +64,7 @@ public: VectorValues xyInit = lpSolveInit.optimize(xy0).first; double yOpt = xyInit.at(yKey)[0]; xyInit.erase(yKey); - if ( yOpt > 0) + if (yOpt > 0) throw InfeasibleOrUnboundedProblem(); else return xyInit; @@ -72,9 +74,10 @@ private: /// build initial LP LP::shared_ptr buildInitialLP(Key yKey) const { LP::shared_ptr initLP(new LP()); - initLP->cost = LinearCost(yKey, ones(1)); // min y - initLP->equalities = lp_.equalities; // st. Ax = b - initLP->inequalities = addSlackVariableToInequalities(yKey, lp_.inequalities); // Cx-y <= d + initLP->cost = LinearCost(yKey, ones(1)); // min y + initLP->equalities = lp_.equalities; // st. Ax = b + initLP->inequalities = addSlackVariableToInequalities(yKey, + lp_.inequalities); // Cx-y <= d return initLP; } @@ -83,7 +86,7 @@ private: Key maxK = 0; BOOST_FOREACH(Key key, keysDim | boost::adaptors::map_keys) if (maxK < key) - maxK = key; + maxK = key; return maxK; } @@ -93,7 +96,8 @@ private: */ GaussianFactorGraph::shared_ptr buildInitOfInitGraph() const { // first add equality constraints Ax = b - GaussianFactorGraph::shared_ptr initGraph(new GaussianFactorGraph(lp_.equalities)); + GaussianFactorGraph::shared_ptr initGraph( + new GaussianFactorGraph(lp_.equalities)); // create factor ||x||^2 and add to the graph const KeyDimMap& keysDim = lpSolver_.keysDim(); @@ -110,15 +114,15 @@ private: BOOST_FOREACH(const LinearInequality::shared_ptr& factor, lp_.inequalities) { double error = factor->error(x0); if (error > y0) - y0 = error; + y0 = error; } return y0; } - /// Collect all terms of a factor into a container. - std::vector > collectTerms(const LinearInequality& factor) const { - std::vector > terms; + std::vector > collectTerms( + const LinearInequality& factor) const { + std::vector < std::pair > terms; for (Factor::const_iterator it = factor.begin(); it != factor.end(); it++) { terms.push_back(make_pair(*it, factor.getA(it))); } diff --git a/gtsam_unstable/linear/LPSolver.cpp b/gtsam_unstable/linear/LPSolver.cpp index 5b2cc6bd0..e7d7ac08d 100644 --- a/gtsam_unstable/linear/LPSolver.cpp +++ b/gtsam_unstable/linear/LPSolver.cpp @@ -9,9 +9,11 @@ #include #include #include +#include namespace gtsam { -LPSolver::LPSolver(const LP &lp) : lp_(lp) { +LPSolver::LPSolver(const LP &lp) : + lp_(lp) { // Push back factors that are the same in every iteration to the base graph. // Those include the equality constraints and zero priors for keys that are // not in the cost @@ -37,7 +39,7 @@ LPSolver::LPSolver(const LP &lp) : lp_(lp) { GaussianFactorGraph::shared_ptr LPSolver::createZeroPriors( const KeyVector &costKeys, const KeyDimMap &keysDim) const { GaussianFactorGraph::shared_ptr graph(new GaussianFactorGraph()); - for (Key key: keysDim | boost::adaptors::map_keys) { + for (Key key : keysDim | boost::adaptors::map_keys) { if (find(costKeys.begin(), costKeys.end(), key) == costKeys.end()) { size_t dim = keysDim.at(key); graph->push_back(JacobianFactor(key, eye(dim), zero(dim))); @@ -50,8 +52,8 @@ LPState LPSolver::iterate(const LPState &state) const { // Solve with the current working set // LP: project the objective neg. gradient to the constraint's null space // to find the direction to move - VectorValues newValues = - solveWithCurrentWorkingSet(state.values, state.workingSet); + VectorValues newValues = solveWithCurrentWorkingSet(state.values, + state.workingSet); // If we CAN'T move further // LP: projection on the constraints' nullspace is zero: we are at a vertex @@ -61,8 +63,8 @@ LPState LPSolver::iterate(const LPState &state) const { // LP: project the objective's gradient onto each constraint gradient to // obtain the dual scaling factors // is it true?? - GaussianFactorGraph::shared_ptr dualGraph = - buildDualGraph(state.workingSet, newValues); + GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, + newValues); VectorValues duals = dualGraph->optimize(); // LP: see which inequality constraint has wrong pulling direction, i.e., dual < 0 int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); @@ -71,13 +73,15 @@ LPState LPSolver::iterate(const LPState &state) const { // TODO If we still have infeasible equality constraints: the problem is // over-constrained. No solution! // ... - return LPState(newValues, duals, state.workingSet, true, state.iterations + 1); + return LPState(newValues, duals, state.workingSet, true, + state.iterations + 1); } else { // Inactivate the leaving constraint // LP: remove the bad ineq constraint out of the working set InequalityFactorGraph newWorkingSet = state.workingSet; newWorkingSet.at(leavingFactor)->inactivate(); - return LPState(newValues, duals, newWorkingSet, false, state.iterations + 1); + return LPState(newValues, duals, newWorkingSet, false, + state.iterations + 1); } } else { // If we CAN make some progress, i.e. p_k != 0 @@ -88,14 +92,16 @@ LPState LPSolver::iterate(const LPState &state) const { double alpha; int factorIx; VectorValues p = newValues - state.values; - boost::tie(alpha, factorIx) = // using 16.41 + boost::tie(alpha, factorIx) = // using 16.41 computeStepSize(state.workingSet, state.values, p); // also add to the working set the one that complains the most InequalityFactorGraph newWorkingSet = state.workingSet; - if (factorIx >= 0) newWorkingSet.at(factorIx)->activate(); + if (factorIx >= 0) + newWorkingSet.at(factorIx)->activate(); // step! newValues = state.values + alpha * p; - return LPState(newValues, state.duals, newWorkingSet, false, state.iterations + 1); + return LPState(newValues, state.duals, newWorkingSet, false, + state.iterations + 1); } } @@ -113,36 +119,36 @@ GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors( return graph; } -VectorValues LPSolver::solveWithCurrentWorkingSet( - const VectorValues &xk, const InequalityFactorGraph &workingSet) const { - GaussianFactorGraph workingGraph = baseGraph_; // || X - Xk + g ||^2 +VectorValues LPSolver::solveWithCurrentWorkingSet(const VectorValues &xk, + const InequalityFactorGraph &workingSet) const { + GaussianFactorGraph workingGraph = baseGraph_; // || X - Xk + g ||^2 workingGraph.push_back(*createLeastSquareFactors(lp_.cost, xk)); - for (const LinearInequality::shared_ptr &factor: workingSet) { - if (factor->active()) workingGraph.push_back(factor); + for (const LinearInequality::shared_ptr &factor : workingSet) { + if (factor->active()) + workingGraph.push_back(factor); } return workingGraph.optimize(); } -boost::shared_ptr LPSolver::createDualFactor( - Key key, const InequalityFactorGraph &workingSet, - const VectorValues &delta) const { +boost::shared_ptr LPSolver::createDualFactor(Key key, + const InequalityFactorGraph &workingSet, const VectorValues &delta) const { // Transpose the A matrix of constrained factors to have the jacobian of the // dual key - TermsContainer Aterms = collectDualJacobians( - key, lp_.equalities, equalityVariableIndex_); - TermsContainer AtermsInequalities = collectDualJacobians( - key, workingSet, inequalityVariableIndex_); + TermsContainer Aterms = collectDualJacobians < LinearEquality + > (key, lp_.equalities, equalityVariableIndex_); + TermsContainer AtermsInequalities = collectDualJacobians < LinearInequality + > (key, workingSet, inequalityVariableIndex_); Aterms.insert(Aterms.end(), AtermsInequalities.begin(), - AtermsInequalities.end()); + AtermsInequalities.end()); // Collect the gradients of unconstrained cost factors to the b vector if (Aterms.size() > 0) { Vector b = zero(delta.at(key).size()); Factor::const_iterator it = lp_.cost.find(key); - if (it != lp_.cost.end()) b = lp_.cost.getA(it).transpose(); - return boost::make_shared( - Aterms, b); // compute the least-square approximation of dual variables + if (it != lp_.cost.end()) + b = lp_.cost.getA(it).transpose(); + return boost::make_shared < JacobianFactor > (Aterms, b); // compute the least-square approximation of dual variables } else { return boost::make_shared(); } @@ -152,13 +158,14 @@ InequalityFactorGraph LPSolver::identifyActiveConstraints( const InequalityFactorGraph &inequalities, const VectorValues &initialValues, const VectorValues &duals) const { InequalityFactorGraph workingSet; - for (const LinearInequality::shared_ptr &factor : inequalities) { + for (const LinearInequality::shared_ptr &factor : inequalities) { LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); double error = workingFactor->error(initialValues); // TODO: find a feasible initial point for LPSolver. // For now, we just throw an exception - if (error > 0) throw InfeasibleInitialValues(); + if (error > 0) + throw InfeasibleInitialValues(); if (fabs(error) < 1e-7) { workingFactor->activate(); @@ -174,8 +181,8 @@ std::pair LPSolver::optimize( const VectorValues &initialValues, const VectorValues &duals) const { { // Initialize workingSet from the feasible initialValues - InequalityFactorGraph workingSet = - identifyActiveConstraints(lp_.inequalities, initialValues, duals); + InequalityFactorGraph workingSet = identifyActiveConstraints( + lp_.inequalities, initialValues, duals); LPState state(initialValues, duals, workingSet, false, 0); /// main loop of the solver @@ -189,7 +196,14 @@ std::pair LPSolver::optimize( boost::tuples::tuple LPSolver::computeStepSize( const InequalityFactorGraph &workingSet, const VectorValues &xk, const VectorValues &p) const { - return ActiveSetSolver::computeStepSize( - workingSet, xk, p, std::numeric_limits::infinity()); + return ActiveSetSolver::computeStepSize(workingSet, xk, p, + std::numeric_limits::infinity()); +} + +pair LPSolver::optimize() const { + LPInitSolverMatlab initSolver(*this); + VectorValues initValues = initSolver.solve(); + return optimize(initValues); } } + diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index 1ad781782..1d09af5ff 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -21,18 +21,18 @@ namespace gtsam { typedef std::map KeyDimMap; class LPSolver: public ActiveSetSolver { - const LP& lp_; //!< the linear programming problem + const LP &lp_; //!< the linear programming problem KeyDimMap keysDim_; //!< key-dim map of all variables in the constraints, used to create zero priors public: /// Constructor - LPSolver(const LP& lp); + LPSolver(const LP &lp); - const LP& lp() const { + const LP &lp() const { return lp_; } - const KeyDimMap& keysDim() const { + const KeyDimMap &keysDim() const { return keysDim_; } @@ -41,9 +41,9 @@ public: * mapping between every factor key and it's corresponding dimensionality. */ template - KeyDimMap collectKeysDim(const LinearGraph& linearGraph) const { + KeyDimMap collectKeysDim(const LinearGraph &linearGraph) const { KeyDimMap keysDim; - BOOST_FOREACH(const typename LinearGraph::sharedFactor& factor, linearGraph) { + BOOST_FOREACH(const typename LinearGraph::sharedFactor &factor, linearGraph) { if (!factor) continue; BOOST_FOREACH(Key key, factor->keys()) keysDim[key] = factor->getDim(factor->find(key)); @@ -52,8 +52,8 @@ public: } /// Create a zero prior for any keys in the graph that don't exist in the cost - GaussianFactorGraph::shared_ptr createZeroPriors(const KeyVector& costKeys, - const KeyDimMap& keysDim) const; + GaussianFactorGraph::shared_ptr createZeroPriors(const KeyVector &costKeys, + const KeyDimMap &keysDim) const; /* * This function performs an iteration of the Active Set Method for solving @@ -61,7 +61,7 @@ public: * to be unfeasible, solved or the current state changed to reflect a new * working set. */ - LPState iterate(const LPState& state) const; + LPState iterate(const LPState &state) const; /** * Create the factor ||x-xk - (-g)||^2 where xk is the current feasible solution @@ -76,11 +76,11 @@ public: * is the projection of the gradient onto the constraints' subspace */ GaussianFactorGraph::shared_ptr createLeastSquareFactors( - const LinearCost& cost, const VectorValues& xk) const; + const LinearCost &cost, const VectorValues &xk) const; /// Find solution with the current working set - VectorValues solveWithCurrentWorkingSet(const VectorValues& xk, - const InequalityFactorGraph& workingSet) const; + VectorValues solveWithCurrentWorkingSet(const VectorValues &xk, + const InequalityFactorGraph &workingSet) const; /* * A dual factor takes the objective function and a set of constraints. @@ -89,12 +89,12 @@ public: * function g are dual factors and lambda is the lagrangian multiplier. */ JacobianFactor::shared_ptr createDualFactor(Key key, - const InequalityFactorGraph& workingSet, const VectorValues& delta) const; + const InequalityFactorGraph &workingSet, const VectorValues &delta) const; /// TODO(comment) boost::tuple computeStepSize( - const InequalityFactorGraph& workingSet, const VectorValues& xk, - const VectorValues& p) const; + const InequalityFactorGraph &workingSet, const VectorValues &xk, + const VectorValues &p) const; /* * Given an initial value this function determine which constraints are active @@ -102,34 +102,19 @@ public: * A constraint Ax <= b is active if we have an x' s.t. Ax' = b */ InequalityFactorGraph identifyActiveConstraints( - const InequalityFactorGraph& inequalities, - const VectorValues& initialValues, const VectorValues& duals) const; + const InequalityFactorGraph &inequalities, + const VectorValues &initialValues, const VectorValues &duals) const; /** Optimize with the provided feasible initial values * TODO: throw exception if the initial values is not feasible wrt inequality constraints * TODO: comment duals */ - pair optimize(const VectorValues& initialValues, - const VectorValues& duals = VectorValues()) const; + pair optimize(const VectorValues &initialValues, + const VectorValues &duals = VectorValues()) const; /** - * Optimize without initial values - * TODO: Find a feasible initial solution that doesn't involve simplex method - * nor Solving another LP + * Optimize without initial values. */ - pair optimize() const { - - // Initialize workingSet from the feasible initialValues -// InequalityFactorGraph workingSet = identifyActiveConstraints( -// lp_.inequalities, initialValues, duals); -// LPState state(initialValues, duals, workingSet, false, 0); - - /// main loop of the solver -// while (!state.converged) { -// state = iterate(state); -// } - -// return make_pair(state.values, state.duals); - } + pair optimize() const; }; -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/linear/tests/testLPSolver.cpp b/gtsam_unstable/linear/tests/testLPSolver.cpp index bb080cc7e..fc40f5a36 100644 --- a/gtsam_unstable/linear/tests/testLPSolver.cpp +++ b/gtsam_unstable/linear/tests/testLPSolver.cpp @@ -31,7 +31,6 @@ #include #include - using namespace std; using namespace gtsam; using namespace gtsam::symbol_shorthand; @@ -46,21 +45,16 @@ using namespace gtsam::symbol_shorthand; */ LP simpleLP1() { LP lp; - lp.cost = LinearCost(1, Vector2( -1., -1.)); // min -x1-x2 (max x1+x2) - lp.inequalities.push_back( - LinearInequality(1, Vector2( -1, 0), 0, 1)); // x1 >= 0 - lp.inequalities.push_back( - LinearInequality(1, Vector2( 0, -1), 0, 2)); // x2 >= 0 - lp.inequalities.push_back( - LinearInequality(1, Vector2( 1, 2), 4, 3)); // x1 + 2*x2 <= 4 - lp.inequalities.push_back( - LinearInequality(1, Vector2( 4, 2), 12, 4)); // 4x1 + 2x2 <= 12 - lp.inequalities.push_back( - LinearInequality(1, Vector2( -1, 1), 1, 5)); // -x1 + x2 <= 1 + lp.cost = LinearCost(1, Vector2(-1., -1.)); // min -x1-x2 (max x1+x2) + lp.inequalities.push_back(LinearInequality(1, Vector2(-1, 0), 0, 1)); // x1 >= 0 + lp.inequalities.push_back(LinearInequality(1, Vector2(0, -1), 0, 2)); // x2 >= 0 + lp.inequalities.push_back(LinearInequality(1, Vector2(1, 2), 4, 3)); // x1 + 2*x2 <= 4 + lp.inequalities.push_back(LinearInequality(1, Vector2(4, 2), 12, 4)); // 4x1 + 2x2 <= 12 + lp.inequalities.push_back(LinearInequality(1, Vector2(-1, 1), 1, 5)); // -x1 + x2 <= 1 return lp; } -LP infeasibleLP(){ +LP infeasibleLP() { LP lp; lp.cost = LinearCost(1, Vector3(-1, -1, -2)); @@ -91,13 +85,13 @@ TEST(LPInitSolverMatlab, initialization) { expectedInitLP.inequalities.push_back( LinearInequality(1, Vector2( -1, 0), 2, Vector::Constant(1, -1), 0, 1)); // -x1 - y <= 0 expectedInitLP.inequalities.push_back( - LinearInequality(1, Vector2( 0, -1), 2, Vector::Constant(1, -1), 0, 2)); // -x2 - y <= 0 + LinearInequality(1, Vector2( 0, -1), 2, Vector::Constant(1, -1), 0, 2));// -x2 - y <= 0 expectedInitLP.inequalities.push_back( - LinearInequality(1, Vector2( 1, 2), 2, Vector::Constant(1, -1), 4, 3)); // x1 + 2*x2 - y <= 4 + LinearInequality(1, Vector2( 1, 2), 2, Vector::Constant(1, -1), 4, 3));// x1 + 2*x2 - y <= 4 expectedInitLP.inequalities.push_back( - LinearInequality(1, Vector2( 4, 2), 2, Vector::Constant(1, -1), 12, 4)); // 4x1 + 2x2 - y <= 12 + LinearInequality(1, Vector2( 4, 2), 2, Vector::Constant(1, -1), 12, 4));// 4x1 + 2x2 - y <= 12 expectedInitLP.inequalities.push_back( - LinearInequality(1, Vector2( -1, 1), 2, Vector::Constant(1, -1), 1, 5)); // -x1 + x2 - y <= 1 + LinearInequality(1, Vector2( -1, 1), 2, Vector::Constant(1, -1), 1, 5));// -x1 + x2 - y <= 1 CHECK(assert_equal(expectedInitLP, *initLP, 1e-10)); LPSolver lpSolveInit(*initLP); @@ -122,61 +116,56 @@ TEST(LPInitSolverMatlab, initialization) { * x + 2y = 6 */ TEST(LPSolver, overConstrainedLinearSystem) { - GaussianFactorGraph graph; - Matrix A1 = Vector3(1,1,1); - Matrix A2 = Vector3(1,-1,2); - Vector b = Vector3( 1, 5, 6); - JacobianFactor factor(1, A1, 2, A2, b, noiseModel::Constrained::All(3)); - graph.push_back(factor); +GaussianFactorGraph graph; +Matrix A1 = Vector3(1,1,1); +Matrix A2 = Vector3(1,-1,2); +Vector b = Vector3( 1, 5, 6); +JacobianFactor factor(1, A1, 2, A2, b, noiseModel::Constrained::All(3)); +graph.push_back(factor); - VectorValues x = graph.optimize(); - // This check confirms that gtsam linear constraint solver can't handle over-constrained system - CHECK(factor.error(x) != 0.0); +VectorValues x = graph.optimize(); +// This check confirms that gtsam linear constraint solver can't handle over-constrained system +CHECK(factor.error(x) != 0.0); } TEST(LPSolver, overConstrainedLinearSystem2) { - GaussianFactorGraph graph; - graph.push_back(JacobianFactor(1, ones(1, 1), 2, ones(1, 1), ones(1), noiseModel::Constrained::All(1))); - graph.push_back(JacobianFactor(1, ones(1, 1), 2, -ones(1, 1), 5*ones(1), noiseModel::Constrained::All(1))); - graph.push_back(JacobianFactor(1, ones(1, 1), 2, 2*ones(1, 1), 6*ones(1), noiseModel::Constrained::All(1))); - VectorValues x = graph.optimize(); - // This check confirms that gtsam linear constraint solver can't handle over-constrained system - CHECK(graph.error(x) != 0.0); +GaussianFactorGraph graph; +graph.push_back(JacobianFactor(1, ones(1, 1), 2, ones(1, 1), ones(1), noiseModel::Constrained::All(1))); +graph.push_back(JacobianFactor(1, ones(1, 1), 2, -ones(1, 1), 5*ones(1), noiseModel::Constrained::All(1))); +graph.push_back(JacobianFactor(1, ones(1, 1), 2, 2*ones(1, 1), 6*ones(1), noiseModel::Constrained::All(1))); +VectorValues x = graph.optimize(); +// This check confirms that gtsam linear constraint solver can't handle over-constrained system +CHECK(graph.error(x) != 0.0); } /* ************************************************************************* */ TEST(LPSolver, simpleTest1) { - LP lp = simpleLP1(); +LP lp = simpleLP1(); +LPSolver lpSolver(lp); +VectorValues init; +init.insert(1, zero(2)); - LPSolver lpSolver(lp); - VectorValues init; - init.insert(1, zero(2)); +VectorValues x1 = lpSolver.solveWithCurrentWorkingSet(init, + InequalityFactorGraph()); +VectorValues expected_x1; +expected_x1.insert(1, Vector2( 1, 1)); +CHECK(assert_equal(expected_x1, x1, 1e-10)); - VectorValues x1 = lpSolver.solveWithCurrentWorkingSet(init, - InequalityFactorGraph()); - VectorValues expected_x1; - expected_x1.insert(1, Vector2( 1, 1)); - CHECK(assert_equal(expected_x1, x1, 1e-10)); - - VectorValues result, duals; - boost::tie(result, duals) = lpSolver.optimize(init); - VectorValues expectedResult; - expectedResult.insert(1, Vector2(8./3., 2./3.)); - CHECK(assert_equal(expectedResult, result, 1e-10)); +VectorValues result, duals; +boost::tie(result, duals) = lpSolver.optimize(init); +VectorValues expectedResult; +expectedResult.insert(1, Vector2(8./3., 2./3.)); +CHECK(assert_equal(expectedResult, result, 1e-10)); } - /* ************************************************************************* */ TEST(LPSolver, testWithoutInitialValues) { -// LP lp = simpleLP1(); -// -// LPSolver lpSolver(lp); -// VectorValues result, duals; -// boost::tie(result, duals) = lpSolver.optimize(); -// -// VectorValues expectedResult; -// expectedResult.insert(1, Vector2(8./3., 2./3.)); -// CHECK(assert_equal(expectedResult, result, 1e-10)); +LP lp = simpleLP1(); +LPSolver lpSolver(lp); +VectorValues result,duals, expectedResult; +expectedResult.insert(1, Vector2(8./3., 2./3.)); +boost::tie(result, duals) = lpSolver.optimize(); +CHECK(assert_equal(expectedResult, result)); } /** @@ -187,18 +176,18 @@ TEST(LPSolver, testWithoutInitialValues) { */ /* ************************************************************************* */ TEST(LPSolver, LinearCost) { - LinearCost cost(1, Vector3( 2., 4., 6.)); - VectorValues x; - x.insert(1, Vector3( 1., 3., 5.)); - double error = cost.error(x); - double expectedError = 44.0; - DOUBLES_EQUAL(expectedError, error, 1e-100); +LinearCost cost(1, Vector3( 2., 4., 6.)); +VectorValues x; +x.insert(1, Vector3( 1., 3., 5.)); +double error = cost.error(x); +double expectedError = 44.0; +DOUBLES_EQUAL(expectedError, error, 1e-100); } /* ************************************************************************* */ int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); +TestResult tr; +return TestRegistry::runAllTests(tr); } /* ************************************************************************* */