clean up headers, add banners, refactor implementation to cpp

gtsam_unstable/linear/LP.h

@@ -1,3 +1,14 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
 /**
  * @file     LP.h
  * @brief    Struct used to hold a Linear Programming Problem
@@ -9,6 +20,7 @@
 
 #include <gtsam_unstable/linear/LinearCost.h>
 #include <gtsam_unstable/linear/EqualityFactorGraph.h>
+#include <gtsam_unstable/linear/InequalityFactorGraph.h>
 
 #include <string>
 

gtsam_unstable/linear/LPInitSolver.cpp

@@ -0,0 +1,109 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file     LPInitSolver.h
+ * @brief    This finds a feasible solution for an LP problem
+ * @author   Duy Nguyen Ta
+ * @author   Ivan Dario Jimenez
+ * @date     6/16/16
+ */
+
+#include <gtsam_unstable/linear/LPInitSolver.h>
+#include <gtsam_unstable/linear/LPSolver.h>
+
+namespace gtsam {
+
+/******************************************************************************/
+VectorValues LPInitSolver::solve() const {
+  // Build the graph to solve for the initial value of the initial problem
+  GaussianFactorGraph::shared_ptr initOfInitGraph = buildInitOfInitGraph();
+  VectorValues x0 = initOfInitGraph->optimize();
+  double y0 = compute_y0(x0);
+  Key yKey = maxKey(lp_) + 1;  // the unique key for y0
+  VectorValues xy0(x0);
+  xy0.insert(yKey, Vector::Constant(1, y0));
+
+  // Formulate and solve the initial LP
+  LP::shared_ptr initLP = buildInitialLP(yKey);
+
+  // solve the initialLP
+  LPSolver lpSolveInit(*initLP);
+  VectorValues xyInit = lpSolveInit.optimize(xy0).first;
+  double yOpt = xyInit.at(yKey)[0];
+  xyInit.erase(yKey);
+  if (yOpt > 0)
+    throw InfeasibleOrUnboundedProblem();
+  else
+    return xyInit;
+}
+
+/******************************************************************************/
+LP::shared_ptr LPInitSolver::buildInitialLP(Key yKey) const {
+  LP::shared_ptr initLP(new LP());
+  initLP->cost = LinearCost(yKey, I_1x1);  // min y
+  initLP->equalities = lp_.equalities;     // st. Ax = b
+  initLP->inequalities =
+      addSlackVariableToInequalities(yKey,
+                                     lp_.inequalities);  // Cx-y <= d
+  return initLP;
+}
+
+/******************************************************************************/
+GaussianFactorGraph::shared_ptr LPInitSolver::buildInitOfInitGraph() const {
+  // first add equality constraints Ax = b
+  GaussianFactorGraph::shared_ptr initGraph(
+      new GaussianFactorGraph(lp_.equalities));
+
+  // create factor ||x||^2 and add to the graph
+  const KeyDimMap& constrainedKeyDim = lp_.constrainedKeyDimMap();
+  for (Key key : constrainedKeyDim | boost::adaptors::map_keys) {
+    size_t dim = constrainedKeyDim.at(key);
+    initGraph->push_back(
+        JacobianFactor(key, Matrix::Identity(dim, dim), Vector::Zero(dim)));
+  }
+  return initGraph;
+}
+
+/******************************************************************************/
+double LPInitSolver::compute_y0(const VectorValues& x0) const {
+  double y0 = -std::numeric_limits<double>::infinity();
+  for (const auto& factor : lp_.inequalities) {
+    double error = factor->error(x0);
+    if (error > y0) y0 = error;
+  }
+  return y0;
+}
+
+/******************************************************************************/
+std::vector<std::pair<Key, Matrix> > LPInitSolver::collectTerms(
+    const LinearInequality& factor) const {
+  std::vector<std::pair<Key, Matrix> > terms;
+  for (Factor::const_iterator it = factor.begin(); it != factor.end(); it++) {
+    terms.push_back(make_pair(*it, factor.getA(it)));
+  }
+  return terms;
+}
+
+/******************************************************************************/
+InequalityFactorGraph LPInitSolver::addSlackVariableToInequalities(
+    Key yKey, const InequalityFactorGraph& inequalities) const {
+  InequalityFactorGraph slackInequalities;
+  for (const auto& factor : lp_.inequalities) {
+    std::vector<std::pair<Key, Matrix> > 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(LinearInequality(terms, d, factor->dualKey()));
+  }
+  return slackInequalities;
+}
+
+}

gtsam_unstable/linear/LPInitSolver.h

@@ -1,3 +1,14 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
 /**
  * @file     LPInitSolver.h
  * @brief    This LPInitSolver implements the strategy in Matlab.
@@ -8,6 +19,7 @@
 
 #pragma once
 
+#include <gtsam_unstable/linear/LP.h>
 #include <gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h>
 #include <gtsam_unstable/linear/QPSolver.h>
 #include <CppUnitLite/Test.h>
@@ -44,102 +56,35 @@ private:
   const LP& lp_;
 
 public:
-  LPInitSolver(const LP& lp) : lp_(lp) {
-  }
+  /// Construct with an LP problem
+  LPInitSolver(const LP& lp) : lp_(lp) {}
 
-  virtual ~LPInitSolver() {
-  }
-
-  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();
-    double y0 = compute_y0(x0);
-    Key yKey = maxKey(lp_) + 1; // the unique key for y0
-    VectorValues xy0(x0);
-    xy0.insert(yKey, Vector::Constant(1, y0));
-
-    // Formulate and solve the initial LP
-    LP::shared_ptr initLP = buildInitialLP(yKey);
-
-    // solve the initialLP
-    LPSolver lpSolveInit(*initLP);
-    VectorValues xyInit = lpSolveInit.optimize(xy0).first;
-    double yOpt = xyInit.at(yKey)[0];
-    xyInit.erase(yKey);
-    if (yOpt > 0)
-      throw InfeasibleOrUnboundedProblem();
-    else
-      return xyInit;
-  }
+  ///@return a feasible initialization point
+  VectorValues solve() const;
 
 private:
   /// build initial LP
-  LP::shared_ptr buildInitialLP(Key yKey) const {
-    LP::shared_ptr initLP(new LP());
-    initLP->cost = LinearCost(yKey, I_1x1); // min y
-    initLP->equalities = lp_.equalities; // st. Ax = b
-    initLP->inequalities = addSlackVariableToInequalities(yKey,
-        lp_.inequalities); // Cx-y <= d
-    return initLP;
-  }
+  LP::shared_ptr buildInitialLP(Key yKey) const;
 
   /**
    * Build the following graph to solve for an initial value of the initial problem
    *    min   ||x||^2    s.t.   Ax = b
    */
-  GaussianFactorGraph::shared_ptr buildInitOfInitGraph() const {
-    // first add equality constraints Ax = b
-    GaussianFactorGraph::shared_ptr initGraph(
-        new GaussianFactorGraph(lp_.equalities));
-
-    // create factor ||x||^2 and add to the graph
-    const KeyDimMap& constrainedKeyDim = lp_.constrainedKeyDimMap();
-    for (Key key : constrainedKeyDim | boost::adaptors::map_keys) {
-      size_t dim = constrainedKeyDim.at(key);
-      initGraph->push_back(
-          JacobianFactor(key, Matrix::Identity(dim, dim), Vector::Zero(dim)));
-    }
-    return initGraph;
-  }
+  GaussianFactorGraph::shared_ptr buildInitOfInitGraph() const;
 
   /// y = max_j ( Cj*x0  - dj )  -- due to the inequality constraints y >= Cx - d
-  double compute_y0(const VectorValues& x0) const {
-    double y0 = -std::numeric_limits<double>::infinity();
-    for (const auto& factor : lp_.inequalities) {
-      double error = factor->error(x0);
-      if (error > y0)
-        y0 = error;
-    }
-    return y0;
-  }
+  double compute_y0(const VectorValues& x0) const;
 
   /// Collect all terms of a factor into a container.
   std::vector<std::pair<Key, Matrix> > collectTerms(
-      const LinearInequality& factor) const {
-    std::vector < std::pair<Key, Matrix> > terms;
-    for (Factor::const_iterator it = factor.begin(); it != factor.end(); it++) {
-      terms.push_back(make_pair(*it, factor.getA(it)));
-    }
-    return terms;
-  }
+      const LinearInequality& factor) const;
 
   /// Turn Cx <= d into Cx - y <= d factors
   InequalityFactorGraph addSlackVariableToInequalities(Key yKey,
-      const InequalityFactorGraph& inequalities) const {
-    InequalityFactorGraph slackInequalities;
-    for (const auto& factor : lp_.inequalities) {
-      std::vector < std::pair<Key, Matrix> > 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(
-          LinearInequality(terms, d, factor->dualKey()));
-    }
-    return slackInequalities;
-  }
+      const InequalityFactorGraph& inequalities) const;
 
   // friend class for unit-testing private methods
-  FRIEND_TEST(LPInitSolver, initialization)
-  ;
+  FRIEND_TEST(LPInitSolver, initialization);
 };
+
 }

gtsam_unstable/linear/LPSolver.cpp

@@ -1,3 +1,14 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
 /**
  * @file     LPSolver.cpp
  * @brief    
@@ -7,9 +18,8 @@
  */
 
 #include <gtsam_unstable/linear/LPSolver.h>
-#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
-#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam_unstable/linear/LPInitSolver.h>
+#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
 
 namespace gtsam {
 //******************************************************************************

gtsam_unstable/linear/LPSolver.h

@@ -1,3 +1,14 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
 /**
  * @file     LPSolver.h
  * @brief    Class used to solve Linear Programming Problems as defined in LP.h
@@ -14,11 +25,8 @@
 #include <gtsam_unstable/linear/LinearCost.h>
 #include <gtsam/linear/VectorValues.h>
 
-#include <boost/range/adaptor/map.hpp>
-
 namespace gtsam {
 
-
 class LPSolver: public ActiveSetSolver {
   const LP &lp_; //!< the linear programming problem
 public:

gtsam_unstable/linear/QPInitSolver.h

@@ -1,6 +1,17 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
 /**
  * @file     QPInitSolver.h
- * @brief    This QPInitSolver implements the strategy in Matlab.
+ * @brief    This finds a feasible solution for a QP problem
  * @author   Duy Nguyen Ta
  * @author   Ivan Dario Jimenez
  * @date     6/16/16
@@ -24,9 +35,7 @@ public:
     /// Constructor with a QP problem
     QPInitSolver(const QP& qp) : qp_(qp) {}
 
-    /**
-     * @return a feasible initialization point
-     */
+    ///@return a feasible initialization point
     VectorValues solve() const {
       // Make an LP with any linear cost function. It doesn't matter for
       // initialization.

gtsam_unstable/linear/QPSolver.cpp

@@ -16,13 +16,9 @@
  * @author  Duy-Nguyen Ta
  */
 
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/inference/FactorGraph-inst.h>
 #include <gtsam_unstable/linear/QPSolver.h>
-#include <gtsam_unstable/linear/LPSolver.h>
-#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
 #include <gtsam_unstable/linear/QPInitSolver.h>
-#include <boost/range/adaptor/map.hpp>
+#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
 
 using namespace std;