Added a separate ConstraintOptimizer, and moved CQP test from testMatrix to testConstraintOptimizer

.cproject

@@ -317,7 +317,6 @@
 </target>
 <target name="clean" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>clean</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -477,6 +476,7 @@
 </target>
 <target name="testBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testBayesTree.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -484,7 +484,6 @@
 </target>
 <target name="testSymbolicBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testSymbolicBayesNet.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -492,6 +491,7 @@
 </target>
 <target name="testSymbolicFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testSymbolicFactorGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -683,6 +683,7 @@
 </target>
 <target name="testGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testGraph.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -738,7 +739,6 @@
 </target>
 <target name="testSimulated2D.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments/>
 <buildTarget>testSimulated2D.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -786,6 +786,7 @@
 </target>
 <target name="testErrors.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testErrors.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>false</useDefaultCommand>
@@ -793,6 +794,7 @@
 </target>
 <target name="testDSF.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments/>
 <buildTarget>testDSF.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -806,6 +808,14 @@
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
+<target name="testConstraintOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<buildCommand>make</buildCommand>
+<buildArguments/>
+<buildTarget>testConstraintOptimizer.run</buildTarget>
+<stopOnError>true</stopOnError>
+<useDefaultCommand>true</useDefaultCommand>
+<runAllBuilders>true</runAllBuilders>
+</target>
 <target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
 <buildArguments>-j2</buildArguments>

cpp/ConstraintOptimizer.cpp

@@ -0,0 +1,39 @@
+/**
+ * @file ConstraintOptimizer.cpp
+ * @author Alex Cunningham
+ */
+
+#include <ConstraintOptimizer.h>
+
+using namespace std;
+using namespace gtsam;
+
+
+/* ************************************************************************* */
+pair<Vector, Vector> gtsam::solveCQP(const Matrix& B, const Matrix& A,
+								     const Vector& g, const Vector& h) {
+	// find the dimensions
+	size_t n = B.size1(), p = A.size2();
+
+	// verify matrices
+	if (n != B.size2())
+		throw invalid_argument("solveCQP: B matrix is not square!");
+	if (A.size1() != n)
+		throw invalid_argument("solveCQP: A matrix needs m = B.size1()");
+
+	// construct G matrix
+	Matrix G = zeros(n+p, n+p);
+	insertSub(G, B, 0, 0);
+	insertSub(G, A, 0, n);
+	insertSub(G, trans(A), n, 0);
+
+	Vector rhs = zero(n+p);
+	subInsert(rhs, -1.0*g, 0);
+	subInsert(rhs, -1.0*h, n);
+
+	// solve the system with the LDL solver
+	Vector fullResult = solve_ldl(G, rhs);
+
+	return make_pair(sub(fullResult, 0, n), sub(fullResult, n, n+p));
+}
+

cpp/ConstraintOptimizer.h

@@ -0,0 +1,33 @@
+/**
+ * @file ConstraintOptimizer.h
+ * @brief Utilities and classes required for solving Constrained Problems
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include <Matrix.h>
+
+namespace gtsam {
+
+	/**
+	 * Basic function that uses LDL factorization to solve a
+	 * KKT system (state and lagrange multipliers) of the form:
+	 * Gd=b, where
+	 * G = [B  A]  d = [ x ] b = - [g]
+	 *     [A' 0]      [lam]       [h]
+	 * B = Hessian of Lagragian function
+	 * A = Gradient of constraints
+	 * x = state
+	 * lam = vector of lagrange mulipliers
+	 * g = gradient of f(x) evaluated a point
+	 * h = current value of c(x)
+	 *
+	 * @return pair of state and lambas
+	 */
+	std::pair<Vector, Vector> solveCQP(const Matrix& B, const Matrix& A,
+									   const Vector& g, const Vector& h);
+
+} // \namespace gtsam
+
+

cpp/Makefile.am

@@ -151,9 +151,13 @@ testNonlinearEquality_SOURCES   = testNonlinearEquality.cpp
 testNonlinearEquality_LDADD     = libgtsam.la
 
 # SQP
-check_PROGRAMS += testSQP 
+headers += ConstraintOptimizer.h
+sources += ConstraintOptimizer.cpp
+check_PROGRAMS += testSQP testConstraintOptimizer
 testSQP_SOURCES                 = $(example) testSQP.cpp
 testSQP_LDADD                   = libgtsam.la
+testConstraintOptimizer_SOURCES = $(example) testConstraintOptimizer.cpp
+testConstraintOptimizer_LDADD   = libgtsam.la
 
 # geometry
 headers += Lie.h Lie-inl.h
@@ -294,7 +298,7 @@ endif
 include_HEADERS = $(headers)
 
 AM_CXXFLAGS += -I.. 
-AM_LDFLAGS = -L../CppUnitLite -lCppUnitLite ../ldl/libldl.a $(BOOST_LDFLAGS) $(boost_serialization)
+AM_LDFLAGS = -L../CppUnitLite -lCppUnitLite $(BOOST_LDFLAGS) $(boost_serialization)
 
 # adding cblas implementation - split into a default linux version using the 
 # autotools script, and a mac version that is hardcoded 

cpp/testConstraintOptimizer

@@ -0,0 +1,148 @@
+#! /bin/sh
+
+# testConstraintOptimizer - temporary wrapper script for .libs/testConstraintOptimizer
+# Generated by ltmain.sh (GNU libtool) 2.2.6 Debian-2.2.6a-4
+#
+# The testConstraintOptimizer program cannot be directly executed until all the libtool
+# libraries that it depends on are installed.
+#
+# This wrapper script should never be moved out of the build directory.
+# If it is, it will not operate correctly.
+
+# Sed substitution that helps us do robust quoting.  It backslashifies
+# metacharacters that are still active within double-quoted strings.
+Xsed='/bin/sed -e 1s/^X//'
+sed_quote_subst='s/\([`"$\\]\)/\\\1/g'
+
+# Be Bourne compatible
+if test -n "${ZSH_VERSION+set}" && (emulate sh) >/dev/null 2>&1; then
+  emulate sh
+  NULLCMD=:
+  # Zsh 3.x and 4.x performs word splitting on ${1+"$@"}, which
+  # is contrary to our usage.  Disable this feature.
+  alias -g '${1+"$@"}'='"$@"'
+  setopt NO_GLOB_SUBST
+else
+  case `(set -o) 2>/dev/null` in *posix*) set -o posix;; esac
+fi
+BIN_SH=xpg4; export BIN_SH # for Tru64
+DUALCASE=1; export DUALCASE # for MKS sh
+
+# The HP-UX ksh and POSIX shell print the target directory to stdout
+# if CDPATH is set.
+(unset CDPATH) >/dev/null 2>&1 && unset CDPATH
+
+relink_command="(cd /home/alexgc/borg/gtsam/cpp; { test -z \"\${LIBRARY_PATH+set}\" || unset LIBRARY_PATH || { LIBRARY_PATH=; export LIBRARY_PATH; }; }; { test -z \"\${COMPILER_PATH+set}\" || unset COMPILER_PATH || { COMPILER_PATH=; export COMPILER_PATH; }; }; { test -z \"\${GCC_EXEC_PREFIX+set}\" || unset GCC_EXEC_PREFIX || { GCC_EXEC_PREFIX=; export GCC_EXEC_PREFIX; }; }; { test -z \"\${LD_RUN_PATH+set}\" || unset LD_RUN_PATH || { LD_RUN_PATH=; export LD_RUN_PATH; }; }; LD_LIBRARY_PATH=/usr/lib/jvm/java-6-openjdk/jre/lib/amd64/server:/usr/lib/jvm/java-6-openjdk/jre/lib/amd64:/usr/lib/jvm/java-6-openjdk/jre/../lib/amd64:/usr/lib64/xulrunner-addons; export LD_LIBRARY_PATH; PATH=/home/alexgc/ros/ros/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games; export PATH; g++ -g -I/usr/include/ -fPIC -I.. -g -O2 -march=core2 -DNDEBUG -o \$progdir/\$file testConstraintOptimizer.o  -L/home/alexgc/borg/gtsam/CppUnitLite -lCppUnitLite ../ldl/libldl.a ./.libs/libgtsam.so -Wl,-rpath -Wl,/home/alexgc/borg/gtsam/cpp/.libs)"
+
+# This environment variable determines our operation mode.
+if test "$libtool_install_magic" = "%%%MAGIC variable%%%"; then
+  # install mode needs the following variables:
+  generated_by_libtool_version='2.2.6'
+  notinst_deplibs=' libgtsam.la'
+else
+  # When we are sourced in execute mode, $file and $ECHO are already set.
+  if test "$libtool_execute_magic" != "%%%MAGIC variable%%%"; then
+    ECHO="echo"
+    file="$0"
+    # Make sure echo works.
+    if test "X$1" = X--no-reexec; then
+      # Discard the --no-reexec flag, and continue.
+      shift
+    elif test "X`{ $ECHO '\t'; } 2>/dev/null`" = 'X\t'; then
+      # Yippee, $ECHO works!
+      :
+    else
+      # Restart under the correct shell, and then maybe $ECHO will work.
+      exec /bin/sh "$0" --no-reexec ${1+"$@"}
+    fi
+  fi
+
+  # Find the directory that this script lives in.
+  thisdir=`$ECHO "X$file" | $Xsed -e 's%/[^/]*$%%'`
+  test "x$thisdir" = "x$file" && thisdir=.
+
+  # Follow symbolic links until we get to the real thisdir.
+  file=`ls -ld "$file" | /bin/sed -n 's/.*-> //p'`
+  while test -n "$file"; do
+    destdir=`$ECHO "X$file" | $Xsed -e 's%/[^/]*$%%'`
+
+    # If there was a directory component, then change thisdir.
+    if test "x$destdir" != "x$file"; then
+      case "$destdir" in
+      [\\/]* | [A-Za-z]:[\\/]*) thisdir="$destdir" ;;
+      *) thisdir="$thisdir/$destdir" ;;
+      esac
+    fi
+
+    file=`$ECHO "X$file" | $Xsed -e 's%^.*/%%'`
+    file=`ls -ld "$thisdir/$file" | /bin/sed -n 's/.*-> //p'`
+  done
+
+
+  # Usually 'no', except on cygwin/mingw when embedded into
+  # the cwrapper.
+  WRAPPER_SCRIPT_BELONGS_IN_OBJDIR=no
+  if test "$WRAPPER_SCRIPT_BELONGS_IN_OBJDIR" = "yes"; then
+    # special case for '.'
+    if test "$thisdir" = "."; then
+      thisdir=`pwd`
+    fi
+    # remove .libs from thisdir
+    case "$thisdir" in
+    *[\\/].libs ) thisdir=`$ECHO "X$thisdir" | $Xsed -e 's%[\\/][^\\/]*$%%'` ;;
+    .libs )   thisdir=. ;;
+    esac
+  fi
+
+  # Try to get the absolute directory name.
+  absdir=`cd "$thisdir" && pwd`
+  test -n "$absdir" && thisdir="$absdir"
+
+  program=lt-'testConstraintOptimizer'
+  progdir="$thisdir/.libs"
+
+  if test ! -f "$progdir/$program" ||
+     { file=`ls -1dt "$progdir/$program" "$progdir/../$program" 2>/dev/null | /bin/sed 1q`; \
+       test "X$file" != "X$progdir/$program"; }; then
+
+    file="$$-$program"
+
+    if test ! -d "$progdir"; then
+      mkdir "$progdir"
+    else
+      rm -f "$progdir/$file"
+    fi
+
+    # relink executable if necessary
+    if test -n "$relink_command"; then
+      if relink_command_output=`eval $relink_command 2>&1`; then :
+      else
+	echo "$relink_command_output" >&2
+	rm -f "$progdir/$file"
+	exit 1
+      fi
+    fi
+
+    mv -f "$progdir/$file" "$progdir/$program" 2>/dev/null ||
+    { rm -f "$progdir/$program";
+      mv -f "$progdir/$file" "$progdir/$program"; }
+    rm -f "$progdir/$file"
+  fi
+
+  if test -f "$progdir/$program"; then
+    if test "$libtool_execute_magic" != "%%%MAGIC variable%%%"; then
+      # Run the actual program with our arguments.
+
+      exec "$progdir/$program" ${1+"$@"}
+
+      $ECHO "$0: cannot exec $program $*" 1>&2
+      exit 1
+    fi
+  else
+    # The program doesn't exist.
+    $ECHO "$0: error: \`$progdir/$program' does not exist" 1>&2
+    $ECHO "This script is just a wrapper for $program." 1>&2
+    echo "See the libtool documentation for more information." 1>&2
+    exit 1
+  fi
+fi

cpp/testConstraintOptimizer.cpp

@@ -0,0 +1,72 @@
+/**
+ * @file testConstraintOptimizer.cpp
+ * @brief Tests the optimization engine for SQP and BFGS Quadratic programming techniques
+ * @author Alex Cunningham
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <boost/tuple/tuple.hpp>
+
+#include <ConstraintOptimizer.h>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+// Example of a single Constrained QP problem from the matlab testCQP.m file.
+TEST( matrix, CQP_example ) {
+
+	Matrix A = Matrix_(3, 2,
+			-1.0,  -1.0,
+			-2.0,   1.0,
+			 1.0,  -1.0);
+	Matrix At = trans(A),
+		   B = 2.0 * eye(3,3);
+
+	Vector b = Vector_(2, 4.0, -2.0),
+	 	   g = zero(3);
+
+	Matrix G = zeros(5,5);
+	insertSub(G, B, 0, 0);
+	insertSub(G, A, 0, 3);
+	insertSub(G, At, 3, 0);
+
+	Vector rhs = zero(5);
+	subInsert(rhs, -1.0*g, 0);
+	subInsert(rhs, -1.0*b, 3);
+
+	// solve the system with the LDL solver
+	Vector actualFull = solve_ldl(G, rhs);
+	Vector actual = sub(actualFull, 0, 3);
+
+	Vector expected = Vector_(3, 2.0/7.0, 10.0/7.0, -6.0/7.0);
+
+	CHECK(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+TEST( matrix, CQP_example_automatic ) {
+
+	Matrix A = Matrix_(3, 2,
+			-1.0,  -1.0,
+			-2.0,   1.0,
+			 1.0,  -1.0);
+	Matrix At = trans(A),
+		   B = 2.0 * eye(3,3);
+
+	Vector g = zero(3),
+		   h = Vector_(2, 4.0, -2.0);
+
+	Vector actState, actLam;
+	boost::tie(actState, actLam) = solveCQP(B, A, g, h);
+
+	Vector expected = Vector_(3, 2.0/7.0, 10.0/7.0, -6.0/7.0);
+
+	CHECK(assert_equal(expected, actState));
+	CHECK(actLam.size() == 2);
+}
+	
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */

cpp/testMatrix.cpp

@@ -966,39 +966,6 @@ TEST( matrix, LDL_factorization ) {
 
 }
 
-
-/* ************************************************************************* */
-// Example of a single Constrained QP problem from the matlab testCQP.m file.
-TEST( matrix, CQP_example ) {
-
-	Matrix A = Matrix_(3, 2,
-			-1.0,  -1.0,
-			-2.0,   1.0,
-			 1.0,  -1.0);
-	Matrix At = trans(A),
-		   B = 2.0 * eye(3,3);
-
-	Vector b = Vector_(2, 4.0, -2.0),
-	 	   g = zero(3);
-
-	Matrix G = zeros(5,5);
-	insertSub(G, B, 0, 0);
-	insertSub(G, A, 0, 3);
-	insertSub(G, At, 3, 0);
-
-	Vector rhs = zero(5);
-	subInsert(rhs, -1.0*g, 0);
-	subInsert(rhs, -1.0*b, 3);
-
-	// solve the system with the LDL solver
-	Vector actualFull = solve_ldl(G, rhs);
-	Vector actual = sub(actualFull, 0, 3);
-
-	Vector expected = Vector_(3, 2.0/7.0, 10.0/7.0, -6.0/7.0);
-
-	CHECK(assert_equal(expected, actual));
-}
-
 /* ************************************************************************* */
 int main() {
 	TestResult tr;