Merge remote-tracking branch 'origin/feature/LPSolver' into feature/LPSolver

gtsam_unstable/linear/EqualityFactorGraph.h

@@ -22,18 +22,16 @@
 #include <gtsam_unstable/linear/LinearEquality.h>
 
 namespace gtsam {
+
 /**
- * This class is used to represent an equality constraint on
+ * Collection of all Linear Equality constraints Ax=b of
- * a Programming problem of the form Ax = b.
+ * a Programming problem as a Factor Graph
  */
 class EqualityFactorGraph: public FactorGraph<LinearEquality> {
 public:
   typedef boost::shared_ptr<EqualityFactorGraph> shared_ptr;
 
-  /** compute error of a guess.
+  /// Compute error of a guess.
-   * TODO: This code is duplicated in GaussianFactorGraph and NonlinearFactorGraph!!
-   * Remove it!
-   */
   double error(const VectorValues& x) const {
     double total_error = 0.;
     for (const sharedFactor& factor : *this) {

gtsam_unstable/linear/InequalityFactorGraph.h

@@ -25,6 +25,10 @@
 
 namespace gtsam {
 
+/**
+ * Collection of all Linear Inequality constraints Ax-b <= 0 of
+ * a Programming problem as a Factor Graph
+ */
 class InequalityFactorGraph: public FactorGraph<LinearInequality> {
 private:
   typedef FactorGraph<LinearInequality> Base;

gtsam_unstable/linear/LinearInequality.h

@@ -14,6 +14,7 @@
  * @brief   LinearInequality derived from Base with constrained noise model
  * @date    Nov 27, 2014
  * @author  Duy-Nguyen Ta
+ * @author  Ivan Dario Jimenez
  */
 
 #pragma once
@@ -26,7 +27,7 @@ namespace gtsam {
 typedef Eigen::RowVectorXd RowVector;
 
 /**
- * This class defines a linear inequality constraint g(x)<=0,
+ * This class defines a linear inequality constraint Ax-b <= 0,
  * inheriting JacobianFactor with the special Constrained noise model
  */
 class LinearInequality: public JacobianFactor {

gtsam_unstable/linear/RawQP.cpp

@@ -225,31 +225,31 @@ QP RawQP::makeQP() {
   madeQP.cost.push_back(obj);
 
   for (auto kv : E) {
-    std::vector < std::pair<Key, Matrix11> > KeyMatPair;
+    std::map<Key, Matrix11> keyMatrixMap;
     for (auto km : kv.second) {
-      KeyMatPair.push_back(km);
+      keyMatrixMap.insert(km);
     }
     madeQP.equalities.push_back(
-        LinearEquality(KeyMatPair, b[kv.first] * I_1x1, dual_key_num++));
+        LinearEquality(keyMatrixMap, b[kv.first] * I_1x1, dual_key_num++));
   }
 
   for (auto kv : IG) {
-    std::vector < std::pair<Key, Matrix11> > KeyMatPair;
+    std::map<Key, Matrix11> keyMatrixMap;
     for (auto km : kv.second) {
       km.second = -km.second;
-      KeyMatPair.push_back(km);
+      keyMatrixMap.insert(km);
     }
     madeQP.inequalities.push_back(
-        LinearInequality(KeyMatPair, -b[kv.first], dual_key_num++));
+        LinearInequality(keyMatrixMap, -b[kv.first], dual_key_num++));
   }
 
   for (auto kv : IL) {
-    std::vector < std::pair<Key, Matrix11> > KeyMatPair;
+    std::map<Key, Matrix11> keyMatrixMap;
     for (auto km : kv.second) {
-      KeyMatPair.push_back(km);
+      keyMatrixMap.insert(km);
     }
     madeQP.inequalities.push_back(
-        LinearInequality(KeyMatPair, b[kv.first], dual_key_num++));
+        LinearInequality(keyMatrixMap, b[kv.first], dual_key_num++));
   }
 
   for (Key k : keys) {

gtsam_unstable/linear/tests/testQPSolver.cpp

@@ -222,9 +222,9 @@ pair<QP, QP> testParser(QPSParser parser) {
   // 2x + y >= 2
   // -x + 2y <= 6
   expectedqp.inequalities.push_back(
-      LinearInequality(X2, -I_1x1, X1, -2.0 * I_1x1, -2, 0));
+      LinearInequality(X1, -2.0 * I_1x1, X2, -I_1x1, -2, 0));
   expectedqp.inequalities.push_back(
-      LinearInequality(X2, 2.0 * I_1x1, X1, -I_1x1, 6, 1));
+      LinearInequality(X1, -I_1x1, X2, 2.0 * I_1x1, 6, 1));
   // x<= 20
   expectedqp.inequalities.push_back(LinearInequality(X1, I_1x1, 20, 4));
   //x >= 0
@@ -237,23 +237,21 @@ pair<QP, QP> testParser(QPSParser parser) {
 
 TEST(QPSolver, ParserSyntaticTest) {
   auto expectedActual = testParser(QPSParser("QPExample.QPS"));
-  CHECK(
+  CHECK(assert_equal(expectedActual.first.cost, expectedActual.second.cost,
-      assert_equal(expectedActual.first.cost, expectedActual.second.cost, 1e-7));
+                     1e-7));
-  CHECK(
+  CHECK(assert_equal(expectedActual.first.inequalities,
-      assert_equal(expectedActual.first.inequalities,
+                     expectedActual.second.inequalities, 1e-7));
-          expectedActual.second.inequalities, 1e-7));
+  CHECK(assert_equal(expectedActual.first.equalities,
-  CHECK(
+                     expectedActual.second.equalities, 1e-7));
-      assert_equal(expectedActual.first.equalities,
-          expectedActual.second.equalities, 1e-7));
 }
 
 TEST(QPSolver, ParserSemanticTest) {
   auto expected_actual = testParser(QPSParser("QPExample.QPS"));
   VectorValues actualSolution, expectedSolution;
-  boost::tie(expectedSolution, boost::tuples::ignore) = QPSolver(
+  boost::tie(expectedSolution, boost::tuples::ignore) =
-      expected_actual.first).optimize();
+      QPSolver(expected_actual.first).optimize();
-  boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(
+  boost::tie(actualSolution, boost::tuples::ignore) =
-      expected_actual.second).optimize();
+      QPSolver(expected_actual.second).optimize();
   CHECK(assert_equal(actualSolution, expectedSolution, 1e-7));
 }