diff --git a/examples/Data/HS35MOD.QPS b/examples/Data/HS35MOD.QPS new file mode 100644 index 000000000..2fe75ef96 --- /dev/null +++ b/examples/Data/HS35MOD.QPS @@ -0,0 +1,21 @@ +NAME HS35MOD +ROWS + N OBJ.FUNC + G R------1 +COLUMNS + C------1 OBJ.FUNC -.800000e+01 R------1 -.100000e+01 + C------2 OBJ.FUNC -.600000e+01 R------1 -.100000e+01 + C------3 OBJ.FUNC -.400000e+01 R------1 -.200000e+01 +RHS + RHS OBJ.FUNC -.900000e+01 + RHS R------1 -.300000e+01 +RANGES +BOUNDS + FX BOUNDS C------2 0.500000e+00 +QUADOBJ + C------1 C------1 0.400000e+01 + C------1 C------2 0.200000e+01 + C------1 C------3 0.200000e+01 + C------2 C------2 0.400000e+01 + C------3 C------3 0.200000e+01 +ENDATA diff --git a/gtsam_unstable/linear/RawQP.cpp b/gtsam_unstable/linear/RawQP.cpp index 26a83e099..9a2306c55 100644 --- a/gtsam_unstable/linear/RawQP.cpp +++ b/gtsam_unstable/linear/RawQP.cpp @@ -198,6 +198,8 @@ void RawQP::addBound( up[varname_to_key[var_]] = number; else if (type_.compare(std::string("LO")) == 0) lo[varname_to_key[var_]] = number; + else if (type_.compare(std::string("FX")) == 0) + fx[varname_to_key[var_]] = number; else std::cout << "Invalid Bound Type: " << type_ << std::endl; @@ -334,6 +336,9 @@ QP RawQP::makeQP() { for (Key k : keys) { if (std::find(Free.begin(), Free.end(), k) != Free.end()) continue; + if (fx.count(k) == 1) + madeQP.equalities.push_back( + LinearEquality(k, I_1x1, fx[k] * I_1x1, dual_key_num++)); if (up.count(k) == 1) madeQP.inequalities.push_back( LinearInequality(k, I_1x1, up[k], dual_key_num++)); diff --git a/gtsam_unstable/linear/RawQP.h b/gtsam_unstable/linear/RawQP.h index 26ebd7236..75045aa2e 100644 --- a/gtsam_unstable/linear/RawQP.h +++ b/gtsam_unstable/linear/RawQP.h @@ -54,12 +54,13 @@ private: std::string name_; std::unordered_map up; std::unordered_map lo; + std::unordered_map fx; std::vector Free; const bool debug = false; public: RawQP() : - row_to_constraint_v(), E(), IG(), IL(), varNumber(1), b(), ranges(), g(), varname_to_key(), H(), f(), obj_name(), name_(), up(), lo(), Free() { + row_to_constraint_v(), E(), IG(), IL(), varNumber(1), b(), ranges(), g(), varname_to_key(), H(), f(), obj_name(), name_(), up(), lo(), fx(), Free() { } void setName( diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index ab6f22cb2..95272408b 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -303,6 +303,15 @@ TEST(QPSolver, HS35) { CHECK(assert_equal(1.11111111e-01,error_actual, 1e-7)) } +TEST(QPSolver, HS35MOD) { + QP problem = QPSParser("HS35MOD.QPS").Parse(); + VectorValues actualSolution; + boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize(); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(2.50000001e-01,error_actual, 1e-7)) +} + + /* ************************************************************************* */ // Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html QP createTestMatlabQPEx() {