diff --git a/.cproject b/.cproject
index 8fc54f11b..eb83cb85b 100644
--- a/.cproject
+++ b/.cproject
@@ -317,7 +317,6 @@
make
-
clean
true
true
@@ -477,6 +476,7 @@
make
+
testBayesTree.run
true
false
@@ -484,7 +484,6 @@
make
-
testSymbolicBayesNet.run
true
false
@@ -492,6 +491,7 @@
make
+
testSymbolicFactorGraph.run
true
false
@@ -683,6 +683,7 @@
make
+
testGraph.run
true
false
@@ -738,7 +739,6 @@
make
-
testSimulated2D.run
true
false
@@ -786,6 +786,7 @@
make
+
testErrors.run
true
false
@@ -793,6 +794,7 @@
make
+
testDSF.run
true
true
@@ -806,6 +808,14 @@
true
true
+
+make
+
+testConstraintOptimizer.run
+true
+true
+true
+
make
-j2
diff --git a/cpp/ConstraintOptimizer.cpp b/cpp/ConstraintOptimizer.cpp
new file mode 100644
index 000000000..d811817bf
--- /dev/null
+++ b/cpp/ConstraintOptimizer.cpp
@@ -0,0 +1,39 @@
+/**
+ * @file ConstraintOptimizer.cpp
+ * @author Alex Cunningham
+ */
+
+#include
+
+using namespace std;
+using namespace gtsam;
+
+
+/* ************************************************************************* */
+pair 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));
+}
+
diff --git a/cpp/ConstraintOptimizer.h b/cpp/ConstraintOptimizer.h
new file mode 100644
index 000000000..d9c51ab1e
--- /dev/null
+++ b/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
+
+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 solveCQP(const Matrix& B, const Matrix& A,
+ const Vector& g, const Vector& h);
+
+} // \namespace gtsam
+
+
diff --git a/cpp/Makefile.am b/cpp/Makefile.am
index 2872d0353..f1fe84634 100644
--- a/cpp/Makefile.am
+++ b/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
diff --git a/cpp/testConstraintOptimizer b/cpp/testConstraintOptimizer
new file mode 100755
index 000000000..6ad048061
--- /dev/null
+++ b/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
diff --git a/cpp/testConstraintOptimizer.cpp b/cpp/testConstraintOptimizer.cpp
new file mode 100644
index 000000000..5ef1a9711
--- /dev/null
+++ b/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
+#include
+
+#include
+
+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); }
+/* ************************************************************************* */
diff --git a/cpp/testMatrix.cpp b/cpp/testMatrix.cpp
index 76bd8505e..05ca90bdd 100644
--- a/cpp/testMatrix.cpp
+++ b/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;