Removed LDL, ConstraintOptimizer things, and FusionTupleConfig. Added *.valgrind run commands so that tests can be checked for memory errors. Removed some extraneous stuff in configure.ac
Alex Cunningham
parent
291685e633
commit
f6ada87314
104
.cproject
104
.cproject
|
|
@ -390,6 +390,7 @@
|
|||
</target>
|
||||
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testErrors.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -403,6 +404,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="tests/testSPQRUtil.valgrind" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testSPQRUtil.valgrind</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="build/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -517,6 +526,7 @@
|
|||
</target>
|
||||
<target name="testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testBayesTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -524,6 +534,7 @@
|
|||
</target>
|
||||
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testBinaryBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -571,6 +582,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -578,6 +590,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -585,6 +598,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -664,7 +678,6 @@
|
|||
</target>
|
||||
<target name="testGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -760,7 +773,6 @@
|
|||
</target>
|
||||
<target name="testInference.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testInference.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -768,7 +780,6 @@
|
|||
</target>
|
||||
<target name="testGaussianBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -776,7 +787,6 @@
|
|||
</target>
|
||||
<target name="testGaussianFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -784,7 +794,6 @@
|
|||
</target>
|
||||
<target name="testJunctionTree.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testJunctionTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -792,7 +801,6 @@
|
|||
</target>
|
||||
<target name="testSymbolicBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -800,7 +808,6 @@
|
|||
</target>
|
||||
<target name="testSymbolicFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1016,7 +1023,6 @@
|
|||
</target>
|
||||
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimulated2DOriented.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1056,7 +1062,6 @@
|
|||
</target>
|
||||
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimulated2D.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1064,7 +1069,6 @@
|
|||
</target>
|
||||
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSimulated3D.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -1150,46 +1154,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>all</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="dist" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>dist</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
@ -1286,6 +1250,46 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>clean</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>all</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="dist" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>dist</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
|
|
|
|||
1
README
1
README
|
|
@ -37,7 +37,6 @@ Finally, there are some local libraries built needed in the rest of the code:
|
|||
colamd COLAMD and CCOLAMD by Tim Davis needed for re-ordering
|
||||
CppUnitLite unit test library
|
||||
doc documentation
|
||||
ldl LDL by Tim Davis (will probably go)
|
||||
m4 local M4 macros
|
||||
spqr_mini Core frontal solver from SuiteSparse (will probably go as not LGPL)
|
||||
|
||||
|
|
|
|||
|
|
@ -59,10 +59,6 @@ if USE_LAPACK
|
|||
AM_CPPFLAGS += -DGT_USE_LAPACK
|
||||
endif
|
||||
|
||||
if USE_LDL
|
||||
AM_CPPFLAGS += -DGT_USE_LDL
|
||||
endif
|
||||
|
||||
# On Mac, we compile using the BLAS/LAPACK headers in the Accelerate framework
|
||||
if USE_ACCELERATE_MACOS
|
||||
AM_CPPFLAGS += -I/System/Library/Frameworks/vecLib.framework/Headers
|
||||
|
|
@ -75,9 +71,6 @@ TESTS = $(check_PROGRAMS)
|
|||
AM_DEFAULT_SOURCE_EXT = .cpp
|
||||
AM_LDFLAGS = $(BOOST_LDFLAGS) $(boost_serialization)
|
||||
LDADD = libbase.la ../CppUnitLite/libCppUnitLite.a
|
||||
if USE_LDL
|
||||
LDADD += libldl.la
|
||||
endif
|
||||
|
||||
if USE_BLAS_LINUX
|
||||
AM_LDFLAGS += -lcblas -latlas
|
||||
|
|
@ -99,4 +92,7 @@ endif
|
|||
%.run: % $(LDADD)
|
||||
./$^
|
||||
|
||||
# rule to run executable with valgrind
|
||||
%.valgrind: % $(LDADD)
|
||||
valgrind ./$^
|
||||
#----------------------------------------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -973,90 +973,6 @@ Matrix RtR(const Matrix &A)
|
|||
return trans(LLt(A));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
#ifdef GT_USE_LDL
|
||||
Vector solve_ldl(const Matrix& M, const Vector& rhs) {
|
||||
|
||||
int N = M.size1(); // size of the matrix
|
||||
|
||||
// count the nonzero entries above diagonal
|
||||
double thresh = 1e-9;
|
||||
int nrANZ = 0; // # of nonzeros on diagonal and upper triangular part of A
|
||||
for (int i=0; i<N; ++i) // rows
|
||||
for (int j=i; j<N; ++j) // columns
|
||||
if (fabs(M(i,j)) > thresh)
|
||||
++nrANZ;
|
||||
|
||||
// components to pass in
|
||||
int * Ap = new int[N+1],
|
||||
* Ai = new int[nrANZ];
|
||||
double * Ax = new double[nrANZ],
|
||||
* b = new double[N];
|
||||
|
||||
// set ending value for Ap
|
||||
Ap[N] = nrANZ;
|
||||
|
||||
// copy in the full A matrix to compressed column form
|
||||
int t = 0; // count the elements added
|
||||
for (int j=0; j<N; ++j) { // columns
|
||||
Ap[j] = t; // add to the column indices
|
||||
for (int i=0; i<=j; ++i) { // rows
|
||||
const double& m = M(i,j);
|
||||
if (fabs(m) > thresh) {
|
||||
Ai[t] = i;
|
||||
Ax[t++] = m;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// copy in RHS
|
||||
for (int i = 0; i < N; ++i)
|
||||
b[i] = rhs(i);
|
||||
|
||||
// workspace variables
|
||||
double * D = new double[N],
|
||||
* Y = new double[N];
|
||||
int * Lp = new int [N+1],
|
||||
* Parent = new int [N],
|
||||
* Lnz = new int [N],
|
||||
* Flag = new int [N],
|
||||
* Pattern = new int [N];
|
||||
|
||||
// factorize A into LDL' (P and Pinv not used)
|
||||
LDL_symbolic (N, Ap, Ai, Lp, Parent, Lnz, Flag, NULL, NULL);
|
||||
|
||||
int nrLNZ = Lp[N]; // # of nonzeros below the diagonal of L
|
||||
|
||||
// after getting size of L, initialize storage arrays
|
||||
double * Lx = new double[nrLNZ];
|
||||
int * Li = new int [nrLNZ];
|
||||
|
||||
int d = LDL_numeric (N, Ap, Ai, Ax, Lp, Parent, Lnz, Li, Lx, D, Y, Pattern,
|
||||
Flag, NULL, NULL);
|
||||
|
||||
if (d == N) {
|
||||
/* solve Ax=b, overwriting b with the solution x */
|
||||
LDL_lsolve (N, b, Lp, Li, Lx) ;
|
||||
LDL_dsolve (N, b, D) ;
|
||||
LDL_ltsolve (N, b, Lp, Li, Lx) ;
|
||||
} else {
|
||||
throw runtime_error("ldl_numeric failed");
|
||||
}
|
||||
|
||||
// copy solution out
|
||||
Vector result = Vector_(N, b);
|
||||
|
||||
// cleanup
|
||||
delete[] Ap; delete[] Ai;
|
||||
delete[] Ax; delete[] b;
|
||||
|
||||
delete[] Lx; delete[] D; delete[] Y;
|
||||
delete[] Li; delete[] Lp; delete[] Parent; delete[] Lnz; delete[] Flag; delete[] Pattern;
|
||||
|
||||
return result;
|
||||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
* This is not ultra efficient, but not terrible, either.
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -388,11 +388,6 @@ Matrix RtR(const Matrix& A);
|
|||
/** Return the inverse of a S.P.D. matrix. Inversion is done via Cholesky decomposition. */
|
||||
Matrix cholesky_inverse(const Matrix &A);
|
||||
|
||||
/** Solve Ax=b with S.P.D. matrix using Davis' LDL code */
|
||||
#ifdef GT_USE_LDL
|
||||
Vector solve_ldl(const Matrix& A, const Vector& b);
|
||||
#endif
|
||||
|
||||
/**
|
||||
* Numerical exponential map, naive approach, not industrial strength !!!
|
||||
* @param A matrix to exponentiate
|
||||
|
|
|
|||
30
configure.ac
30
configure.ac
|
|
@ -14,7 +14,7 @@ AC_CONFIG_SRCDIR([base/DSFVector.cpp])
|
|||
AC_CONFIG_SRCDIR([geometry/Cal3_S2.cpp])
|
||||
AC_CONFIG_SRCDIR([inference/SymbolicFactor.cpp])
|
||||
AC_CONFIG_SRCDIR([linear/GaussianFactor.cpp])
|
||||
AC_CONFIG_SRCDIR([nonlinear/ConstraintOptimizer.cpp])
|
||||
AC_CONFIG_SRCDIR([nonlinear/NonlinearOptimizer.cpp])
|
||||
AC_CONFIG_SRCDIR([slam/pose2SLAM.cpp])
|
||||
AC_CONFIG_SRCDIR([tests/testSQP.cpp])
|
||||
AC_CONFIG_SRCDIR([wrap/wrap.cpp])
|
||||
|
|
@ -49,9 +49,6 @@ case $host_os in
|
|||
;;
|
||||
esac
|
||||
|
||||
# search for a blas implementation. Kai: this can not be easily turned off for Mac.
|
||||
# AX_BLAS(true)
|
||||
|
||||
# enable BLAS with general purpose script
|
||||
AC_ARG_ENABLE([blas],
|
||||
[ --enable-blas Enable external BLAS library],
|
||||
|
|
@ -92,17 +89,6 @@ AC_ARG_ENABLE([spqr],
|
|||
|
||||
AM_CONDITIONAL([USE_SPQR], [test x$spqr = xtrue])
|
||||
|
||||
# enable using LDL library from SuiteSparse
|
||||
AC_ARG_ENABLE([ldl],
|
||||
[ --enable-ldl Enable LDL library support],
|
||||
[case "${enableval}" in
|
||||
yes) ldl=true ;;
|
||||
no) ldl=false ;;
|
||||
*) AC_MSG_ERROR([bad value ${enableval} for --enable-ldl]) ;;
|
||||
esac],[ldl=false])
|
||||
|
||||
AM_CONDITIONAL([USE_LDL], [test x$ldl = xtrue])
|
||||
|
||||
# enable profiling
|
||||
AC_ARG_ENABLE([profiling],
|
||||
[ --enable-profiling Enable profiling],
|
||||
|
|
@ -128,10 +114,6 @@ AM_CONDITIONAL([ENABLE_SERIALIZATION], [test x$serialization = xtrue])
|
|||
# Checks for programs.
|
||||
AC_PROG_CXX
|
||||
AC_PROG_CC
|
||||
# FIXME: Need to use boost macros to get serialization library linked
|
||||
#AX_BOOST_BASE([1.37.0])
|
||||
#AX_BOOST_SERIALIZATION
|
||||
#AX_BOOST_BASE([1.33]) # does not work on windows, even after compiling & installing boost manually
|
||||
|
||||
# Checks for libraries.
|
||||
LT_INIT
|
||||
|
|
@ -169,15 +151,5 @@ AC_ARG_WITH([boost],
|
|||
[--with-boost has to be specified])
|
||||
])
|
||||
AC_SUBST([boost])
|
||||
|
||||
# ask for boost serialization
|
||||
#AC_ARG_WITH([boost_serialization],
|
||||
# [AS_HELP_STRING([--with-boost-serialization],
|
||||
# [(optional) use the Serialization library from boost - specify the library linking command with the full name of the library
|
||||
# e.g. --with-boost-serialization=-lboost_serialization-gcc-mt-d-1_33_1])],
|
||||
# [AC_DEFINE([HAVE_BOOST_SERIALIZATION], ["<boost/foreach.hpp>"], [boost serialization flag])
|
||||
#
|
||||
#AC_SUBST([boost_serialization])
|
||||
|
||||
|
||||
AC_OUTPUT
|
||||
|
|
|
|||
|
|
@ -22,12 +22,12 @@ AM_LDFLAGS = $(BOOST_LDFLAGS)
|
|||
AM_CPPFLAGS = -I$(boost) -I$(top_srcdir)/..
|
||||
LDADD = ../libgtsam.la
|
||||
AM_DEFAULT_SOURCE_EXT = .cpp
|
||||
if USE_LDL
|
||||
LDADD += libldl.la
|
||||
endif
|
||||
|
||||
# rule to run an executable
|
||||
%.run: % $(LDADD)
|
||||
./$^
|
||||
|
||||
# rule to run executable with valgrind
|
||||
%.valgrind: % $(LDADD)
|
||||
valgrind ./$^
|
||||
#----------------------------------------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -50,12 +50,12 @@ TESTS = $(check_PROGRAMS)
|
|||
AM_LDFLAGS = $(BOOST_LDFLAGS) $(boost_serialization)
|
||||
LDADD = libgeometry.la ../base/libbase.la ../CppUnitLite/libCppUnitLite.a
|
||||
AM_DEFAULT_SOURCE_EXT = .cpp
|
||||
if USE_LDL
|
||||
LDADD += libldl.la
|
||||
endif
|
||||
|
||||
# rule to run an executable
|
||||
%.run: % $(LDADD)
|
||||
./$^
|
||||
|
||||
# rule to run executable with valgrind
|
||||
%.valgrind: % $(LDADD)
|
||||
valgrind ./$^
|
||||
#----------------------------------------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -69,14 +69,15 @@ AM_LDFLAGS = $(BOOST_LDFLAGS) $(boost_serialization)
|
|||
LDADD = libinference.la ../base/libbase.la
|
||||
LDADD += ../CppUnitLite/libCppUnitLite.a ../colamd/libcolamd.la
|
||||
AM_DEFAULT_SOURCE_EXT = .cpp
|
||||
if USE_LDL
|
||||
LDADD += libldl.la
|
||||
endif
|
||||
|
||||
# rule to run an executable
|
||||
%.run: % $(LDADD)
|
||||
./$^
|
||||
|
||||
# rule to run executable with valgrind
|
||||
%.valgrind: % $(LDADD)
|
||||
valgrind ./$^
|
||||
|
||||
if USE_LAPACK
|
||||
AM_CXXFLAGS += -DGT_USE_LAPACK
|
||||
LDADD += ../spqr_mini/libspqr_mini.la
|
||||
|
|
|
|||
|
|
@ -57,14 +57,15 @@ AM_LDFLAGS = $(BOOST_LDFLAGS) $(boost_serialization)
|
|||
LDADD = liblinear.la ../inference/libinference.la ../base/libbase.la
|
||||
LDADD += ../CppUnitLite/libCppUnitLite.a ../colamd/libcolamd.la
|
||||
AM_DEFAULT_SOURCE_EXT = .cpp
|
||||
if USE_LDL
|
||||
LDADD += libldl.la
|
||||
endif
|
||||
|
||||
# rule to run an executable
|
||||
%.run: % $(LDADD)
|
||||
./$^
|
||||
|
||||
# rule to run executable with valgrind
|
||||
%.valgrind: % $(LDADD)
|
||||
valgrind ./$^
|
||||
|
||||
if USE_LAPACK
|
||||
AM_CXXFLAGS += -DGT_USE_LAPACK
|
||||
LDADD += ../spqr_mini/libspqr_mini.la
|
||||
|
|
|
|||
|
|
@ -1,72 +0,0 @@
|
|||
/**
|
||||
* @file ConstraintOptimizer.cpp
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
/** IMPORTANT NOTE: this file is only compiled when LDL is available */
|
||||
|
||||
#include <gtsam/nonlinear/ConstraintOptimizer.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
/* ************************************************************************* */
|
||||
void gtsam::BFGSEstimator::update(const Vector& dfx, const boost::optional<Vector&> step) {
|
||||
if (step) {
|
||||
Vector Bis = B_ * *step,
|
||||
y = dfx - prev_dfx_;
|
||||
B_ = B_ + outer_prod(y, y) / inner_prod(y, *step)
|
||||
- outer_prod(Bis, Bis) / inner_prod(*step, Bis);
|
||||
}
|
||||
prev_dfx_ = dfx;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector gtsam::linesearch(const Vector& x0, const Vector& delta,
|
||||
double (*penalty)(const Vector&), size_t maxIt) {
|
||||
|
||||
// calculate the initial error
|
||||
double init_error = penalty(x0);
|
||||
Vector step = delta;
|
||||
for (size_t i=0; i<maxIt; ++i) {
|
||||
Vector x = x0 + step;
|
||||
double cur_error = penalty(x);
|
||||
if (cur_error < init_error) // if we have improved, return the step
|
||||
return step;
|
||||
else { // otherwise, make a smaller step
|
||||
step = 0.5 * step;
|
||||
}
|
||||
}
|
||||
|
||||
// TODO: should do something clever here
|
||||
return step;
|
||||
}
|
||||
|
||||
|
|
@ -1,85 +0,0 @@
|
|||
/**
|
||||
* @file ConstraintOptimizer.h
|
||||
* @brief Utilities and classes required for solving Constrained Problems
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
/**
|
||||
* IMPORTANT NOTE: This is an EXPERIMENTAL system that is not ready for use!
|
||||
* DO NOT USE if you actually wanted to accomplish something
|
||||
*
|
||||
* REQUIRES --enable-ldl flag to be set for this class to work, not compiled otherwise
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <boost/optional.hpp>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* BFGS Hessian estimator
|
||||
* Maintains an estimate for a hessian matrix using
|
||||
* only derivatives and the step
|
||||
*/
|
||||
class BFGSEstimator {
|
||||
protected:
|
||||
size_t n_; // dimension of square matrix
|
||||
Matrix B_; // current estimate
|
||||
Vector prev_dfx_; // previous gradient value
|
||||
public:
|
||||
|
||||
/**
|
||||
* Creates an estimator of a particular dimension
|
||||
*/
|
||||
BFGSEstimator(size_t n) : n_(n), B_(eye(n,n)) {}
|
||||
|
||||
~BFGSEstimator() {}
|
||||
|
||||
/**
|
||||
* Direct vector interface - useful for small problems
|
||||
*
|
||||
* Update will set the previous gradient and update the
|
||||
* estimate for the Hessian. When specified without
|
||||
* the step, this is the initialization phase, and will not
|
||||
* change the Hessian estimate
|
||||
* @param df is the gradient of the function
|
||||
* @param step is the step vector applied at the last iteration
|
||||
*/
|
||||
void update(const Vector& df, const boost::optional<Vector&> step = boost::none);
|
||||
|
||||
// access functions
|
||||
const Matrix& getB() const { return B_; }
|
||||
size_t dim() const { return n_; }
|
||||
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* 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);
|
||||
|
||||
/**
|
||||
* Simple line search function using an externally specified
|
||||
* penalty function
|
||||
*/
|
||||
Vector linesearch(const Vector& x, const Vector& delta,
|
||||
double (*penalty)(const Vector&), size_t maxIt = 10);
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
|
@ -1,393 +0,0 @@
|
|||
/**
|
||||
* @file FusionTupleConfig.h
|
||||
* @brief Experimental tuple config using boost.Fusion
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <boost/fusion/container/set.hpp>
|
||||
#include <boost/fusion/include/make_set.hpp>
|
||||
#include <boost/fusion/include/as_vector.hpp>
|
||||
#include <boost/fusion/include/at_key.hpp>
|
||||
#include <boost/fusion/include/has_key.hpp>
|
||||
#include <boost/fusion/include/zip.hpp>
|
||||
#include <boost/fusion/include/all.hpp>
|
||||
#include <boost/fusion/algorithm/iteration.hpp>
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/nonlinear/LieConfig.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* This config uses a real tuple to store types
|
||||
* The template parameter should be a boost fusion structure, like
|
||||
* set<Config1, Config2>
|
||||
*/
|
||||
template<class Configs>
|
||||
class FusionTupleConfig : public Testable<FusionTupleConfig<Configs> >{
|
||||
|
||||
public:
|
||||
/** useful types */
|
||||
typedef Configs BaseTuple;
|
||||
|
||||
protected:
|
||||
/** the underlying tuple storing everything */
|
||||
Configs base_tuple_;
|
||||
|
||||
private:
|
||||
/** helper structs to make use of fusion algorithms */
|
||||
struct size_helper {
|
||||
typedef size_t result_type;
|
||||
template<typename T>
|
||||
size_t operator()(const T& t, const size_t& s) const {
|
||||
return s + t.size();
|
||||
}
|
||||
};
|
||||
|
||||
struct dim_helper {
|
||||
typedef size_t result_type;
|
||||
template<typename T>
|
||||
size_t operator()(const T& t, const size_t& s) const {
|
||||
return s + t.dim();
|
||||
}
|
||||
};
|
||||
|
||||
struct zero_helper {
|
||||
typedef VectorConfig result_type;
|
||||
template<typename T>
|
||||
result_type operator()(const T& t, const result_type& s) const {
|
||||
result_type new_s(s);
|
||||
new_s.insert(t.zero());
|
||||
return new_s;
|
||||
}
|
||||
};
|
||||
|
||||
struct update_helper {
|
||||
typedef Configs result_type;
|
||||
template<typename T>
|
||||
result_type operator()(const T& t, const result_type& s) const {
|
||||
result_type new_s(s);
|
||||
boost::fusion::at_key<T>(new_s).update(t);
|
||||
return new_s;
|
||||
}
|
||||
};
|
||||
|
||||
struct expmap_helper {
|
||||
typedef FusionTupleConfig<Configs> result_type;
|
||||
VectorConfig delta;
|
||||
expmap_helper(const VectorConfig& d) : delta(d) {}
|
||||
template<typename T>
|
||||
result_type operator()(const T& t, const result_type& s) const {
|
||||
result_type new_s(s);
|
||||
boost::fusion::at_key<T>(new_s.base_tuple_) = T(gtsam::expmap(t, delta));
|
||||
return new_s;
|
||||
}
|
||||
};
|
||||
|
||||
struct logmap_helper {
|
||||
typedef VectorConfig result_type;
|
||||
template<typename T>
|
||||
result_type operator()(const T& t, const result_type& s) const {
|
||||
result_type new_s(s);
|
||||
new_s.insert(gtsam::logmap(boost::fusion::at_c<0>(t), boost::fusion::at_c<1>(t)));
|
||||
return new_s;
|
||||
}
|
||||
};
|
||||
|
||||
struct empty_helper {
|
||||
template<typename T>
|
||||
bool operator()(T t) const {
|
||||
return t.empty();
|
||||
}
|
||||
};
|
||||
|
||||
struct print_helper {
|
||||
template<typename T>
|
||||
void operator()(T t) const {
|
||||
t.print();
|
||||
}
|
||||
};
|
||||
|
||||
struct equals_helper {
|
||||
double tol;
|
||||
equals_helper(double t) : tol(t) {}
|
||||
template<typename T>
|
||||
bool operator()(T t) const {
|
||||
return boost::fusion::at_c<0>(t).equals(boost::fusion::at_c<1>(t), tol);
|
||||
}
|
||||
};
|
||||
|
||||
/** two separate function objects for arbitrary copy construction */
|
||||
template<typename Ret, typename Config>
|
||||
struct assign_inner {
|
||||
typedef Ret result_type;
|
||||
|
||||
Config config;
|
||||
assign_inner(const Config& cfg) : config(cfg) {}
|
||||
|
||||
template<typename T>
|
||||
result_type operator()(const T& t, const result_type& s) const {
|
||||
result_type new_s(s);
|
||||
T new_cfg(config);
|
||||
if (!new_cfg.empty()) boost::fusion::at_key<T>(new_s) = new_cfg;
|
||||
return new_s;
|
||||
}
|
||||
};
|
||||
|
||||
template<typename Ret>
|
||||
struct assign_outer {
|
||||
typedef Ret result_type;
|
||||
|
||||
template<typename T> // T is the config from the "other" config
|
||||
Ret operator()(const T& t, const Ret& s) const {
|
||||
assign_inner<Ret, T> helper(t);
|
||||
return boost::fusion::fold(s, s, helper); // loop over the "self" config
|
||||
}
|
||||
};
|
||||
|
||||
public:
|
||||
/** create an empty config */
|
||||
FusionTupleConfig() {}
|
||||
|
||||
/** copy constructor */
|
||||
FusionTupleConfig(const FusionTupleConfig<Configs>& other) : base_tuple_(other.base_tuple_) {}
|
||||
|
||||
/** direct initialization of the underlying structure */
|
||||
FusionTupleConfig(const Configs& cfg_set) : base_tuple_(cfg_set) {}
|
||||
|
||||
/** initialization from arbitrary other configs */
|
||||
template<class Configs2>
|
||||
FusionTupleConfig(const FusionTupleConfig<Configs2>& other)
|
||||
: base_tuple_(boost::fusion::fold(other.base_tuple(), Configs(), assign_outer<Configs>()))
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~FusionTupleConfig() {}
|
||||
|
||||
/** insertion */
|
||||
template <class Key, class Value>
|
||||
void insert(const Key& j, const Value& x) {
|
||||
config_<LieConfig<Key> >().insert(j,x);
|
||||
}
|
||||
|
||||
/** insert a full config at a time */
|
||||
template<class Config>
|
||||
void insertSub(const Config& config) {
|
||||
config_<Config>().insert(config);
|
||||
}
|
||||
|
||||
/**
|
||||
* Update function for whole configs - this will change existing values
|
||||
* @param config is a config to add
|
||||
*/
|
||||
void update(const FusionTupleConfig<Configs>& config) {
|
||||
base_tuple_ = boost::fusion::accumulate(
|
||||
config.base_tuple_, base_tuple_,
|
||||
update_helper());
|
||||
}
|
||||
|
||||
/**
|
||||
* Update function for whole subconfigs - this will change existing values
|
||||
* @param config is a config to add
|
||||
*/
|
||||
template<class Config>
|
||||
void update(const Config& config) {
|
||||
config_<Config>().update(config);
|
||||
}
|
||||
|
||||
/**
|
||||
* Update function for single key/value pairs - will change existing values
|
||||
* @param key is the variable identifier
|
||||
* @param value is the variable value to update
|
||||
*/
|
||||
template<class Key, class Value>
|
||||
void update(const Key& key, const Value& value) {
|
||||
config_<LieConfig<Key> >().update(key,value);
|
||||
}
|
||||
|
||||
/** check if a given element exists */
|
||||
template<class Key>
|
||||
bool exists(const Key& j) const {
|
||||
return config<LieConfig<Key> >().exists(j);
|
||||
}
|
||||
|
||||
/** a variant of exists */
|
||||
template<class Key>
|
||||
boost::optional<typename Key::Value_t> exists_(const Key& j) const {
|
||||
return config<LieConfig<Key> >().exists_(j);
|
||||
}
|
||||
|
||||
/** retrieve a point */
|
||||
template<class Key>
|
||||
const typename Key::Value_t & at(const Key& j) const {
|
||||
return config<LieConfig<Key> >().at(j);
|
||||
}
|
||||
|
||||
/** access operator */
|
||||
template<class Key>
|
||||
const typename Key::Value_t & operator[](const Key& j) const { return at<Key>(j); }
|
||||
|
||||
/** retrieve a reference to a full subconfig */
|
||||
template<class Config>
|
||||
const Config & config() const {
|
||||
return boost::fusion::at_key<Config>(base_tuple_);
|
||||
}
|
||||
|
||||
/** size of the config - sum all sizes from subconfigs */
|
||||
size_t size() const {
|
||||
return boost::fusion::accumulate(base_tuple_, 0,
|
||||
size_helper());
|
||||
}
|
||||
|
||||
/** combined dimension of the subconfigs */
|
||||
size_t dim() const {
|
||||
return boost::fusion::accumulate(base_tuple_, 0,
|
||||
dim_helper());
|
||||
}
|
||||
|
||||
/** number of configs in the config */
|
||||
size_t nrConfigs() const {
|
||||
return boost::fusion::size(base_tuple_);
|
||||
}
|
||||
|
||||
/** erases a specific key */
|
||||
template<class Key>
|
||||
void erase(const Key& j) {
|
||||
config_<LieConfig<Key> >().erase(j);
|
||||
}
|
||||
|
||||
/** clears the config */
|
||||
void clear() {
|
||||
base_tuple_ = Configs();
|
||||
}
|
||||
|
||||
/** returns true if the config is empty */
|
||||
bool empty() const {
|
||||
return boost::fusion::all(base_tuple_,
|
||||
empty_helper());
|
||||
}
|
||||
|
||||
/** print */
|
||||
void print(const std::string& s="") const {
|
||||
std::cout << "FusionTupleConfig " << s << ":" << std::endl;
|
||||
boost::fusion::for_each(base_tuple_, print_helper());
|
||||
}
|
||||
|
||||
/** equals */
|
||||
bool equals(const FusionTupleConfig<Configs>& other, double tol=1e-9) const {
|
||||
equals_helper helper(tol);
|
||||
return boost::fusion::all(
|
||||
boost::fusion::zip(base_tuple_,other.base_tuple_), helper);
|
||||
}
|
||||
|
||||
/** zero: create VectorConfig of appropriate structure */
|
||||
VectorConfig zero() const {
|
||||
return boost::fusion::accumulate(base_tuple_, VectorConfig(),
|
||||
zero_helper());
|
||||
}
|
||||
|
||||
FusionTupleConfig<Configs> expmap(const VectorConfig& delta) const {
|
||||
return boost::fusion::accumulate(base_tuple_, base_tuple_,
|
||||
expmap_helper(delta));
|
||||
}
|
||||
|
||||
VectorConfig logmap(const FusionTupleConfig<Configs>& cp) const {
|
||||
return boost::fusion::accumulate(boost::fusion::zip(base_tuple_, cp.base_tuple_),
|
||||
VectorConfig(), logmap_helper());
|
||||
}
|
||||
|
||||
/**
|
||||
* direct access to the underlying fusion set - don't use this normally
|
||||
* TODO: make this a friend function or similar
|
||||
*/
|
||||
const BaseTuple & base_tuple() const { return base_tuple_; }
|
||||
|
||||
private:
|
||||
|
||||
/** retrieve a non-const reference to a full subconfig */
|
||||
template<class Config>
|
||||
Config & config_() {
|
||||
return boost::fusion::at_key<Config>(base_tuple_);
|
||||
}
|
||||
|
||||
|
||||
/** Serialization function */
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive & ar, const unsigned int version) {
|
||||
ar & BOOST_SERIALIZATION_NVP(base_tuple_);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/** Exmap static functions */
|
||||
template<class Configs>
|
||||
inline FusionTupleConfig<Configs> expmap(const FusionTupleConfig<Configs>& c, const VectorConfig& delta) {
|
||||
return c.expmap(delta);
|
||||
}
|
||||
|
||||
/** logmap static functions */
|
||||
template<class Configs>
|
||||
inline VectorConfig logmap(const FusionTupleConfig<Configs>& c0, const FusionTupleConfig<Configs>& cp) {
|
||||
return c0.logmap(cp);
|
||||
}
|
||||
|
||||
/**
|
||||
* Easy-to-use versions of FusionTupleConfig
|
||||
* These versions provide a simpler interface more like
|
||||
* the existing TupleConfig. Each version has a number, and
|
||||
* takes explicit template arguments for config types.
|
||||
*/
|
||||
template<class C1>
|
||||
struct FusionTupleConfig1 : public FusionTupleConfig<boost::fusion::set<C1> > {
|
||||
typedef FusionTupleConfig<boost::fusion::set<C1> > Base;
|
||||
typedef FusionTupleConfig1<C1> This;
|
||||
|
||||
typedef C1 Config1;
|
||||
|
||||
FusionTupleConfig1() {}
|
||||
FusionTupleConfig1(const Base& c) : Base(c) {}
|
||||
FusionTupleConfig1(const C1& c1) : Base(boost::fusion::make_set(c1)) {}
|
||||
|
||||
const Config1& first() const { return boost::fusion::at_key<C1>(this->base_tuple_); }
|
||||
};
|
||||
|
||||
template<class C1, class C2>
|
||||
struct FusionTupleConfig2 : public FusionTupleConfig<boost::fusion::set<C1, C2> > {
|
||||
typedef FusionTupleConfig<boost::fusion::set<C1, C2> > Base;
|
||||
typedef FusionTupleConfig2<C1,C2> This;
|
||||
|
||||
typedef C1 Config1;
|
||||
typedef C2 Config2;
|
||||
|
||||
FusionTupleConfig2() {}
|
||||
FusionTupleConfig2(const Base& c) : Base(c) {}
|
||||
FusionTupleConfig2(const C1& c1, const C2& c2) : Base(boost::fusion::make_set(c1, c2)) {}
|
||||
|
||||
const Config1& first() const { return boost::fusion::at_key<C1>(this->base_tuple_); }
|
||||
const Config2& second() const { return boost::fusion::at_key<C2>(this->base_tuple_); }
|
||||
};
|
||||
|
||||
template<class C1, class C2, class C3>
|
||||
struct FusionTupleConfig3 : public FusionTupleConfig<boost::fusion::set<C1, C2, C3> > {
|
||||
typedef FusionTupleConfig<boost::fusion::set<C1, C2, C3> > Base;
|
||||
|
||||
typedef C1 Config1;
|
||||
typedef C2 Config2;
|
||||
typedef C3 Config3;
|
||||
|
||||
FusionTupleConfig3() {}
|
||||
FusionTupleConfig3(const Base& c) : Base(c) {}
|
||||
FusionTupleConfig3(const C1& c1, const C2& c2, const C3& c3)
|
||||
: Base(boost::fusion::make_set(c1, c2, c3)) {}
|
||||
|
||||
const Config1& first() const { return boost::fusion::at_key<C1>(this->base_tuple_); }
|
||||
const Config2& second() const { return boost::fusion::at_key<C2>(this->base_tuple_); }
|
||||
const Config3& third() const { return boost::fusion::at_key<C3>(this->base_tuple_); }
|
||||
};
|
||||
|
||||
|
||||
} // \namespace gtsam
|
||||
|
||||
|
|
@ -17,7 +17,6 @@ check_PROGRAMS =
|
|||
|
||||
# Lie Groups
|
||||
headers += LieConfig.h LieConfig-inl.h TupleConfig.h TupleConfig-inl.h
|
||||
headers += FusionTupleConfig.h
|
||||
check_PROGRAMS += tests/testLieConfig
|
||||
|
||||
# Nonlinear nonlinear
|
||||
|
|
@ -30,12 +29,6 @@ sources += NonlinearOptimizer.cpp
|
|||
headers += NonlinearConstraint.h
|
||||
headers += NonlinearEquality.h
|
||||
|
||||
# SQP
|
||||
if USE_LDL
|
||||
sources += ConstraintOptimizer.cpp
|
||||
check_PROGRAMS += tests/testConstraintOptimizer
|
||||
endif
|
||||
|
||||
#----------------------------------------------------------------------------------------------------
|
||||
# Create a libtool library that is not installed
|
||||
# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am
|
||||
|
|
@ -57,14 +50,15 @@ AM_LDFLAGS = $(BOOST_LDFLAGS) $(boost_serialization)
|
|||
LDADD = libnonlinear.la ../linear/liblinear.la ../inference/libinference.la ../base/libbase.la
|
||||
LDADD += ../CppUnitLite/libCppUnitLite.a ../colamd/libcolamd.la
|
||||
AM_DEFAULT_SOURCE_EXT = .cpp
|
||||
if USE_LDL
|
||||
LDADD += libldl.la
|
||||
endif
|
||||
|
||||
# rule to run an executable
|
||||
%.run: % $(LDADD)
|
||||
./$^
|
||||
|
||||
# rule to run executable with valgrind
|
||||
%.valgrind: % $(LDADD)
|
||||
valgrind ./$^
|
||||
|
||||
if USE_LAPACK
|
||||
AM_CXXFLAGS += -DGT_USE_LAPACK
|
||||
LDADD += ../spqr_mini/libspqr_mini.la
|
||||
|
|
|
|||
|
|
@ -78,9 +78,6 @@ AM_DEFAULT_SOURCE_EXT = .cpp
|
|||
AM_LDFLAGS = $(BOOST_LDFLAGS) $(boost_serialization)
|
||||
LDADD = libslam.la ../geometry/libgeometry.la ../nonlinear/libnonlinear.la ../linear/liblinear.la ../inference/libinference.la ../base/libbase.la
|
||||
LDADD += ../CppUnitLite/libCppUnitLite.a ../colamd/libcolamd.la
|
||||
if USE_LDL
|
||||
LDADD += libldl.la
|
||||
endif
|
||||
|
||||
if USE_LAPACK
|
||||
LDADD += ../spqr_mini/libspqr_mini.la
|
||||
|
|
@ -90,3 +87,6 @@ endif
|
|||
%.run: % $(LDADD)
|
||||
./$^
|
||||
|
||||
# rule to run executable with valgrind
|
||||
%.valgrind: % $(LDADD)
|
||||
valgrind ./$^
|
||||
|
|
|
|||
|
|
@ -22,13 +22,6 @@ if ENABLE_SERIALIZATION
|
|||
check_PROGRAMS += testSerialization
|
||||
endif
|
||||
|
||||
# experimental
|
||||
#check_PROGRAMS += testFusionTupleConfig # Doesn't work on Macs
|
||||
|
||||
if USE_LDL
|
||||
check_PROGRAMS += testConstraintOptimizer
|
||||
endif
|
||||
|
||||
# Timing tests
|
||||
noinst_PROGRAMS = timeGaussianFactorGraph timeFactorOverhead
|
||||
|
||||
|
|
@ -51,3 +44,6 @@ AM_DEFAULT_SOURCE_EXT = .cpp
|
|||
%.run: % $(LDADD)
|
||||
./$^
|
||||
|
||||
# rule to run executable with valgrind
|
||||
%.valgrind: % $(LDADD)
|
||||
valgrind ./$^
|
||||
|
|
|
|||
|
|
@ -1,318 +0,0 @@
|
|||
/**
|
||||
* @file testConstraintOptimizer.cpp
|
||||
* @brief Tests the optimization engine for SQP and BFGS Quadratic programming techniques
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
|
||||
#include <boost/tuple/tuple.hpp>
|
||||
#include <boost/optional.hpp>
|
||||
|
||||
#include <gtsam/CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
#include <gtsam/nonlinear/ConstraintOptimizer.h>
|
||||
|
||||
#define GTSAM_MAGIC_KEY
|
||||
|
||||
#include <boost/assign/std/list.hpp> // for operator +=
|
||||
using namespace boost::assign;
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
#include <gtsam/slam/smallExample.h>
|
||||
using namespace example;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( matrix, unconstrained_fg_ata ) {
|
||||
// create a graph
|
||||
GaussianFactorGraph fg = createGaussianFactorGraph();
|
||||
|
||||
Matrix A; Vector b;
|
||||
Ordering ordering;
|
||||
ordering += Symbol('l', 1), Symbol('x', 1), Symbol('x', 2);
|
||||
boost::tie(A, b) = fg.matrix(ordering);
|
||||
Matrix B_ata = prod(trans(A), A);
|
||||
|
||||
// solve subproblem
|
||||
Vector actual = solve_ldl(B_ata, prod(trans(A), b));
|
||||
|
||||
// verify
|
||||
Vector expected = createCorrectDelta().vector();
|
||||
CHECK(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//TEST( matrix, unconstrained_fg ) {
|
||||
// // create a graph
|
||||
// GaussianFactorGraph fg = createGaussianFactorGraph();
|
||||
//
|
||||
// Matrix A; Vector b;
|
||||
// Ordering ordering;
|
||||
// ordering += Symbol('l', 1), Symbol('x', 1), Symbol('x', 2);
|
||||
// boost::tie(A, b) = fg.matrix(ordering);
|
||||
// Matrix B_ata = prod(trans(A), A);
|
||||
//// print(B_ata, "B_ata");
|
||||
//// print(b, " b");
|
||||
//
|
||||
// // parameters
|
||||
// size_t maxIt = 50;
|
||||
// double stepsize = 0.1;
|
||||
//
|
||||
// // iterate to solve
|
||||
// VectorConfig x = createZeroDelta();
|
||||
// BFGSEstimator B(x.dim());
|
||||
//
|
||||
// Vector step;
|
||||
//
|
||||
// for (size_t i=0; i<maxIt; ++i) {
|
||||
//// cout << "Error at Iteration: " << i << " is " << fg.error(x) << endl;
|
||||
//
|
||||
// // find the gradient
|
||||
// Vector dfx = fg.gradient(x).vector();
|
||||
//// print(dfx, " dfx");
|
||||
// CHECK(assert_equal(-1.0 * prod(trans(A), b - A*x.vector()), dfx));
|
||||
//
|
||||
// // update hessian
|
||||
// if (i>0) {
|
||||
// B.update(dfx, step);
|
||||
// } else {
|
||||
// B.update(dfx);
|
||||
// }
|
||||
//
|
||||
// // solve subproblem
|
||||
//// print(B.getB(), " B_bfgs");
|
||||
// Vector delta = solve_ldl(B.getB(), -dfx);
|
||||
//// Vector delta = solve_ldl(B_ata, -dfx);
|
||||
//
|
||||
//// print(delta, " delta");
|
||||
//
|
||||
// // update
|
||||
// step = stepsize * delta;
|
||||
//// step = linesearch(x, delta, penalty); // TODO: switch here
|
||||
// x = expmap(x, step);
|
||||
//// print(step, " step");
|
||||
// }
|
||||
//
|
||||
// // verify
|
||||
// VectorConfig expected = createCorrectDelta();
|
||||
// CHECK(assert_equal(expected,x, 1e-4));
|
||||
//}
|
||||
|
||||
SharedDiagonal probModel1 = sharedSigma(1,1.0);
|
||||
SharedDiagonal probModel2 = sharedSigma(2,1.0);
|
||||
SharedDiagonal constraintModel1 = noiseModel::Constrained::All(1);
|
||||
|
||||
/* *********************************************************************
|
||||
* This example uses a nonlinear objective function and
|
||||
* nonlinear equality constraint. The formulation is actually
|
||||
* the Cholesky form that creates the full Hessian explicitly,
|
||||
* which should really be avoided with our QR-based machinery.
|
||||
*
|
||||
* Note: the update equation used here has a fixed step size
|
||||
* and gain that is rather arbitrarily chosen, and as such,
|
||||
* will take a silly number of iterations.
|
||||
*/
|
||||
TEST (SQP, problem1_cholesky ) {
|
||||
bool verbose = false;
|
||||
// use a nonlinear function of f(x) = x^2+y^2
|
||||
// nonlinear equality constraint: g(x) = x^2-5-y=0
|
||||
// Lagrangian: f(x) + \lambda*g(x)
|
||||
|
||||
// Symbols
|
||||
Symbol x1("x1"), y1("y1"), L1("L1");
|
||||
|
||||
// state structure: [x y \lambda]
|
||||
VectorConfig init, state;
|
||||
init.insert(x1, Vector_(1, 1.0));
|
||||
init.insert(y1, Vector_(1, 1.0));
|
||||
init.insert(L1, Vector_(1, 1.0));
|
||||
state = init;
|
||||
|
||||
if (verbose) init.print("Initial State");
|
||||
|
||||
// loop until convergence
|
||||
int maxIt = 10;
|
||||
for (int i = 0; i<maxIt; ++i) {
|
||||
if (verbose) cout << "\n******************************\nIteration: " << i+1 << endl;
|
||||
|
||||
// extract the states
|
||||
double x, y, lambda;
|
||||
x = state[x1](0);
|
||||
y = state[y1](0);
|
||||
lambda = state[L1](0);
|
||||
|
||||
// calculate the components
|
||||
Matrix H1, H2, gradG;
|
||||
Vector gradL, gx;
|
||||
|
||||
// hessian of lagrangian function, in two columns:
|
||||
H1 = Matrix_(2,1,
|
||||
2.0+2.0*lambda,
|
||||
0.0);
|
||||
H2 = Matrix_(2,1,
|
||||
0.0,
|
||||
2.0);
|
||||
|
||||
// deriviative of lagrangian function
|
||||
gradL = Vector_(2,
|
||||
2.0*x*(1+lambda),
|
||||
2.0*y-lambda);
|
||||
|
||||
// constraint derivatives
|
||||
gradG = Matrix_(2,1,
|
||||
2.0*x,
|
||||
0.0);
|
||||
|
||||
// constraint value
|
||||
gx = Vector_(1,
|
||||
x*x-5-y);
|
||||
|
||||
// create a factor for the states
|
||||
GaussianFactor::shared_ptr f1(new
|
||||
GaussianFactor(x1, H1, y1, H2, L1, gradG, gradL, probModel2));
|
||||
|
||||
// create a factor for the lagrange multiplier
|
||||
GaussianFactor::shared_ptr f2(new
|
||||
GaussianFactor(x1, -sub(gradG, 0, 1, 0, 1),
|
||||
y1, -sub(gradG, 1, 2, 0, 1), -gx, constraintModel1));
|
||||
|
||||
// construct graph
|
||||
GaussianFactorGraph fg;
|
||||
fg.push_back(f1);
|
||||
fg.push_back(f2);
|
||||
if (verbose) fg.print("Graph");
|
||||
|
||||
// solve
|
||||
Ordering ord;
|
||||
ord += x1, y1, L1;
|
||||
VectorConfig delta = fg.optimize(ord).scale(-1.0);
|
||||
if (verbose) delta.print("Delta");
|
||||
|
||||
// update initial estimate
|
||||
VectorConfig newState = expmap(state, delta);
|
||||
state = newState;
|
||||
|
||||
if (verbose) state.print("Updated State");
|
||||
}
|
||||
|
||||
// verify that it converges to the nearest optimal point
|
||||
VectorConfig expected;
|
||||
expected.insert(L1, Vector_(1, -1.0));
|
||||
expected.insert(x1, Vector_(1, 2.12));
|
||||
expected.insert(y1, Vector_(1, -0.5));
|
||||
CHECK(assert_equal(expected,state, 1e-2));
|
||||
}
|
||||
|
||||
/* *********************************************************************
|
||||
* This example uses a nonlinear objective function and
|
||||
* nonlinear equality constraint. This formulation splits
|
||||
* the constraint into a factor and a linear constraint.
|
||||
*
|
||||
* This example uses the same silly number of iterations as the
|
||||
* previous example.
|
||||
*/
|
||||
TEST (SQP, problem1_sqp ) {
|
||||
bool verbose = false;
|
||||
// use a nonlinear function of f(x) = x^2+y^2
|
||||
// nonlinear equality constraint: g(x) = x^2-5-y=0
|
||||
// Lagrangian: f(x) + \lambda*g(x)
|
||||
|
||||
// Symbols
|
||||
Symbol x1("x1"), y1("y1"), L1("L1");
|
||||
|
||||
// state structure: [x y \lambda]
|
||||
VectorConfig init, state;
|
||||
init.insert(x1, Vector_(1, 1.0));
|
||||
init.insert(y1, Vector_(1, 1.0));
|
||||
init.insert(L1, Vector_(1, 1.0));
|
||||
state = init;
|
||||
|
||||
if (verbose) init.print("Initial State");
|
||||
|
||||
// loop until convergence
|
||||
int maxIt = 5;
|
||||
for (int i = 0; i<maxIt; ++i) {
|
||||
if (verbose) cout << "\n******************************\nIteration: " << i+1 << endl;
|
||||
|
||||
// extract the states
|
||||
double x, y, lambda;
|
||||
x = state[x1](0);
|
||||
y = state[y1](0);
|
||||
lambda = state[L1](0);
|
||||
|
||||
/** create the linear factor
|
||||
* ||h(x)-z||^2 => ||Ax-b||^2
|
||||
* where:
|
||||
* h(x) simply returns the inputs
|
||||
* z zeros(2)
|
||||
* A identity
|
||||
* b linearization point
|
||||
*/
|
||||
Matrix A = eye(2);
|
||||
Vector b = Vector_(2, x, y);
|
||||
GaussianFactor::shared_ptr f1(
|
||||
new GaussianFactor(x1, sub(A, 0,2, 0,1), // A(:,1)
|
||||
y1, sub(A, 0,2, 1,2), // A(:,2)
|
||||
b, // rhs of f(x)
|
||||
probModel2)); // arbitrary sigma
|
||||
|
||||
/** create the constraint-linear factor
|
||||
* Provides a mechanism to use variable gain to force the constraint
|
||||
* \lambda*gradG*dx + d\lambda = zero
|
||||
* formulated in matrix form as:
|
||||
* [\lambda*gradG eye(1)] [dx; d\lambda] = zero
|
||||
*/
|
||||
Matrix gradG = Matrix_(1, 2,2*x, -1.0);
|
||||
GaussianFactor::shared_ptr f2(
|
||||
new GaussianFactor(x1, lambda*sub(gradG, 0,1, 0,1), // scaled gradG(:,1)
|
||||
y1, lambda*sub(gradG, 0,1, 1,2), // scaled gradG(:,2)
|
||||
L1, eye(1), // dlambda term
|
||||
Vector_(1, 0.0), // rhs is zero
|
||||
probModel1)); // arbitrary sigma
|
||||
|
||||
// create the actual constraint
|
||||
// [gradG] [x; y] - g = 0
|
||||
Vector g = Vector_(1,x*x-y-5);
|
||||
GaussianFactor::shared_ptr c1(
|
||||
new GaussianFactor(x1, sub(gradG, 0,1, 0,1), // slice first part of gradG
|
||||
y1, sub(gradG, 0,1, 1,2), // slice second part of gradG
|
||||
g, // value of constraint function
|
||||
constraintModel1)); // force to constraint
|
||||
|
||||
// construct graph
|
||||
GaussianFactorGraph fg;
|
||||
fg.push_back(f1);
|
||||
fg.push_back(f2);
|
||||
fg.push_back(c1);
|
||||
if (verbose) fg.print("Graph");
|
||||
|
||||
// solve
|
||||
Ordering ord;
|
||||
ord += x1, y1, L1;
|
||||
VectorConfig delta = fg.optimize(ord);
|
||||
if (verbose) delta.print("Delta");
|
||||
|
||||
// update initial estimate
|
||||
VectorConfig newState = expmap(state, delta.scale(-1.0));
|
||||
|
||||
// set the state to the updated state
|
||||
state = newState;
|
||||
|
||||
if (verbose) state.print("Updated State");
|
||||
}
|
||||
|
||||
// verify that it converges to the nearest optimal point
|
||||
VectorConfig expected;
|
||||
expected.insert(x1, Vector_(1, 2.12));
|
||||
expected.insert(y1, Vector_(1, -0.5));
|
||||
CHECK(assert_equal(state[x1], expected[x1], 1e-2));
|
||||
CHECK(assert_equal(state[y1], expected[y1], 1e-2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -1,518 +0,0 @@
|
|||
/**
|
||||
* @file testFusionTupleConfig.cpp
|
||||
* @author Alex Cunningham
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <gtsam/CppUnitLite/TestHarness.h>
|
||||
|
||||
#define GTSAM_MAGIC_KEY
|
||||
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <gtsam/linear/VectorConfig.h>
|
||||
|
||||
#include <gtsam/slam/BearingRangeFactor.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>
|
||||
#include <gtsam/nonlinear/LieConfig-inl.h>
|
||||
#include <gtsam/nonlinear/NonlinearOptimizer-inl.h>
|
||||
#include <gtsam/nonlinear/FusionTupleConfig.h>
|
||||
|
||||
using namespace boost;
|
||||
using namespace gtsam;
|
||||
using namespace std;
|
||||
|
||||
static const double tol = 1e-5;
|
||||
|
||||
typedef TypedSymbol<Pose2, 'x'> PoseKey;
|
||||
typedef TypedSymbol<Point2, 'l'> PointKey;
|
||||
typedef LieConfig<PoseKey> PoseConfig;
|
||||
typedef LieConfig<PointKey> PointConfig;
|
||||
|
||||
// some generic poses, points and keys
|
||||
PoseKey x1(1), x2(2);
|
||||
Pose2 x1_val(1.0, 2.0, 0.3), x2_val(2.0, 3.0, 0.4);
|
||||
PointKey l1(1), l2(2);
|
||||
Point2 l1_val(1.0, 2.0), l2_val(3.0, 4.0);
|
||||
|
||||
typedef FusionTupleConfig<fusion::set<PointConfig> > TupPointConfig;
|
||||
typedef FusionTupleConfig<fusion::set<PoseConfig> > TupPoseConfig;
|
||||
typedef FusionTupleConfig<fusion::set<PoseConfig, PointConfig> > TupPairConfig;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testFusionTupleConfig, basic_config_operations ) {
|
||||
|
||||
// some initialized configs to start with
|
||||
PoseConfig poseConfig1;
|
||||
poseConfig1.insert(x1, x1_val);
|
||||
PointConfig pointConfig1;
|
||||
pointConfig1.insert(l1, l1_val);
|
||||
|
||||
// basic initialization
|
||||
TupPointConfig cfg1A, cfg2A(pointConfig1);
|
||||
EXPECT(cfg1A.size() == 0);
|
||||
EXPECT(cfg2A.size() == 1);
|
||||
EXPECT(cfg1A.empty());
|
||||
EXPECT(!cfg2A.empty());
|
||||
|
||||
TupPoseConfig cfg1B, cfg2B(poseConfig1);
|
||||
EXPECT(cfg1B.size() == 0);
|
||||
EXPECT(cfg2B.size() == 1);
|
||||
EXPECT(cfg1B.empty());
|
||||
EXPECT(!cfg2B.empty());
|
||||
|
||||
TupPairConfig cfg1, cfg2(fusion::make_set(poseConfig1, pointConfig1));
|
||||
EXPECT(cfg1.size() == 0);
|
||||
EXPECT(cfg1.empty());
|
||||
EXPECT(cfg2.size() == 2);
|
||||
EXPECT(!cfg2.empty());
|
||||
|
||||
// insertion
|
||||
cfg1A.insert(l1, l1_val);
|
||||
EXPECT(cfg1A.size() == 1);
|
||||
cfg1A.insert(l2, l2_val);
|
||||
EXPECT(cfg1A.size() == 2);
|
||||
cfg2A.insert(l2, l2_val);
|
||||
EXPECT(cfg2A.size() == 2);
|
||||
|
||||
cfg1B.insert(x1, x1_val);
|
||||
EXPECT(cfg1B.size() == 1);
|
||||
cfg1B.insert(x2, x2_val);
|
||||
EXPECT(cfg1B.size() == 2);
|
||||
cfg2B.insert(x2, x2_val);
|
||||
EXPECT(cfg2B.size() == 2);
|
||||
|
||||
cfg1.insert(x1, x1_val);
|
||||
cfg1.insert(x2, x2_val);
|
||||
cfg1.insert(l1, l1_val);
|
||||
cfg1.insert(l2, l2_val);
|
||||
EXPECT(cfg1.size() == 4);
|
||||
|
||||
// exists
|
||||
EXPECT(cfg1.exists(x1));
|
||||
EXPECT(cfg1.exists(x2));
|
||||
EXPECT(cfg1.exists(l1));
|
||||
EXPECT(cfg1.exists(l2));
|
||||
|
||||
// retrieval of elements
|
||||
EXPECT(assert_equal(l1_val, cfg1A.at(l1)));
|
||||
EXPECT(assert_equal(l2_val, cfg1A.at(l2)));
|
||||
|
||||
EXPECT(assert_equal(x1_val, cfg1B.at(x1)));
|
||||
EXPECT(assert_equal(x2_val, cfg1B.at(x2)));
|
||||
|
||||
EXPECT(assert_equal(l1_val, cfg1.at(l1)));
|
||||
EXPECT(assert_equal(l2_val, cfg1.at(l2)));
|
||||
EXPECT(assert_equal(x1_val, cfg1.at(x1)));
|
||||
EXPECT(assert_equal(x2_val, cfg1.at(x2)));
|
||||
|
||||
// config extraction
|
||||
PointConfig expPointConfig;
|
||||
expPointConfig.insert(l1, l1_val);
|
||||
expPointConfig.insert(l2, l2_val);
|
||||
|
||||
PoseConfig expPoseConfig;
|
||||
expPoseConfig.insert(x1, x1_val);
|
||||
expPoseConfig.insert(x2, x2_val);
|
||||
|
||||
EXPECT(assert_equal(expPointConfig, cfg1A.config<PointConfig>()));
|
||||
EXPECT(assert_equal(expPoseConfig, cfg1B.config<PoseConfig>()));
|
||||
|
||||
EXPECT(assert_equal(expPointConfig, cfg1.config<PointConfig>()));
|
||||
EXPECT(assert_equal(expPoseConfig, cfg1.config<PoseConfig>()));
|
||||
|
||||
// getting sizes of configs
|
||||
EXPECT(cfg1A.nrConfigs() == 1);
|
||||
EXPECT(cfg1B.nrConfigs() == 1);
|
||||
EXPECT(cfg1.nrConfigs() == 2);
|
||||
|
||||
// erase
|
||||
cfg1.erase(x1);
|
||||
EXPECT(cfg1.size() == 3);
|
||||
EXPECT(!cfg1.exists(x1));
|
||||
cfg1.erase(l1);
|
||||
EXPECT(cfg1.size() == 2);
|
||||
EXPECT(!cfg1.exists(l1));
|
||||
|
||||
// clear
|
||||
cfg1.clear();
|
||||
cfg1A.clear(); cfg1B.clear();
|
||||
EXPECT(cfg1.size() == 0);
|
||||
EXPECT(cfg1.empty());
|
||||
EXPECT(cfg1A.size() == 0);
|
||||
EXPECT(cfg1A.empty());
|
||||
EXPECT(cfg1B.size() == 0);
|
||||
EXPECT(cfg1B.empty());
|
||||
|
||||
cfg2.clear();
|
||||
cfg2A.clear(); cfg2B.clear();
|
||||
EXPECT(cfg2.size() == 0);
|
||||
EXPECT(cfg2.empty());
|
||||
EXPECT(cfg2A.size() == 0);
|
||||
EXPECT(cfg2A.empty());
|
||||
EXPECT(cfg2B.size() == 0);
|
||||
EXPECT(cfg2B.empty());
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testFusionTupleConfig, slicing ) {
|
||||
TupPairConfig cfg_pair;
|
||||
cfg_pair.insert(x1, x1_val);
|
||||
cfg_pair.insert(x2, x2_val);
|
||||
cfg_pair.insert(l1, l1_val);
|
||||
cfg_pair.insert(l2, l2_val);
|
||||
|
||||
// initialize configs by slicing a larger config
|
||||
TupPoseConfig cfg_pose(cfg_pair);
|
||||
TupPointConfig cfg_point(cfg_pair);
|
||||
|
||||
PointConfig expPointConfig;
|
||||
expPointConfig.insert(l1, l1_val);
|
||||
expPointConfig.insert(l2, l2_val);
|
||||
|
||||
PoseConfig expPoseConfig;
|
||||
expPoseConfig.insert(x1, x1_val);
|
||||
expPoseConfig.insert(x2, x2_val);
|
||||
|
||||
// verify
|
||||
EXPECT(assert_equal(expPointConfig, cfg_point.config<PointConfig>()));
|
||||
EXPECT(assert_equal(expPoseConfig, cfg_pose.config<PoseConfig>()));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testFusionTupleConfig, upgrading ) {
|
||||
TupPoseConfig small;
|
||||
small.insert(x1, x1_val);
|
||||
|
||||
TupPairConfig actual(small);
|
||||
EXPECT(actual.size() == 1);
|
||||
EXPECT(assert_equal(x1_val, actual.at(x1), tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testFusionTupleConfig, equals ) {
|
||||
TupPairConfig c1, c2, c3, c4, c5, c6;
|
||||
c1.insert(l1, l1_val);
|
||||
c1.insert(l2, l2_val);
|
||||
c1.insert(x1, x1_val);
|
||||
c1.insert(x2, x2_val);
|
||||
c4 = c1;
|
||||
c2.insert(x1, x1_val);
|
||||
c2.insert(x2, x2_val);
|
||||
|
||||
EXPECT(assert_equal(c1, c1, tol));
|
||||
EXPECT(assert_equal(c4, c1, tol));
|
||||
EXPECT(assert_equal(c4, c4, tol));
|
||||
EXPECT(!c1.equals(c2, tol));
|
||||
EXPECT(!c2.equals(c1, tol));
|
||||
EXPECT(!c1.equals(c3, tol));
|
||||
EXPECT(!c2.equals(c3, tol));
|
||||
EXPECT(assert_equal(c3, c3));
|
||||
|
||||
c5.insert(l1, l1_val);
|
||||
c6.insert(l1, l1_val + Point2(1e-6, 1e-6));
|
||||
EXPECT(assert_equal(c5, c6, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testFusionTupleConfig, insert_equals1 )
|
||||
{
|
||||
TupPairConfig expected;
|
||||
expected.insert(PoseKey(1), x1_val);
|
||||
expected.insert(PoseKey(2), x2_val);
|
||||
expected.insert(PointKey(1), l1_val);
|
||||
expected.insert(PointKey(2), l2_val);
|
||||
|
||||
TupPairConfig actual;
|
||||
actual.insert(PoseKey(1), x1_val);
|
||||
actual.insert(PoseKey(2), x2_val);
|
||||
actual.insert(PointKey(1), l1_val);
|
||||
actual.insert(PointKey(2), l2_val);
|
||||
|
||||
CHECK(assert_equal(expected,actual));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testFusionTupleConfig, insert_equals2 )
|
||||
{
|
||||
TupPairConfig config1;
|
||||
config1.insert(PoseKey(1), x1_val);
|
||||
config1.insert(PoseKey(2), x2_val);
|
||||
config1.insert(PointKey(1), l1_val);
|
||||
config1.insert(PointKey(2), l2_val);
|
||||
|
||||
TupPairConfig config2;
|
||||
config2.insert(PoseKey(1), x1_val);
|
||||
config2.insert(PoseKey(2), x2_val);
|
||||
config2.insert(PointKey(1), l1_val);
|
||||
|
||||
EXPECT(!config1.equals(config2));
|
||||
|
||||
config2.insert(PointKey(2), Point2(9,11));
|
||||
|
||||
EXPECT(!config1.equals(config2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testFusionTupleConfig, insert_duplicate )
|
||||
{
|
||||
TupPairConfig config1;
|
||||
config1.insert(x1, x1_val); // 3
|
||||
config1.insert(x2, x2_val); // 6
|
||||
config1.insert(l1, l1_val); // 8
|
||||
config1.insert(l2, l2_val); // 10
|
||||
config1.insert(l2, l1_val); // still 10 !!!!
|
||||
|
||||
EXPECT(assert_equal(l2_val, config1[PointKey(2)]));
|
||||
LONGS_EQUAL(4,config1.size());
|
||||
LONGS_EQUAL(10,config1.dim());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testFusionTupleConfig, size_dim )
|
||||
{
|
||||
TupPairConfig config1;
|
||||
config1.insert(PoseKey(1), x1_val);
|
||||
config1.insert(PoseKey(2), x2_val);
|
||||
config1.insert(PointKey(1), l1_val);
|
||||
config1.insert(PointKey(2), l2_val);
|
||||
|
||||
EXPECT(config1.size() == 4);
|
||||
EXPECT(config1.dim() == 10);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testFusionTupleConfig, at)
|
||||
{
|
||||
TupPairConfig config1;
|
||||
config1.insert(PoseKey(1), x1_val);
|
||||
config1.insert(PoseKey(2), x2_val);
|
||||
config1.insert(PointKey(1), l1_val);
|
||||
config1.insert(PointKey(2), l2_val);
|
||||
|
||||
EXPECT(assert_equal(x1_val, config1[PoseKey(1)]));
|
||||
EXPECT(assert_equal(x2_val, config1[PoseKey(2)]));
|
||||
EXPECT(assert_equal(l1_val, config1[PointKey(1)]));
|
||||
EXPECT(assert_equal(l2_val, config1[PointKey(2)]));
|
||||
|
||||
CHECK_EXCEPTION(config1[PoseKey(3)], std::invalid_argument);
|
||||
CHECK_EXCEPTION(config1[PointKey(3)], std::invalid_argument);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testFusionTupleConfig, zero_expmap_logmap)
|
||||
{
|
||||
Pose2 xA1(1,2,3), xA2(6,7,8);
|
||||
Point2 lA1(4,5), lA2(9,10);
|
||||
|
||||
TupPairConfig config1;
|
||||
config1.insert(PoseKey(1), xA1);
|
||||
config1.insert(PoseKey(2), xA2);
|
||||
config1.insert(PointKey(1), lA1);
|
||||
config1.insert(PointKey(2), lA2);
|
||||
|
||||
VectorConfig expected_zero;
|
||||
expected_zero.insert("x1", zero(3));
|
||||
expected_zero.insert("x2", zero(3));
|
||||
expected_zero.insert("l1", zero(2));
|
||||
expected_zero.insert("l2", zero(2));
|
||||
|
||||
EXPECT(assert_equal(expected_zero, config1.zero()));
|
||||
|
||||
VectorConfig delta;
|
||||
delta.insert("x1", Vector_(3, 1.0, 1.1, 1.2));
|
||||
delta.insert("x2", Vector_(3, 1.3, 1.4, 1.5));
|
||||
delta.insert("l1", Vector_(2, 1.0, 1.1));
|
||||
delta.insert("l2", Vector_(2, 1.3, 1.4));
|
||||
|
||||
TupPairConfig expected;
|
||||
expected.insert(PoseKey(1), expmap(xA1, Vector_(3, 1.0, 1.1, 1.2)));
|
||||
expected.insert(PoseKey(2), expmap(xA2, Vector_(3, 1.3, 1.4, 1.5)));
|
||||
expected.insert(PointKey(1), Point2(5.0, 6.1));
|
||||
expected.insert(PointKey(2), Point2(10.3, 11.4));
|
||||
|
||||
TupPairConfig actual = expmap(config1, delta);
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
|
||||
// Check log
|
||||
VectorConfig expected_log = delta;
|
||||
VectorConfig actual_log = logmap(config1,actual);
|
||||
EXPECT(assert_equal(expected_log, actual_log));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testFusionTupleConfig, partial_insert) {
|
||||
TupPairConfig init, expected;
|
||||
|
||||
Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0);
|
||||
Point2 point1(2.0, 3.0), point2(5.0, 6.0);
|
||||
|
||||
init.insert(x1, pose1);
|
||||
init.insert(l1, point1);
|
||||
|
||||
PoseConfig cfg1;
|
||||
cfg1.insert(x2, pose2);
|
||||
|
||||
init.insertSub(cfg1);
|
||||
|
||||
expected.insert(x1, pose1);
|
||||
expected.insert(l1, point1);
|
||||
expected.insert(x2, pose2);
|
||||
|
||||
CHECK(assert_equal(expected, init));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testFusionTupleConfig, update) {
|
||||
TupPairConfig init, superset, expected;
|
||||
|
||||
Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0);
|
||||
Point2 point1(2.0, 3.0), point2(5.0, 6.0);
|
||||
|
||||
init.insert(x1, pose1);
|
||||
init.insert(l1, point1);
|
||||
|
||||
Pose2 pose1_(1.0, 2.0, 0.4);
|
||||
Point2 point1_(2.0, 4.0);
|
||||
superset.insert(x1, pose1_);
|
||||
superset.insert(l1, point1_);
|
||||
superset.insert(x2, pose2);
|
||||
superset.insert(l2, point2);
|
||||
init.update(superset);
|
||||
|
||||
expected.insert(x1, pose1_);
|
||||
expected.insert(l1, point1_);
|
||||
|
||||
CHECK(assert_equal(expected, init));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testFusionTupleConfig, update_element )
|
||||
{
|
||||
TupPairConfig cfg;
|
||||
PoseKey xk(1);
|
||||
PointKey lk(1);
|
||||
|
||||
cfg.insert(xk, x1_val);
|
||||
EXPECT(cfg.size() == 1);
|
||||
EXPECT(assert_equal(x1_val, cfg.at(xk)));
|
||||
|
||||
cfg.update(xk, x2_val);
|
||||
EXPECT(cfg.size() == 1);
|
||||
EXPECT(assert_equal(x2_val, cfg.at(xk)));
|
||||
|
||||
cfg.insert(lk, l1_val);
|
||||
EXPECT(cfg.size() == 2);
|
||||
EXPECT(assert_equal(l1_val, cfg.at(lk)));
|
||||
|
||||
cfg.update(lk, l2_val);
|
||||
EXPECT(cfg.size() == 2);
|
||||
EXPECT(assert_equal(l2_val, cfg.at(lk)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testFusionTupleConfig, expmap)
|
||||
{
|
||||
Pose2 xA1(1,2,3), xA2(6,7,8);
|
||||
PoseKey x1k(1), x2k(2);
|
||||
Point2 lA1(4,5), lA2(9,10);
|
||||
PointKey l1k(1), l2k(2);
|
||||
|
||||
TupPairConfig config1;
|
||||
config1.insert(x1k, xA1);
|
||||
config1.insert(x2k, xA2);
|
||||
config1.insert(l1k, lA1);
|
||||
config1.insert(l2k, lA2);
|
||||
|
||||
VectorConfig delta;
|
||||
delta.insert("x1", Vector_(3, 1.0, 1.1, 1.2));
|
||||
delta.insert("x2", Vector_(3, 1.3, 1.4, 1.5));
|
||||
delta.insert("l1", Vector_(2, 1.0, 1.1));
|
||||
delta.insert("l2", Vector_(2, 1.3, 1.4));
|
||||
|
||||
TupPairConfig expected;
|
||||
expected.insert(x1k, expmap(xA1, Vector_(3, 1.0, 1.1, 1.2)));
|
||||
expected.insert(x2k, expmap(xA2, Vector_(3, 1.3, 1.4, 1.5)));
|
||||
expected.insert(l1k, Point2(5.0, 6.1));
|
||||
expected.insert(l2k, Point2(10.3, 11.4));
|
||||
|
||||
CHECK(assert_equal(expected, expmap(config1, delta)));
|
||||
CHECK(assert_equal(delta, logmap(config1, expected)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testFusionTupleConfig, configN)
|
||||
{
|
||||
typedef FusionTupleConfig2<PoseConfig, PointConfig> ConfigA;
|
||||
PointConfig expPointConfig;
|
||||
expPointConfig.insert(l1, l1_val);
|
||||
expPointConfig.insert(l2, l2_val);
|
||||
|
||||
PoseConfig expPoseConfig;
|
||||
expPoseConfig.insert(x1, x1_val);
|
||||
expPoseConfig.insert(x2, x2_val);
|
||||
|
||||
ConfigA cfg1;
|
||||
EXPECT(cfg1.empty());
|
||||
|
||||
ConfigA cfg2(expPoseConfig, expPointConfig);
|
||||
|
||||
EXPECT(assert_equal(expPoseConfig, cfg2.config<PoseConfig>()));
|
||||
EXPECT(assert_equal(expPointConfig, cfg2.config<PointConfig>()));
|
||||
|
||||
EXPECT(assert_equal(expPoseConfig, cfg2.first()));
|
||||
EXPECT(assert_equal(expPointConfig, cfg2.second()));
|
||||
|
||||
ConfigA cfg3(cfg2);
|
||||
EXPECT(assert_equal(cfg2, cfg3));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( testFusionTupleConfig, basic_factor)
|
||||
{
|
||||
// planar example system
|
||||
typedef FusionTupleConfig2<PoseConfig, PointConfig> Config; // pair config
|
||||
typedef FusionTupleConfig1<PoseConfig> TestPoseConfig;
|
||||
typedef NonlinearFactorGraph<Config> Graph;
|
||||
typedef NonlinearOptimizer<Graph,Config> Optimizer;
|
||||
|
||||
// Factors
|
||||
// typedef PriorFactor<TestPoseConfig, PoseKey, Pose2> Prior; // fails to add to graph
|
||||
typedef PriorFactor<Config, PoseKey> Prior;
|
||||
typedef BetweenFactor<Config, PoseKey> Odometry;
|
||||
typedef BearingRangeFactor<Config, PoseKey, PointKey> BearingRange;
|
||||
|
||||
PoseKey pose1k(1), pose2k(2), pose3k(3);
|
||||
Pose2 pose1, pose2(2.0, 0.0, 0.0), pose3(4.0, 0.0, 0.0);
|
||||
SharedDiagonal prior_model = noiseModel::Isotropic::Sigma(3, 0.1);
|
||||
SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
|
||||
|
||||
Graph graph;
|
||||
graph.add(Prior(pose1k, pose1, prior_model));
|
||||
graph.add(Odometry(pose1k, pose2k, between(pose1, pose2), odom_model));
|
||||
graph.add(Odometry(pose2k, pose3k, between(pose2, pose3), odom_model));
|
||||
|
||||
Config init;
|
||||
init.insert(pose1k, Pose2(0.2, 0.4, 0.0));
|
||||
init.insert(pose2k, Pose2(1.8,-0.4, 0.3));
|
||||
init.insert(pose3k, Pose2(4.1, 0.0,-0.2));
|
||||
|
||||
Optimizer::shared_config actual = Optimizer::optimizeLM(graph, init);
|
||||
|
||||
Config expected;
|
||||
expected.insert(pose1k, pose1);
|
||||
expected.insert(pose2k, pose2);
|
||||
expected.insert(pose3k, pose3);
|
||||
|
||||
EXPECT(assert_equal(expected, *actual, tol));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue