diff --git a/gtsam_unstable/linear/ActiveSetSolver.h b/gtsam_unstable/linear/ActiveSetSolver.h index 24f3654bb..cb7545028 100644 --- a/gtsam_unstable/linear/ActiveSetSolver.h +++ b/gtsam_unstable/linear/ActiveSetSolver.h @@ -43,8 +43,7 @@ public: * Dual Jacobians used to build a dual factor graph. */ template - TermsContainer collectDualJacobians(Key key, - const FactorGraph& graph, + TermsContainer collectDualJacobians(Key key, const FactorGraph& graph, const VariableIndex& variableIndex) const { /* * Iterates through each factor in the factor graph and checks @@ -53,43 +52,44 @@ public: */ TermsContainer Aterms; if (variableIndex.find(key) != variableIndex.end()) { - for(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)); + for (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)); + } } + return Aterms; } - return Aterms; -} -/** - * Identifies active constraints that shouldn't be active anymore. - */ -int identifyLeavingConstraint(const InequalityFactorGraph& workingSet, - const VectorValues& lambdas) const; + /** + * Identifies active constraints that shouldn't be active anymore. + */ + int identifyLeavingConstraint(const InequalityFactorGraph& workingSet, + const VectorValues& lambdas) const; -/** - * Builds a dual graph from the current working set. - */ -GaussianFactorGraph::shared_ptr buildDualGraph( - const InequalityFactorGraph& workingSet, const VectorValues& delta) const; + /** + * Builds a dual graph from the current working set. + */ + GaussianFactorGraph::shared_ptr buildDualGraph( + const InequalityFactorGraph& workingSet, const VectorValues& delta) const; protected: -/** - * Protected constructor because this class doesn't have any meaning without - * a concrete Programming problem to solve. - */ -ActiveSetSolver() : - constrainedKeys_() { -} + /** + * Protected constructor because this class doesn't have any meaning without + * a concrete Programming problem to solve. + */ + ActiveSetSolver() : + constrainedKeys_() { + } -/** - * Computes the distance to move from the current point being examined to the next - * location to be examined by the graph. This should only be used where there are less - * than two constraints active. - */ -boost::tuple computeStepSize( - const InequalityFactorGraph& workingSet, const VectorValues& xk, - const VectorValues& p, const double& startAlpha) const; + /** + * Computes the distance to move from the current point being examined to the next + * location to be examined by the graph. This should only be used where there are less + * than two constraints active. + */ + boost::tuple computeStepSize( + const InequalityFactorGraph& workingSet, const VectorValues& xk, + const VectorValues& p, const double& startAlpha) const; }; } // namespace gtsam diff --git a/gtsam_unstable/linear/EqualityFactorGraph.h b/gtsam_unstable/linear/EqualityFactorGraph.h index 2e1c23268..62865dbde 100644 --- a/gtsam_unstable/linear/EqualityFactorGraph.h +++ b/gtsam_unstable/linear/EqualityFactorGraph.h @@ -26,7 +26,7 @@ namespace gtsam { * This class is used to represent an equality constraint on * a Programming problem of the form Ax = b. */ -class EqualityFactorGraph : public FactorGraph { +class EqualityFactorGraph: public FactorGraph { public: typedef boost::shared_ptr shared_ptr; @@ -36,8 +36,8 @@ public: */ double error(const VectorValues& x) const { double total_error = 0.; - for(const sharedFactor& factor: *this){ - if(factor) + for (const sharedFactor& factor : *this) { + if (factor) total_error += factor->error(x); } return total_error; diff --git a/gtsam_unstable/linear/InequalityFactorGraph.h b/gtsam_unstable/linear/InequalityFactorGraph.h index d7083492d..c6a0fb46d 100644 --- a/gtsam_unstable/linear/InequalityFactorGraph.h +++ b/gtsam_unstable/linear/InequalityFactorGraph.h @@ -47,10 +47,10 @@ public: * Compute error of a guess. * Infinity error if it violates an inequality; zero otherwise. */ double error(const VectorValues& x) const { - for(const sharedFactor& factor: *this) { - if(factor) - if (factor->error(x) > 0) - return std::numeric_limits::infinity(); + for (const sharedFactor& factor : *this) { + if (factor) + if (factor->error(x) > 0) + return std::numeric_limits::infinity(); } return 0.0; } diff --git a/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h b/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h index 0c362b0bc..d3a5a14f1 100644 --- a/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h +++ b/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h @@ -5,7 +5,6 @@ * @date 1/24/16 */ - namespace gtsam { class InfeasibleOrUnboundedProblem: public ThreadsafeException< diff --git a/gtsam_unstable/linear/LPInitSolver.h b/gtsam_unstable/linear/LPInitSolver.h index 53a20807a..2b324cd1b 100644 --- a/gtsam_unstable/linear/LPInitSolver.h +++ b/gtsam_unstable/linear/LPInitSolver.h @@ -128,7 +128,7 @@ private: /// Collect all terms of a factor into a container. std::vector > collectTerms( const LinearInequality& factor) const { - std::vector > terms; + 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))); } @@ -140,7 +140,7 @@ private: const InequalityFactorGraph& inequalities) const { InequalityFactorGraph slackInequalities; for (const auto& factor : lp_.inequalities) { - std::vector > terms = collectTerms(*factor); // Cx + std::vector < std::pair > terms = collectTerms(*factor); // Cx terms.push_back(make_pair(yKey, Matrix::Constant(1, 1, -1.0))); // -y double d = factor->getb()[0]; slackInequalities.push_back( diff --git a/gtsam_unstable/linear/LPSolver.cpp b/gtsam_unstable/linear/LPSolver.cpp index 4647237d2..b9f5492af 100644 --- a/gtsam_unstable/linear/LPSolver.cpp +++ b/gtsam_unstable/linear/LPSolver.cpp @@ -109,7 +109,7 @@ GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors( lp_.cost.end(), std::inserter(difference, difference.end())); for (Key k : difference) { size_t dim = keysDim_.at(k); - graph->push_back(JacobianFactor(k, Matrix::Identity(dim,dim), xk.at(k))); + graph->push_back(JacobianFactor(k, Matrix::Identity(dim, dim), xk.at(k))); } } return graph; @@ -136,10 +136,10 @@ 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()); @@ -149,7 +149,7 @@ boost::shared_ptr LPSolver::createDualFactor(Key key, 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 + return boost::make_shared < JacobianFactor > (Aterms, b); // compute the least-square approximation of dual variables } else { return boost::make_shared(); } diff --git a/gtsam_unstable/linear/LinearCost.h b/gtsam_unstable/linear/LinearCost.h index 1e6463399..b489510af 100644 --- a/gtsam_unstable/linear/LinearCost.h +++ b/gtsam_unstable/linear/LinearCost.h @@ -30,90 +30,89 @@ typedef Eigen::RowVectorXd RowVector; */ class LinearCost: public JacobianFactor { public: - typedef LinearCost This; ///< Typedef to this class - typedef JacobianFactor Base; ///< Typedef to base class - typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + typedef LinearCost This; ///< Typedef to this class + typedef JacobianFactor Base; ///< Typedef to base class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class public: - /** default constructor for I/O */ - LinearCost() : - Base() { - } + /** default constructor for I/O */ + LinearCost() : + Base() { + } - /** Conversion from HessianFactor */ - explicit LinearCost(const HessianFactor& hf) { - throw std::runtime_error("Cannot convert HessianFactor to LinearCost"); - } + /** Conversion from HessianFactor */ + explicit LinearCost(const HessianFactor& hf) { + throw std::runtime_error("Cannot convert HessianFactor to LinearCost"); + } - /** Conversion from JacobianFactor */ - explicit LinearCost(const JacobianFactor& jf) : - Base(jf) { - if (jf.isConstrained()) { - throw std::runtime_error( - "Cannot convert a constrained JacobianFactor to LinearCost"); - } + /** Conversion from JacobianFactor */ + explicit LinearCost(const JacobianFactor& jf) : + Base(jf) { + if (jf.isConstrained()) { + throw std::runtime_error( + "Cannot convert a constrained JacobianFactor to LinearCost"); + } - if (jf.get_model()->dim() != 1) { - throw std::runtime_error( - "Only support single-valued linear cost factor!"); - } - } + if (jf.get_model()->dim() != 1) { + throw std::runtime_error( + "Only support single-valued linear cost factor!"); + } + } - /** Construct unary factor */ - LinearCost(Key i1, const RowVector& A1) : - Base(i1, A1, Vector1::Zero()) { - } + /** Construct unary factor */ + LinearCost(Key i1, const RowVector& A1) : + Base(i1, A1, Vector1::Zero()) { + } - /** Construct binary factor */ - LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, - double b) : - Base(i1, A1, i2, A2, Vector1::Zero()) { - } + /** Construct binary factor */ + LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, double b) : + Base(i1, A1, i2, A2, Vector1::Zero()) { + } - /** Construct ternary factor */ - LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3, - const RowVector& A3) : - Base(i1, A1, i2, A2, i3, A3, Vector1::Zero()) { - } + /** Construct ternary factor */ + LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3, + const RowVector& A3) : + Base(i1, A1, i2, A2, i3, A3, Vector1::Zero()) { + } - /** Construct an n-ary factor - * @tparam TERMS A container whose value type is std::pair, specifying the - * collection of keys and matrices making up the factor. */ - template - LinearCost(const TERMS& terms) : - Base(terms, Vector1::Zero()) { - } + /** Construct an n-ary factor + * @tparam TERMS A container whose value type is std::pair, specifying the + * collection of keys and matrices making up the factor. */ + template + LinearCost(const TERMS& terms) : + Base(terms, Vector1::Zero()) { + } - /** Virtual destructor */ - virtual ~LinearCost() { - } + /** Virtual destructor */ + virtual ~LinearCost() { + } - /** equals */ - virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const { - return Base::equals(lf, tol); - } + /** equals */ + virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const { + return Base::equals(lf, tol); + } - /** print */ - virtual void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const { - Base::print(s + " LinearCost: ", formatter); - } + /** print */ + virtual void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const { + Base::print(s + " LinearCost: ", formatter); + } - /** Clone this LinearCost */ - virtual GaussianFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - boost::make_shared(*this)); - } + /** Clone this LinearCost */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::static_pointer_cast < GaussianFactor + > (boost::make_shared < LinearCost > (*this)); + } - /** Special error_vector for constraints (A*x-b) */ - Vector error_vector(const VectorValues& c) const { - return unweighted_error(c); - } + /** Special error_vector for constraints (A*x-b) */ + Vector error_vector(const VectorValues& c) const { + return unweighted_error(c); + } - /** Special error for single-valued inequality constraints. */ - virtual double error(const VectorValues& c) const { - return error_vector(c)[0]; - } + /** Special error for single-valued inequality constraints. */ + virtual double error(const VectorValues& c) const { + return error_vector(c)[0]; + } }; // \ LinearCost diff --git a/gtsam_unstable/linear/LinearEquality.h b/gtsam_unstable/linear/LinearEquality.h index bb52bb33e..2463ef31c 100644 --- a/gtsam_unstable/linear/LinearEquality.h +++ b/gtsam_unstable/linear/LinearEquality.h @@ -44,13 +44,14 @@ public: /** * Construct from a constrained noisemodel JacobianFactor with a dual key. */ - explicit LinearEquality(const JacobianFactor& jf, Key dualKey) : Base(jf), dualKey_(dualKey){ + explicit LinearEquality(const JacobianFactor& jf, Key dualKey) : + Base(jf), dualKey_(dualKey) { if (!jf.isConstrained()) { - throw std::runtime_error("Cannot convert an unconstrained JacobianFactor to LinearEquality"); + throw std::runtime_error( + "Cannot convert an unconstrained JacobianFactor to LinearEquality"); } } - /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ explicit LinearEquality(const HessianFactor& hf) { throw std::runtime_error("Cannot convert HessianFactor to LinearEquality"); @@ -100,15 +101,19 @@ public: /** Clone this LinearEquality */ virtual GaussianFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - boost::make_shared(*this)); + return boost::static_pointer_cast < GaussianFactor + > (boost::make_shared < LinearEquality > (*this)); } /// dual key - Key dualKey() const { return dualKey_; } + Key dualKey() const { + return dualKey_; + } /// for active set method: equality constraints are always active - bool active() const { return true; } + bool active() const { + return true; + } /** Special error_vector for constraints (A*x-b) */ Vector error_vector(const VectorValues& c) const { @@ -123,11 +128,12 @@ public: return 0.0; } -}; // \ LinearEquality - +}; +// \ LinearEquality /// traits -template<> struct traits : public Testable {}; +template<> struct traits : public Testable { +}; } // \ namespace gtsam diff --git a/gtsam_unstable/linear/LinearInequality.h b/gtsam_unstable/linear/LinearInequality.h index ad0bb3f02..be3a6eb3c 100644 --- a/gtsam_unstable/linear/LinearInequality.h +++ b/gtsam_unstable/linear/LinearInequality.h @@ -52,9 +52,11 @@ public: } /** Conversion from JacobianFactor */ - explicit LinearInequality(const JacobianFactor& jf, Key dualKey) : Base(jf), dualKey_(dualKey), active_(true) { + explicit LinearInequality(const JacobianFactor& jf, Key dualKey) : + Base(jf), dualKey_(dualKey), active_(true) { if (!jf.isConstrained()) { - throw std::runtime_error("Cannot convert an unconstrained JacobianFactor to LinearInequality"); + throw std::runtime_error( + "Cannot convert an unconstrained JacobianFactor to LinearInequality"); } if (jf.get_model()->dim() != 1) { @@ -64,20 +66,20 @@ public: /** Construct unary factor */ LinearInequality(Key i1, const RowVector& A1, double b, Key dualKey) : - Base(i1, A1, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_( - dualKey), active_(true) { + Base(i1, A1, (Vector(1) << b).finished(), + noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) { } /** Construct binary factor */ - LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, double b, - Key dualKey) : - Base(i1, A1, i2, A2, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_( - dualKey), active_(true) { + LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, + double b, Key dualKey) : + Base(i1, A1, i2, A2, (Vector(1) << b).finished(), + noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) { } /** Construct ternary factor */ - LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3, - const RowVector& A3, double b, Key dualKey) : + LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, + Key i3, const RowVector& A3, double b, Key dualKey) : Base(i1, A1, i2, A2, i3, A3, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) { } @@ -112,21 +114,29 @@ public: /** Clone this LinearInequality */ virtual GaussianFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - boost::make_shared(*this)); + return boost::static_pointer_cast < GaussianFactor + > (boost::make_shared < LinearInequality > (*this)); } /// dual key - Key dualKey() const { return dualKey_; } + Key dualKey() const { + return dualKey_; + } /// return true if this constraint is active - bool active() const { return active_; } + bool active() const { + return active_; + } /// Make this inequality constraint active - void activate() { active_ = true; } + void activate() { + active_ = true; + } /// Make this inequality constraint inactive - void inactivate() { active_ = false; } + void inactivate() { + active_ = false; + } /** Special error_vector for constraints (A*x-b) */ Vector error_vector(const VectorValues& c) const { @@ -149,10 +159,12 @@ public: return aTp; } -}; // \ LinearInequality +}; +// \ LinearInequality /// traits -template<> struct traits : public Testable {}; +template<> struct traits : public Testable { +}; } // \ namespace gtsam diff --git a/gtsam_unstable/linear/QP.h b/gtsam_unstable/linear/QP.h index 17e3347ce..457a859de 100644 --- a/gtsam_unstable/linear/QP.h +++ b/gtsam_unstable/linear/QP.h @@ -42,7 +42,8 @@ struct QP { QP(const GaussianFactorGraph& _cost, const EqualityFactorGraph& _linearEqualities, const InequalityFactorGraph& _linearInequalities) : - cost(_cost), equalities(_linearEqualities), inequalities(_linearInequalities) { + cost(_cost), equalities(_linearEqualities), inequalities( + _linearInequalities) { } /** print */ diff --git a/gtsam_unstable/linear/RawQP.h b/gtsam_unstable/linear/RawQP.h index 93d27d407..bfb13037e 100644 --- a/gtsam_unstable/linear/RawQP.h +++ b/gtsam_unstable/linear/RawQP.h @@ -36,9 +36,7 @@ private: public: RawQP() : - row_to_constraint_v(), E(), IG(), IL(), varNumber(1), - b(), g(), varname_to_key(), H(), f(), - obj_name(), name_(), up(), lo(), Free() { + row_to_constraint_v(), E(), IG(), IL(), varNumber(1), b(), g(), varname_to_key(), H(), f(), obj_name(), name_(), up(), lo(), Free() { } void setName( diff --git a/gtsam_unstable/linear/tests/testLinearEquality.cpp b/gtsam_unstable/linear/tests/testLinearEquality.cpp index 13b2fa5b5..fa94dd255 100644 --- a/gtsam_unstable/linear/tests/testLinearEquality.cpp +++ b/gtsam_unstable/linear/tests/testLinearEquality.cpp @@ -28,20 +28,20 @@ using namespace std; using namespace gtsam; using namespace boost::assign; -GTSAM_CONCEPT_TESTABLE_INST(LinearEquality) +GTSAM_CONCEPT_TESTABLE_INST (LinearEquality) namespace { - namespace simple { - // Terms we'll use - const vector > terms = list_of > - (make_pair(5, Matrix3::Identity())) - (make_pair(10, 2*Matrix3::Identity())) - (make_pair(15, 3*Matrix3::Identity())); +namespace simple { +// Terms we'll use +const vector > terms = list_of < pair + > (make_pair(5, Matrix3::Identity()))( + make_pair(10, 2 * Matrix3::Identity()))( + make_pair(15, 3 * Matrix3::Identity())); - // RHS and sigmas - const Vector b = (Vector(3) << 1., 2., 3.).finished(); - const SharedDiagonal noise = noiseModel::Constrained::All(3); - } +// RHS and sigmas +const Vector b = (Vector(3) << 1., 2., 3.).finished(); +const SharedDiagonal noise = noiseModel::Constrained::All(3); +} } /* ************************************************************************* */ @@ -53,7 +53,7 @@ TEST(LinearEquality, constructors_and_accessors) { // One term constructor LinearEquality expected( - boost::make_iterator_range(terms.begin(), terms.begin() + 1), b, 0); + boost::make_iterator_range(terms.begin(), terms.begin() + 1), b, 0); LinearEquality actual(terms[0].first, terms[0].second, b, 0); EXPECT(assert_equal(expected, actual)); LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back()); @@ -65,9 +65,9 @@ TEST(LinearEquality, constructors_and_accessors) { // Two term constructor LinearEquality expected( - boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, 0); + boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, 0); LinearEquality actual(terms[0].first, terms[0].second, - terms[1].first, terms[1].second, b, 0); + terms[1].first, terms[1].second, b, 0); EXPECT(assert_equal(expected, actual)); LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back()); EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1))); @@ -78,9 +78,9 @@ TEST(LinearEquality, constructors_and_accessors) { // Three term constructor LinearEquality expected( - boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, 0); + boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, 0); LinearEquality actual(terms[0].first, terms[0].second, - terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, 0); + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, 0); EXPECT(assert_equal(expected, actual)); LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back()); EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1))); @@ -93,10 +93,10 @@ TEST(LinearEquality, constructors_and_accessors) /* ************************************************************************* */ TEST(LinearEquality, Hessian_conversion) { HessianFactor hessian(0, (Matrix(4,4) << - 1.57, 2.695, -1.1, -2.35, - 2.695, 11.3125, -0.65, -10.225, - -1.1, -0.65, 1, 0.5, - -2.35, -10.225, 0.5, 9.25).finished(), + 1.57, 2.695, -1.1, -2.35, + 2.695, 11.3125, -0.65, -10.225, + -1.1, -0.65, 1, 0.5, + -2.35, -10.225, 0.5, 9.25).finished(), (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(), 73.1725); @@ -169,8 +169,8 @@ TEST(LinearEquality, matrices) augmentedJacobianExpected << jacobianExpected, rhsExpected; Matrix augmentedHessianExpected = - augmentedJacobianExpected.transpose() * simple::noise->R().transpose() - * simple::noise->R() * augmentedJacobianExpected; + augmentedJacobianExpected.transpose() * simple::noise->R().transpose() + * simple::noise->R() * augmentedJacobianExpected; // Whitened Jacobian EXPECT(assert_equal(jacobianExpected, factor.jacobian().first)); @@ -210,8 +210,8 @@ TEST(LinearEquality, operators ) // test gradient at zero Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian(); VectorValues expectedG; - expectedG.insert(1, (Vector(2) << 0.2, -0.1).finished()); - expectedG.insert(2, (Vector(2) << -0.2, 0.1).finished()); + expectedG.insert(1, (Vector(2) << 0.2, -0.1).finished()); + expectedG.insert(2, (Vector(2) << -0.2, 0.1).finished()); VectorValues actualG = lf.gradientAtZero(); EXPECT(assert_equal(expectedG, actualG)); } @@ -233,5 +233,8 @@ TEST(LinearEquality, empty ) } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 3b517f2f7..1f21a9e49 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -42,8 +42,8 @@ QP createTestCase() { //TODO: THIS TEST MIGHT BE WRONG : the last parameter might be 5 instead of 10 because the form of the equation // Should be 0.5x'Gx + gx + f : Nocedal 449 qp.cost.push_back( - HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1, - 3.0 * I_1x1, 2.0 * I_1x1, Z_1x1, 10.0)); + HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1, 3.0 * I_1x1, 2.0 * I_1x1, + Z_1x1, 10.0)); // Inequality constraints qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 @@ -96,8 +96,8 @@ QP 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 qp.cost.push_back( - HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1, - 2.0 * I_1x1, Z_1x1, 0.0)); + HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1, 2.0 * I_1x1, Z_1x1, + 0.0)); // Equality constraints // x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector @@ -140,9 +140,9 @@ TEST(QPSolver, indentifyActiveConstraints) { qp.inequalities, currentSolution); CHECK(!workingSet.at(0)->active()); // inactive - CHECK(workingSet.at(1)->active()); // active - CHECK(workingSet.at(2)->active()); // active - CHECK(!workingSet.at(3)->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; @@ -211,14 +211,15 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) { expectedSolution.insert(X(2), (Vector(1) << 0.5).finished()); CHECK(assert_equal(expectedSolution, solution, 1e-100)); } + pair testParser(QPSParser parser) { QP exampleqp = parser.Parse(); QP expectedqp; Key X1(Symbol('X', 1)), X2(Symbol('X', 2)); // min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2 expectedqp.cost.push_back( - HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, 1.5 * kOne, - 10.0 * I_1x1, -2.0 * kOne, 4.0)); + HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, 1.5 * kOne, 10.0 * I_1x1, + -2.0 * kOne, 4.0)); // 2x + y >= 2 // -x + 2y <= 6 expectedqp.inequalities.push_back( @@ -267,13 +268,15 @@ QP 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 qp.cost.push_back( - HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1, - 2.0 * I_1x1, 6 * I_1x1, 1000.0)); + HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1, 2.0 * I_1x1, + 6 * I_1x1, 1000.0)); // Inequality constraints qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 - qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1)); //-x1 + 2*x2 <=2 - qp.inequalities.push_back(LinearInequality(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2)); // 2*x1 + x2 <=3 + qp.inequalities.push_back( + LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1)); //-x1 + 2*x2 <=2 + qp.inequalities.push_back( + LinearInequality(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2)); // 2*x1 + x2 <=3 qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 3)); // -x1 <= 0 qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 4)); // -x2 <= 0