diff --git a/.gitignore b/.gitignore index b3ea282dd..5ce2ad999 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ +/gtsam/inference/FactorGraph.h /build* .idea *.pyc diff --git a/CMakeLists.txt b/CMakeLists.txt index 95cfa9d41..2d6e4c1b3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -241,7 +241,7 @@ else() endif() # Add the bundled version of eigen to the include path so that it can still be included # with #include - include_directories(BEFORE "gtsam/3rdparty/Eigen/") + include_directories(BEFORE SYSTEM "gtsam/3rdparty/Eigen/") # set full path to be used by external projects # this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index e97860eaa..e8c14ac98 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -351,3 +351,5 @@ namespace gtsam { }; // FactorGraph } // namespace gtsam + +#include \ No newline at end of file diff --git a/gtsam_unstable/linear/QP.h b/gtsam_unstable/linear/QP.h index 7776995df..34971aff7 100644 --- a/gtsam_unstable/linear/QP.h +++ b/gtsam_unstable/linear/QP.h @@ -45,6 +45,10 @@ struct QP { _linearInequalities) { } + QP(std::string MPS_FileName): + cost(), equalities(), inequalities() {} + + /** print */ void print(const std::string& s = "") { std::cout << s << std::endl; diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index c4128cdaf..dd5b5c155 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -213,6 +213,41 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) { CHECK(assert_equal(expectedSolution, solution, 1e-100)); } +QP createExampleQP(){ + QP exampleqp; + exampleqp.cost.push_back( + HessianFactor(X(1),Y(1), + 8.0 *ones(1,1), 2.0 * ones(1,1), 1.5*ones(1), + 10.0 *ones(1,1), -2.0 *ones(1), 4.0)); + // 2x + y >= 2 + exampleqp.inequalities.push_back( + LinearInequality(X(1), -2.0*ones(1,1), Y(1), -ones(1,1), -2, 0)); + // -x + 2y <= 6 + exampleqp.inequalities.push_back( + LinearInequality(X(1), -ones(1,1), Y(1), 2.0* ones(1,1), 6, 1)); + //x >= 0 + exampleqp.inequalities.push_back( + LinearInequality(X(1), -ones(1,1), 0, 2)); + // y > = 0 + exampleqp.inequalities.push_back( + LinearInequality(Y(1), -ones(1,1), 0, 3)); + // x<= 20 + exampleqp.inequalities.push_back( + LinearInequality(X(1), ones(1,1), 20, 4)); + return exampleqp; +}; + +TEST(QPSolver, QPExampleData){ + QP exampleqp("QPExample.QPS"); + + QP expectedqp = createExampleQP(); + // min f(x,y) = 4 + 1.5x -y + 0.5(8x^2 + 2xy + 2yx + 10y^2 + +// CHECK(expectedqp.cost.equals(exampleqp.cost, 1e-7)); +// CHECK(expectedqp.inequalities.equals(exampleqp.inequalities, 1e-7)); +// CHECK(expectedqp.equalities.equals(exampleqp.equalities, 1e-7)); +} + /* ************************************************************************* */ // Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html QP createTestMatlabQPEx() { diff --git a/gtsam_unstable/nonlinear/ConstrainedFactor.h b/gtsam_unstable/nonlinear/ConstrainedFactor.h index b95ca4ed6..ccb42dbed 100644 --- a/gtsam_unstable/nonlinear/ConstrainedFactor.h +++ b/gtsam_unstable/nonlinear/ConstrainedFactor.h @@ -40,6 +40,8 @@ public: typedef boost::shared_ptr shared_ptr; public: + /// Default constructor with default key value. + ConstrainedFactor() : dualKey_(0) {} /// Construct with dual key ConstrainedFactor(Key dualKey) : dualKey_(dualKey) {} diff --git a/gtsam_unstable/nonlinear/NonlinearConstraint.h b/gtsam_unstable/nonlinear/NonlinearConstraint.h index bf4b5fbb9..ce6648d10 100644 --- a/gtsam_unstable/nonlinear/NonlinearConstraint.h +++ b/gtsam_unstable/nonlinear/NonlinearConstraint.h @@ -327,7 +327,7 @@ public: /** * Default Constructor for I/O */ - NonlinearConstraint3() { + NonlinearConstraint3(){ } /** diff --git a/gtsam_unstable/nonlinear/tests/testLinearConstraintSQP.cpp b/gtsam_unstable/nonlinear/tests/testLinearConstraintSQP.cpp index c12514156..f0be70198 100644 --- a/gtsam_unstable/nonlinear/tests/testLinearConstraintSQP.cpp +++ b/gtsam_unstable/nonlinear/tests/testLinearConstraintSQP.cpp @@ -38,18 +38,22 @@ const double tol = 1e-10; //****************************************************************************** // x + y - 1 = 0 -class ConstraintProblem1 : public LinearEqualityFactor2 { +class ConstraintProblem1: public LinearEqualityFactor2 { typedef LinearEqualityFactor2 Base; public: - ConstraintProblem1(Key xK, Key yK, Key dualKey) : Base(xK, yK, dualKey, 1) {} + ConstraintProblem1(Key xK, Key yK, Key dualKey) : + Base(xK, yK, dualKey, 1) { + } // x + y - 1 Vector evaluateError(const double& x, const double& y, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - if (H1) *H1 = eye(1); - if (H2) *H2 = eye(1); + if (H1) + *H1 = eye(1); + if (H2) + *H2 = eye(1); return (Vector(1) << x + y - 1.0).finished(); } }; @@ -62,10 +66,10 @@ TEST(testlcnlpSolver, QPProblem) { // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // Hence here we have G11 = 2, G12 = 0, G22 = 2, g1 = 0, g2 = 0, f = 0 HessianFactor hf(X(1), Y(1), 2.0 * ones(1,1), zero(1), zero(1), - 2*ones(1,1), zero(1) , 0); + 2*ones(1,1), zero(1) , 0); EqualityFactorGraph equalities; - LinearEquality linearConstraint(X(1), ones(1), Y(1), ones(1), 1*ones(1), dualKey); // x + y - 1 = 0 + LinearEquality linearConstraint(X(1), ones(1), Y(1), ones(1), 1*ones(1), dualKey);// x + y - 1 = 0 equalities.push_back(linearConstraint); // Compare against QP @@ -86,7 +90,7 @@ TEST(testlcnlpSolver, QPProblem) { Values linPoint; linPoint.insert(X(1), zero(1)); linPoint.insert(Y(1), zero(1)); - lcnlp.cost.add(LinearContainerFactor(hf, linPoint)); // wrap it using linearcontainerfactor + lcnlp.cost.add(LinearContainerFactor(hf, linPoint));// wrap it using linearcontainerfactor lcnlp.linearEqualities.add(ConstraintProblem1(X(1), Y(1), dualKey)); Values initialValues; @@ -106,15 +110,18 @@ TEST(testlcnlpSolver, QPProblem) { /** * A simple linear constraint on Pose3's x coordinate enforcing x==0 */ -class LineConstraintX : public LinearEqualityFactor1 { +class LineConstraintX: public LinearEqualityFactor1 { typedef LinearEqualityFactor1 Base; public: - LineConstraintX(Key key, Key dualKey) : Base(key, dualKey, 1) { + LineConstraintX(Key key, Key dualKey) : + Base(key, dualKey, 1) { } - Vector evaluateError(const Pose3& pose, boost::optional H = boost::none) const { + Vector evaluateError(const Pose3& pose, boost::optional H = + boost::none) const { if (H) - *H = (Matrix(1,6) << zeros(1,3), pose.rotation().matrix().row(0)).finished(); + *H = + (Matrix(1, 6) << zeros(1, 3), pose.rotation().matrix().row(0)).finished(); return (Vector(1) << pose.x()).finished(); } }; @@ -122,7 +129,6 @@ public: TEST(testlcnlpSolver, poseOnALine) { const Key dualKey = 0; - //Instantiate LinearConstraintNLP LinearConstraintNLP lcnlp; lcnlp.cost.add(PriorFactor(X(1), Pose3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 0, 0)), noiseModel::Unit::Create(6))); @@ -144,16 +150,20 @@ TEST(testlcnlpSolver, poseOnALine) { //****************************************************************************** /// x + y - 1 <= 0 -class InequalityProblem1 : public LinearInequalityFactor2 { +class InequalityProblem1: public LinearInequalityFactor2 { typedef LinearInequalityFactor2 Base; public: - InequalityProblem1(Key xK, Key yK, Key dualKey) : Base(xK, yK, dualKey) {} + InequalityProblem1(Key xK, Key yK, Key dualKey) : + Base(xK, yK, dualKey) { + } double computeError(const double& x, const double& y, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - if (H1) *H1 = eye(1); - if (H2) *H2 = eye(1); + if (H1) + *H1 = eye(1); + if (H2) + *H2 = eye(1); return x + y - 1.0; } }; @@ -166,10 +176,10 @@ TEST(testlcnlpSolver, inequalityConstraint) { // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f // Hence here we have G11 = 2, G12 = 0, G22 = 2, g1 = 0, g2 = 0, f = 0 HessianFactor hf(X(1), Y(1), 2.0 * ones(1,1), zero(1), zero(1), - 2*ones(1,1), zero(1) , 0); + 2*ones(1,1), zero(1) , 0); InequalityFactorGraph inequalities; - LinearInequality linearConstraint(X(1), ones(1), Y(1), ones(1), 1.0, dualKey); // x + y - 1 <= 0 + LinearInequality linearConstraint(X(1), ones(1), Y(1), ones(1), 1.0, dualKey);// x + y - 1 <= 0 inequalities.push_back(linearConstraint); // Compare against QP @@ -190,7 +200,7 @@ TEST(testlcnlpSolver, inequalityConstraint) { Values linPoint; linPoint.insert(X(1), zero(1)); linPoint.insert(Y(1), zero(1)); - lcnlp.cost.add(LinearContainerFactor(hf, linPoint)); // wrap it using linearcontainerfactor + lcnlp.cost.add(LinearContainerFactor(hf, linPoint));// wrap it using linearcontainerfactor lcnlp.linearInequalities.add(InequalityProblem1(X(1), Y(1), dualKey)); Values initialValues;