Added CQP example from the matlab test, changed the linking to avoid warnings

cpp/Makefile.am

@@ -283,7 +283,7 @@ AM_CXXFLAGS = -I$(boost) -fPIC
 lib_LTLIBRARIES = libgtsam.la
 libgtsam_la_SOURCES = $(sources)
 libgtsam_la_CPPFLAGS = $(AM_CXXFLAGS)
-libgtsam_la_LDFLAGS = -version-info $(version) ../ldl/libldl.a -L../colamd -lcolamd # -lboost_serialization-mt
+libgtsam_la_LDFLAGS = -version-info $(version) -L../colamd -lcolamd -L../ldl -lldl # ../ldl/libldl.a -lboost_serialization-mt
 
 # enable debug if --enable-debug is set in configure
 if DEBUG

cpp/testMatrix.cpp

@@ -966,6 +966,39 @@ 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;