diff --git a/gtsam_unstable/nonlinear/tests/testLinearlyConstrainedNonlinearOptimizer.cpp b/gtsam_unstable/nonlinear/tests/testLinearlyConstrainedNonlinearOptimizer.cpp
new file mode 100644
index 000000000..78b511e2c
--- /dev/null
+++ b/gtsam_unstable/nonlinear/tests/testLinearlyConstrainedNonlinearOptimizer.cpp
@@ -0,0 +1,137 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    testLinearlyConstrainedNonlinearOptimizer.cpp
+ * @brief   Unit tests for LinearlyConstrainedNonlinearOptimizer
+ * @author  Krunal Chande
+ * @author  Duy-Nguyen Ta
+ * @author  Luca Carlone
+ * @date    Dec 15, 2014
+ */
+
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/LinearContainerFactor.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam_unstable/linear/QPSolver.h>
+#include <CppUnitLite/TestHarness.h>
+#include <iostream>
+
+
+namespace gtsam {
+struct LinearlyConstrainedNLP {
+  NonlinearFactorGraph cost;
+  LinearEqualityFactorGraph equalities;
+  LinearInequalityFactorGraph inequalities;
+};
+
+struct LinearlyConstrainedNLPState {
+  Values values;
+  VectorValues duals;
+  bool converged;
+  LinearlyConstrainedNLPState(const Values& initialValues) :
+      values(initialValues), duals(VectorValues()), converged(false) {
+  }
+};
+class LinearlyConstrainedNonLinearOptimizer {
+  LinearlyConstrainedNLP lcNLP_;
+public:
+  LinearlyConstrainedNonLinearOptimizer(const LinearlyConstrainedNLP& lcNLP): lcNLP_(lcNLP) {}
+
+  LinearlyConstrainedNLPState iterate(const LinearlyConstrainedNLPState& state) const {
+    QP qp;
+    qp.cost = lcNLP_.cost.linearize(state.values);
+    qp.equalities = lcNLP_.equalities;
+    qp.inequalities = lcNLP_.inequalities;
+    QPSolver qpSolver(qp);
+    VectorValues delta, duals;
+    boost::tie(delta, duals) = qpSolver.optimize();
+    LinearlyConstrainedNLPState newState;
+    newState.values = state.values.retract(delta);
+    newState.duals = duals;
+    newState.converged = checkConvergence(newState.values, newState.duals);
+    return newState;
+  }
+
+  /**
+   * Main optimization function.
+   */
+  std::pair<Values, VectorValues> optimize(const Values& initialValues) const {
+    LinearlyConstrainedNLPState state(initialValues);
+    while(!state.converged){
+      state = iterate(state);
+    }
+
+    return std::make_pair(initialValues, VectorValues());
+  }
+};
+}
+
+using namespace std;
+using namespace gtsam::symbol_shorthand;
+using namespace gtsam;
+const double tol = 1e-10;
+//******************************************************************************
+TEST(LinearlyConstrainedNonlinearOptimizer, Problem1 ) {
+
+  // build a quadratic Objective function x1^2 - x1*x2 + x2^2 - 3*x1 + 5
+  // Note the Hessian encodes:
+  //        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 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10
+  HessianFactor lf(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), 3.0 * ones(1),
+      2.0 * ones(1, 1), zero(1), 10.0);
+
+  // build linear inequalities
+  LinearInequalityFactorGraph inequalities;
+  inequalities.push_back(LinearInequality(X(1), ones(1,1), X(2), ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
+  inequalities.push_back(LinearInequality(X(1), -ones(1,1), 0, 1));                 // -x1     <= 0
+  inequalities.push_back(LinearInequality(X(2), -ones(1,1), 0, 2));                 //    -x2  <= 0
+  inequalities.push_back(LinearInequality(X(1), ones(1,1), 1.5, 3));                // x1      <= 3/2
+
+  // Instantiate LinearlyConstrainedNLP, pretending that the cost is not quadratic
+  // (LinearContainerFactor makes a linear factor behave like a nonlinear one)
+  LinearlyConstrainedNLP lcNLP;
+  lcNLP.cost.add(LinearContainerFactor(lf));
+  lcNLP.inequalities = inequalities;
+
+  // Compare against a QP
+  QP qp;
+  qp.cost.add(lf);
+  qp.inequalities = inequalities;
+
+  // instantiate QPsolver
+  QPSolver qpSolver(qp);
+  // create initial values for optimization
+  VectorValues initialVectorValues;
+  initialVectorValues.insert(X(1), zero(1));
+  initialVectorValues.insert(X(2), zero(1));
+  VectorValues expectedSolution = qpSolver.optimize(initialVectorValues).first;
+
+  // instantiate LinearlyConstrainedNonLinearOptimizer
+  LinearlyConstrainedNonLinearOptimizer lcNLPSolver(lcNLP);
+  // create initial values for optimization
+  Values initialValues;
+  initialValues.insert(X(1), 0.0);
+  initialValues.insert(X(2), 0.0);
+  Values actualSolution = lcNLPSolver.optimize(initialValues).first;
+
+
+  DOUBLES_EQUAL(expectedSolution.at(X(1))[0], actualSolution.at<double>(X(1)), tol);
+  DOUBLES_EQUAL(expectedSolution.at(X(2))[0], actualSolution.at<double>(X(2)), tol);
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************
+