diff --git a/.cproject b/.cproject index 1d070ceed..f54554f76 100644 --- a/.cproject +++ b/.cproject @@ -21,7 +21,7 @@ - + @@ -39,6 +39,7 @@ + @@ -71,7 +72,7 @@ - + @@ -98,11 +99,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + + + @@ -258,6 +311,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -284,7 +345,6 @@ make - tests/testBayesTree.run true false @@ -292,7 +352,6 @@ make - testBinaryBayesNet.run true false @@ -340,7 +399,6 @@ make - testSymbolicBayesNet.run true false @@ -348,7 +406,6 @@ make - tests/testSymbolicFactor.run true false @@ -356,7 +413,6 @@ make - testSymbolicFactorGraph.run true false @@ -372,20 +428,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -412,6 +459,7 @@ make + testGraph.run true false @@ -483,6 +531,7 @@ make + testInference.run true false @@ -490,6 +539,7 @@ make + testGaussianFactor.run true false @@ -497,6 +547,7 @@ make + testJunctionTree.run true false @@ -504,6 +555,7 @@ make + testSymbolicBayesNet.run true false @@ -511,6 +563,7 @@ make + testSymbolicFactorGraph.run true false @@ -580,22 +633,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -612,6 +649,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -764,14 +817,6 @@ true true - - make - -j2 - vSFMexample.run - true - true - true - make -j2 @@ -780,6 +825,14 @@ true true + + make + -j2 + vSFMexample.run + true + true + true + make -j2 @@ -1038,7 +1091,6 @@ make - testErrors.run true false @@ -1494,6 +1546,7 @@ make + testSimulated2DOriented.run true false @@ -1533,6 +1586,7 @@ make + testSimulated2D.run true false @@ -1540,6 +1594,7 @@ make + testSimulated3D.run true false @@ -2170,6 +2225,7 @@ make + tests/testGaussianISAM2 true false @@ -2231,6 +2287,21 @@ true true + + cmake + .. + true + false + true + + + make + + nonlinear.testValues.run + true + true + true + make -j2 @@ -2359,6 +2430,13 @@ true true + + cmake + .. + true + false + true + make -j2 diff --git a/.project b/.project index d20fd358e..9856df2ea 100644 --- a/.project +++ b/.project @@ -23,7 +23,7 @@ org.eclipse.cdt.make.core.buildArguments - -j2 + -j5 org.eclipse.cdt.make.core.buildCommand @@ -31,7 +31,7 @@ org.eclipse.cdt.make.core.buildLocation - ${workspace_loc:/gtsam/build} + ${ProjDirPath}/build org.eclipse.cdt.make.core.cleanBuildTarget diff --git a/CppUnitLite/Makefile.am b/CppUnitLite/Makefile.am deleted file mode 100644 index 3e4dbdb05..000000000 --- a/CppUnitLite/Makefile.am +++ /dev/null @@ -1,22 +0,0 @@ -#---------------------------------------------------------------------------------------------------- -# CppUnitLite -# replaced Makefile with automake for easy linking -#---------------------------------------------------------------------------------------------------- - -headers = TestHarness.h -sources = Failure.cpp SimpleString.cpp Test.cpp TestRegistry.cpp TestResult.cpp -headers += $(sources:.cpp=.h) - -if ENABLE_INSTALL_CPPUNITLITE -CppUnitLitedir = $(includedir)/CppUnitLite -lib_LIBRARIES = libCppUnitLite.a -CppUnitLite_HEADERS = $(headers) -libCppUnitLite_a_SOURCES = $(sources) -else -noinst_LIBRARIES = libCppUnitLite.a -noinst_HEADERS = $(headers) -libCppUnitLite_a_SOURCES = $(sources) -endif - - - diff --git a/Makefile.am b/Makefile.am deleted file mode 100644 index 8e68e7d6d..000000000 --- a/Makefile.am +++ /dev/null @@ -1,32 +0,0 @@ -#---------------------------------------------------------------------------------------------------- -# GTSAM top-level automake file -#---------------------------------------------------------------------------------------------------- - -#The option -I m4 tells Autoconf to look for additional Autoconf macros in the m4 subdirectory. -ACLOCAL_AMFLAGS = -I m4 - -# make automake install some standard but missing files -# also use nostdinc to turn off -I. and -I.., we do not need them because -# header files are qualified so they can be included in external projects. -AUTOMAKE_OPTIONS = foreign nostdinc - -# For Doxygen integration -#include aminclude.am - -# All the sub-directories that need to be built -SUBDIRS = CppUnitLite gtsam tests examples - -if ENABLE_BUILD_TOOLBOX -SUBDIRS += wrap -endif - -# Add these files to make sure they're in the distribution -EXTRA_DIST = autogen.sh configure.ac COPYING INSTALL LGPL LICENSE README THANKS USAGE doc - -# For Doxygen integration -#MOSTLYCLEANFILES = $(DX_CLEANFILES) -#EXTRA_DIST += $(DX_CONFIG) - -# Remove .svn directories from dist -dist-hook: - rm -rf `find $(distdir)/doc -type d -name .svn` diff --git a/aminclude.am b/aminclude.am deleted file mode 100644 index 09e6bf065..000000000 --- a/aminclude.am +++ /dev/null @@ -1,186 +0,0 @@ -# Copyright (C) 2004 Oren Ben-Kiki -# This file is distributed under the same terms as the Automake macro files. - -# Generate automatic documentation using Doxygen. Goals and variables values -# are controlled by the various DX_COND_??? conditionals set by autoconf. -# -# The provided goals are: -# doc: Generate all doxygen documentation. -# doxygen-run: Run doxygen, which will generate some of the documentation -# (HTML, CHM, CHI, MAN, RTF, XML) but will not do the post -# processing required for the rest of it (PS, PDF, and some MAN). -# doxygen-man: Rename some doxygen generated man pages. -# doxygen-ps: Generate doxygen PostScript documentation. -# doxygen-pdf: Generate doxygen PDF documentation. -# -# Note that by default these are not integrated into the automake goals. If -# doxygen is used to generate man pages, you can achieve this integration by -# setting man3_MANS to the list of man pages generated and then adding the -# dependency: -# -# $(man3_MANS): doxygen-doc -# -# This will cause make to run doxygen and generate all the documentation. -# -# The following variable is intended for use in Makefile.am: -# -# DX_CLEANFILES = everything to clean. -# -# This is usually added to MOSTLYCLEANFILES. - -## --------------------------------- ## -## Format-independent Doxygen rules. ## -## --------------------------------- ## - -if DX_COND_doc - -## ------------------------------- ## -## Rules specific for HTML output. ## -## ------------------------------- ## - -if DX_COND_html - -DX_CLEAN_HTML = @DX_DOCDIR@/html - -endif DX_COND_html - -## ------------------------------ ## -## Rules specific for CHM output. ## -## ------------------------------ ## - -if DX_COND_chm - -DX_CLEAN_CHM = @DX_DOCDIR@/chm - -if DX_COND_chi - -DX_CLEAN_CHI = @DX_DOCDIR@/@PACKAGE@.chi - -endif DX_COND_chi - -endif DX_COND_chm - -## ------------------------------ ## -## Rules specific for MAN output. ## -## ------------------------------ ## - -if DX_COND_man - -DX_CLEAN_MAN = @DX_DOCDIR@/man - -endif DX_COND_man - -## ------------------------------ ## -## Rules specific for RTF output. ## -## ------------------------------ ## - -if DX_COND_rtf - -DX_CLEAN_RTF = @DX_DOCDIR@/rtf - -endif DX_COND_rtf - -## ------------------------------ ## -## Rules specific for XML output. ## -## ------------------------------ ## - -if DX_COND_xml - -DX_CLEAN_XML = @DX_DOCDIR@/xml - -endif DX_COND_xml - -## ----------------------------- ## -## Rules specific for PS output. ## -## ----------------------------- ## - -if DX_COND_ps - -DX_CLEAN_PS = @DX_DOCDIR@/@PACKAGE@.ps - -DX_PS_GOAL = doxygen-ps - -doxygen-ps: @DX_DOCDIR@/@PACKAGE@.ps - -@DX_DOCDIR@/@PACKAGE@.ps: @DX_DOCDIR@/@PACKAGE@.tag - cd @DX_DOCDIR@/latex; \ - rm -f *.aux *.toc *.idx *.ind *.ilg *.log *.out; \ - $(DX_LATEX) refman.tex; \ - $(MAKEINDEX_PATH) refman.idx; \ - $(DX_LATEX) refman.tex; \ - countdown=5; \ - while $(DX_EGREP) 'Rerun (LaTeX|to get cross-references right)' \ - refman.log > /dev/null 2>&1 \ - && test $$countdown -gt 0; do \ - $(DX_LATEX) refman.tex; \ - countdown=`expr $$countdown - 1`; \ - done; \ - $(DX_DVIPS) -o ../@PACKAGE@.ps refman.dvi - -endif DX_COND_ps - -## ------------------------------ ## -## Rules specific for PDF output. ## -## ------------------------------ ## - -if DX_COND_pdf - -DX_CLEAN_PDF = @DX_DOCDIR@/@PACKAGE@.pdf - -DX_PDF_GOAL = doxygen-pdf - -doxygen-pdf: @DX_DOCDIR@/@PACKAGE@.pdf - -@DX_DOCDIR@/@PACKAGE@.pdf: @DX_DOCDIR@/@PACKAGE@.tag - cd @DX_DOCDIR@/latex; \ - rm -f *.aux *.toc *.idx *.ind *.ilg *.log *.out; \ - $(DX_PDFLATEX) refman.tex; \ - $(DX_MAKEINDEX) refman.idx; \ - $(DX_PDFLATEX) refman.tex; \ - countdown=5; \ - while $(DX_EGREP) 'Rerun (LaTeX|to get cross-references right)' \ - refman.log > /dev/null 2>&1 \ - && test $$countdown -gt 0; do \ - $(DX_PDFLATEX) refman.tex; \ - countdown=`expr $$countdown - 1`; \ - done; \ - mv refman.pdf ../@PACKAGE@.pdf - -endif DX_COND_pdf - -## ------------------------------------------------- ## -## Rules specific for LaTeX (shared for PS and PDF). ## -## ------------------------------------------------- ## - -if DX_COND_latex - -DX_CLEAN_LATEX = @DX_DOCDIR@/latex - -endif DX_COND_latex - -.PHONY: doxygen-run doc $(DX_PS_GOAL) $(DX_PDF_GOAL) - -.INTERMEDIATE: doxygen-run $(DX_PS_GOAL) $(DX_PDF_GOAL) - -doxygen-run: @DX_DOCDIR@/@PACKAGE@.tag - -doc: doxygen-run $(DX_PS_GOAL) $(DX_PDF_GOAL) - -@DX_DOCDIR@/@PACKAGE@.tag: $(DX_CONFIG) $(pkginclude_HEADERS) - rm -rf @DX_DOCDIR@ - $(DX_ENV) $(DX_DOXYGEN) $(srcdir)/$(DX_CONFIG) - -DX_CLEANFILES = \ - @DX_DOCDIR@/@PACKAGE@.tag \ - -r \ - $(DX_CLEAN_HTML) \ - $(DX_CLEAN_CHM) \ - $(DX_CLEAN_CHI) \ - $(DX_CLEAN_MAN) \ - $(DX_CLEAN_RTF) \ - $(DX_CLEAN_XML) \ - $(DX_CLEAN_PS) \ - $(DX_CLEAN_PDF) \ - $(DX_CLEAN_LATEX) - -endif DX_COND_doc diff --git a/autogen.sh b/autogen.sh deleted file mode 100755 index fd0be89ce..000000000 --- a/autogen.sh +++ /dev/null @@ -1,2 +0,0 @@ -#!/bin/sh -autoreconf --force --install -I config -I m4 diff --git a/configure.ac b/configure.ac deleted file mode 100644 index 6bcbfe015..000000000 --- a/configure.ac +++ /dev/null @@ -1,191 +0,0 @@ -# -*- Autoconf -*- -# Process this file with autoconf to produce a configure script. - -AC_PREREQ(2.59) -AC_INIT(gtsam, 0.9.3, dellaert@cc.gatech.edu) -AM_INIT_AUTOMAKE(gtsam, 0.9.3) -AC_CONFIG_MACRO_DIR([m4]) -AC_CONFIG_HEADER([config.h]) -AC_CONFIG_SRCDIR([CppUnitLite/Test.cpp]) -AC_CONFIG_SRCDIR([wrap/wrap.cpp]) -AC_CONFIG_SRCDIR([gtsam/base/DSFVector.cpp]) -AC_CONFIG_SRCDIR([gtsam/geometry/Cal3_S2.cpp]) -AC_CONFIG_SRCDIR([gtsam/inference/SymbolicFactorGraph.cpp]) -AC_CONFIG_SRCDIR([gtsam/linear/GaussianFactor.cpp]) -AC_CONFIG_SRCDIR([gtsam/nonlinear/NonlinearOptimizer.cpp]) -AC_CONFIG_SRCDIR([gtsam/slam/pose2SLAM.cpp]) -AC_CONFIG_SRCDIR([tests/testTupleValues.cpp]) -AC_CONFIG_SRCDIR([examples/SimpleRotation.cpp]) -AC_CONFIG_SRCDIR([gtsam/3rdparty/Makefile.am]) - -# For doxygen support -#DX_HTML_FEATURE(ON) -#DX_CHM_FEATURE(OFF) -#DX_CHI_FEATURE(OFF) -#DX_MAN_FEATURE(OFF) -#DX_RTF_FEATURE(OFF) -#DX_XML_FEATURE(OFF) -#DX_PDF_FEATURE(OFF) -#DX_PS_FEATURE(OFF) -#DX_INIT_DOXYGEN(gtsam, Doxyfile, doc) - -# Check for OS -# needs to be called at some point earlier -AC_CANONICAL_HOST -AM_CONDITIONAL([DARWIN], [case $host_os in darwin*) true;; *) false;; esac]) -AM_CONDITIONAL([LINUX], [case $host_os in linux*) true;; *) false;; esac]) -AM_CONDITIONAL([IS_64BIT], [case $host_cpu in *x86_64*) true;; *) false;; esac]) - -# enable debug variable -AC_ARG_ENABLE([debug], - [ --enable-debug Turn on debugging], - [case "${enableval}" in - yes) debug=true ;; - no) debug=false ;; - *) AC_MSG_ERROR([bad value ${enableval} for --enable-debug]) ;; - esac],[debug=false]) - -AM_CONDITIONAL([DEBUG], [test x$debug = xtrue]) - -# enable profiling -AC_ARG_ENABLE([profiling], - [ --enable-profiling Enable profiling], - [case "${enableval}" in - yes) profiling=true ;; - no) profiling=false ;; - *) AC_MSG_ERROR([bad value ${enableval} for --enable-profiling]) ;; - esac],[profiling=false]) - -AM_CONDITIONAL([USE_PROFILING], [test x$profiling = xtrue]) - -# enable serialization in serialization test -AC_ARG_ENABLE([serialization], - [ --enable-serialization Enable serialization with boost serialization], - [case "${enableval}" in - yes) serialization=true ;; - no) serialization=false ;; - *) AC_MSG_ERROR([bad value ${enableval} for --enable-serialization]) ;; - esac],[serialization=false]) - -AM_CONDITIONAL([ENABLE_SERIALIZATION], [test x$serialization = xtrue]) - -# enable installation of CppUnitLite with gtsam -AC_ARG_ENABLE([install_cppunitlite], - [ --enable-install-cppunitlite Enable installation of CppUnitLite], - [case "${enableval}" in - yes) install_cppunitlite=true ;; - no) install_cppunitlite=false ;; - *) AC_MSG_ERROR([bad value ${enableval} for --enable-install-cppunitlite]) ;; - esac],[install_cppunitlite=false]) - -AM_CONDITIONAL([ENABLE_INSTALL_CPPUNITLITE], [test x$install_cppunitlite = xtrue]) - -# enable matlab toolbox generation -AC_ARG_ENABLE([build_toolbox], - [ --enable-build-toolbox Enable building of the Matlab toolbox], - [case "${enableval}" in - yes) build_toolbox=true ;; - no) build_toolbox=false ;; - *) AC_MSG_ERROR([bad value ${enableval} for --enable-build-toolbox]) ;; - esac],[build_toolbox=false]) - -AM_CONDITIONAL([ENABLE_BUILD_TOOLBOX], [test x$build_toolbox = xtrue]) - -# enable installation of matlab tests -AC_ARG_ENABLE([install_matlab_tests], - [ --enable-install-matlab-tests Enable installation of tests for the Matlab toolbox], - [case "${enableval}" in - yes) install_matlab_tests=true ;; - no) install_matlab_tests=false ;; - *) AC_MSG_ERROR([bad value ${enableval} for --enable-install-matlab-tests]) ;; - esac],[install_matlab_tests=true]) - -AM_CONDITIONAL([ENABLE_INSTALL_MATLAB_TESTS], [test x$install_matlab_tests = xtrue]) - -# enable installation of matlab examples -AC_ARG_ENABLE([install_matlab_examples], - [ --enable-install-matlab-examples Enable installation of examples for the Matlab toolbox], - [case "${enableval}" in - yes) install_matlab_examples=true ;; - no) install_matlab_examples=false ;; - *) AC_MSG_ERROR([bad value ${enableval} for --enable-install-matlab-examples]) ;; - esac],[install_matlab_examples=true]) - -AM_CONDITIONAL([ENABLE_INSTALL_MATLAB_EXAMPLES], [test x$install_matlab_examples = xtrue]) - -# Matlab toolbox: optional flag to change location of toolbox, defaults to install prefix -AC_ARG_WITH([toolbox], - [AS_HELP_STRING([--with-toolbox], - [specify the matlab toolbox directory for installation])], - [toolbox=$withval], - [toolbox=$prefix]) -AC_SUBST([toolbox]) - -# enable installation of the wrap utility -AC_ARG_ENABLE([install_wrap], - [ --enable-install-wrap Enable installation of the wrap tool for generating matlab interfaces], - [case "${enableval}" in - yes) install_wrap=true ;; - no) install_wrap=false ;; - *) AC_MSG_ERROR([bad value ${enableval} for --enable-install-wrap]) ;; - esac],[install_wrap=false]) - -AM_CONDITIONAL([ENABLE_INSTALL_WRAP], [test x$install_wrap = xtrue]) - -# enable unsafe mode for wrap -AC_ARG_ENABLE([unsafe_wrap], - [ --enable-unsafe-wrap Enable using unsafe mode in wrap], - [case "${enableval}" in - yes) unsafe_wrap=true ;; - no) unsafe_wrap=false ;; - *) AC_MSG_ERROR([bad value ${enableval} for --enable-unsafe-wrap]) ;; - esac],[unsafe_wrap=false]) - -AM_CONDITIONAL([ENABLE_UNSAFE_WRAP], [test x$unsafe_wrap = xtrue]) - -# wrap install path: optional flag to change location of wrap, defaults to install prefix/bin -AC_ARG_WITH([wrap], - [AS_HELP_STRING([--with-wrap], - [specify the wrap directory for installation])], - [wrap=$withval], - [wrap=$prefix/bin]) -AC_SUBST([wrap]) - -# Checks for programs. -AC_PROG_CXX -AC_PROG_CC - -# Checks for libraries. -LT_INIT - -# Checks for header files. -AC_HEADER_STDC -AC_CHECK_HEADERS([string.h]) - -# Checks for typedefs, structures, and compiler characteristics. -AC_HEADER_STDBOOL -AC_C_CONST -AC_C_INLINE -AC_TYPE_SIZE_T - -# Checks for library functions. -AC_FUNC_ERROR_AT_LINE -AC_CHECK_FUNCS([pow sqrt]) - -# Check for boost -AX_BOOST_BASE([1.40]) - -AC_CONFIG_FILES([CppUnitLite/Makefile \ -wrap/Makefile \ -gtsam/3rdparty/Makefile \ -gtsam/base/Makefile \ -gtsam/geometry/Makefile \ -gtsam/inference/Makefile \ -gtsam/linear/Makefile \ -gtsam/nonlinear/Makefile \ -gtsam/slam/Makefile gtsam/Makefile \ -tests/Makefile \ -examples/Makefile \ -examples/vSLAMexample/Makefile \ -Makefile]) -AC_OUTPUT diff --git a/examples/CameraResectioning.cpp b/examples/CameraResectioning.cpp index aee073df7..f00509c3b 100644 --- a/examples/CameraResectioning.cpp +++ b/examples/CameraResectioning.cpp @@ -16,38 +16,35 @@ * @date Aug 23, 2011 */ -#include +#include #include #include #include -#include #include #include #include using namespace gtsam; -typedef TypedSymbol PoseKey; -typedef Values PoseValues; - /** * Unary factor for the pose. */ -class ResectioningFactor: public NonlinearFactor1 { - typedef NonlinearFactor1 Base; +class ResectioningFactor: public NoiseModelFactor1 { + typedef NoiseModelFactor1 Base; shared_ptrK K_; // camera's intrinsic parameters Point3 P_; // 3D point on the calibration rig Point2 p_; // 2D measurement of the 3D point public: - ResectioningFactor(const SharedNoiseModel& model, const PoseKey& key, + ResectioningFactor(const SharedNoiseModel& model, const Symbol& key, const shared_ptrK& calib, const Point2& p, const Point3& P) : Base(model, key), K_(calib), P_(P), p_(p) { } - virtual Vector evaluateError(const Pose3& X, boost::optional H = - boost::none) const { + virtual ~ResectioningFactor() {} + + virtual Vector evaluateError(const Pose3& X, boost::optional H = boost::none) const { SimpleCamera camera(*K_, X); Point2 reprojectionError(camera.project(P_, H) - p_); return reprojectionError.vector(); @@ -69,10 +66,10 @@ int main(int argc, char* argv[]) { /* create keys for variables */ // we have only 1 variable to solve: the camera pose - PoseKey X(1); + Symbol X('x',1); /* 1. create graph */ - NonlinearFactorGraph graph; + NonlinearFactorGraph graph; /* 2. add factors to the graph */ // add measurement factors @@ -92,14 +89,13 @@ int main(int argc, char* argv[]) { graph.push_back(factor); /* 3. Create an initial estimate for the camera pose */ - PoseValues initial; + Values initial; initial.insert(X, Pose3(Rot3(1.,0.,0., 0.,-1.,0., 0.,0.,-1.), Point3(0.,0.,2.0))); /* 4. Optimize the graph using Levenberg-Marquardt*/ - PoseValues result = optimize , PoseValues> ( - graph, initial); + Values result = optimize (graph, initial); result.print("Final result: "); return 0; diff --git a/examples/Makefile.am b/examples/Makefile.am deleted file mode 100644 index 14b6fb935..000000000 --- a/examples/Makefile.am +++ /dev/null @@ -1,45 +0,0 @@ -#---------------------------------------------------------------------------------------------------- -# GTSAM Examples -#---------------------------------------------------------------------------------------------------- - -# use nostdinc to turn off -I. and -I.., we do not need them because -# header files are qualified so they can be included in external projects. -AUTOMAKE_OPTIONS = nostdinc - -headers = -sources = -check_PROGRAMS = - -# Examples -noinst_PROGRAMS = SimpleRotation # Optimizes a single nonlinear rotation variable -noinst_PROGRAMS += PlanarSLAMExample_easy # Solves SLAM example from tutorial by using planarSLAM -noinst_PROGRAMS += PlanarSLAMSelfContained_advanced # Solves SLAM example from tutorial with all typedefs in the script -noinst_PROGRAMS += Pose2SLAMExample_easy # Solves SLAM example from tutorial by using Pose2SLAM and easy optimization interface -noinst_PROGRAMS += Pose2SLAMExample_advanced # Solves SLAM example from tutorial by using Pose2SLAM and advanced optimization interface -noinst_PROGRAMS += Pose2SLAMwSPCG_easy # Solves a simple Pose2 SLAM example with advanced SPCG solver -noinst_PROGRAMS += Pose2SLAMwSPCG_advanced # Solves a simple Pose2 SLAM example with easy SPCG solver -noinst_PROGRAMS += elaboratePoint2KalmanFilter # simple linear Kalman filter on a moving 2D point, but done using factor graphs -noinst_PROGRAMS += easyPoint2KalmanFilter # uses the cool generic templated Kalman filter class to do the same -noinst_PROGRAMS += CameraResectioning - -EXTRA_DIST = Data -dist-hook: - rm -rf $(distdir)/Data/.svn - -SUBDIRS = vSLAMexample # visual SLAM examples with 3D point landmarks and 6D camera poses -#---------------------------------------------------------------------------------------------------- -# rules to build local programs -#---------------------------------------------------------------------------------------------------- -AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) -AM_LDFLAGS = $(BOOST_LDFLAGS) -LDADD = ../gtsam/libgtsam.la -AM_DEFAULT_SOURCE_EXT = .cpp - -# rule to run an executable -%.run: % $(LDADD) - ./$^ - -# rule to run executable with valgrind -%.valgrind: % $(LDADD) - valgrind ./% -#---------------------------------------------------------------------------------------------------- diff --git a/examples/PlanarSLAMExample_easy.cpp b/examples/PlanarSLAMExample_easy.cpp index d29b21fa9..1ff3a8677 100644 --- a/examples/PlanarSLAMExample_easy.cpp +++ b/examples/PlanarSLAMExample_easy.cpp @@ -36,25 +36,22 @@ using namespace planarSLAM; * - Landmarks are 2 meters away from the robot trajectory */ int main(int argc, char** argv) { - // create keys for variables - PoseKey x1(1), x2(2), x3(3); - PointKey l1(1), l2(2); - // create graph container and add factors to it + // create graph container and add factors to it Graph graph; /* add prior */ // gaussian for prior SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin - graph.addPrior(x1, prior_measurement, prior_model); // add directly to graph + graph.addPrior(1, prior_measurement, prior_model); // add directly to graph /* add odometry */ // general noisemodel for odometry SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - graph.addOdometry(x1, x2, odom_measurement, odom_model); - graph.addOdometry(x2, x3, odom_measurement, odom_model); + graph.addOdometry(1, 2, odom_measurement, odom_model); + graph.addOdometry(2, 3, odom_measurement, odom_model); /* add measurements */ // general noisemodel for measurements @@ -69,24 +66,24 @@ int main(int argc, char** argv) { range32 = 2.0; // create bearing/range factors and add them - graph.addBearingRange(x1, l1, bearing11, range11, meas_model); - graph.addBearingRange(x2, l1, bearing21, range21, meas_model); - graph.addBearingRange(x3, l2, bearing32, range32, meas_model); + graph.addBearingRange(1, 1, bearing11, range11, meas_model); + graph.addBearingRange(2, 1, bearing21, range21, meas_model); + graph.addBearingRange(3, 2, bearing32, range32, meas_model); graph.print("full graph"); // initialize to noisy points planarSLAM::Values initialEstimate; - initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2)); - initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2)); - initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1)); - initialEstimate.insert(l1, Point2(1.8, 2.1)); - initialEstimate.insert(l2, Point2(4.1, 1.8)); + initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2)); + initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2)); + initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1)); + initialEstimate.insertPoint(1, Point2(1.8, 2.1)); + initialEstimate.insertPoint(2, Point2(4.1, 1.8)); initialEstimate.print("initial estimate"); // optimize using Levenberg-Marquardt optimization with an ordering from colamd - planarSLAM::Values result = optimize(graph, initialEstimate); + planarSLAM::Values result = optimize(graph, initialEstimate); result.print("final result"); return 0; diff --git a/examples/PlanarSLAMSelfContained_advanced.cpp b/examples/PlanarSLAMSelfContained_advanced.cpp index d915a9ebd..65b201ab7 100644 --- a/examples/PlanarSLAMSelfContained_advanced.cpp +++ b/examples/PlanarSLAMSelfContained_advanced.cpp @@ -19,7 +19,7 @@ #include // for all nonlinear keys -#include +#include // for points and poses #include @@ -34,21 +34,14 @@ #include // implementations for structures - needed if self-contained, and these should be included last -#include #include #include #include #include // Main typedefs -typedef gtsam::TypedSymbol PoseKey; // Key for poses, with type included -typedef gtsam::TypedSymbol PointKey; // Key for points, with type included -typedef gtsam::Values PoseValues; // config type for poses -typedef gtsam::Values PointValues; // config type for points -typedef gtsam::TupleValues2 PlanarValues; // main config with two variable classes -typedef gtsam::NonlinearFactorGraph Graph; // graph structure -typedef gtsam::NonlinearOptimizer OptimizerSeqential; // optimization engine for this domain -typedef gtsam::NonlinearOptimizer OptimizerMultifrontal; // optimization engine for this domain +typedef gtsam::NonlinearOptimizer OptimizerSeqential; // optimization engine for this domain +typedef gtsam::NonlinearOptimizer OptimizerMultifrontal; // optimization engine for this domain using namespace std; using namespace gtsam; @@ -64,17 +57,17 @@ using namespace gtsam; */ int main(int argc, char** argv) { // create keys for variables - PoseKey x1(1), x2(2), x3(3); - PointKey l1(1), l2(2); + Symbol x1('x',1), x2('x',2), x3('x',3); + Symbol l1('l',1), l2('l',2); // create graph container and add factors to it - Graph::shared_ptr graph(new Graph); + NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph); /* add prior */ // gaussian for prior SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin - PriorFactor posePrior(x1, prior_measurement, prior_model); // create the factor + PriorFactor posePrior(x1, prior_measurement, prior_model); // create the factor graph->add(posePrior); // add the factor to the graph /* add odometry */ @@ -82,8 +75,8 @@ int main(int argc, char** argv) { SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1)); Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) // create between factors to represent odometry - BetweenFactor odom12(x1, x2, odom_measurement, odom_model); - BetweenFactor odom23(x2, x3, odom_measurement, odom_model); + BetweenFactor odom12(x1, x2, odom_measurement, odom_model); + BetweenFactor odom23(x2, x3, odom_measurement, odom_model); graph->add(odom12); // add both to graph graph->add(odom23); @@ -100,9 +93,9 @@ int main(int argc, char** argv) { range32 = 2.0; // create bearing/range factors - BearingRangeFactor meas11(x1, l1, bearing11, range11, meas_model); - BearingRangeFactor meas21(x2, l1, bearing21, range21, meas_model); - BearingRangeFactor meas32(x3, l2, bearing32, range32, meas_model); + BearingRangeFactor meas11(x1, l1, bearing11, range11, meas_model); + BearingRangeFactor meas21(x2, l1, bearing21, range21, meas_model); + BearingRangeFactor meas32(x3, l2, bearing32, range32, meas_model); // add the factors graph->add(meas11); @@ -112,7 +105,7 @@ int main(int argc, char** argv) { graph->print("Full Graph"); // initialize to noisy points - boost::shared_ptr initial(new PlanarValues); + boost::shared_ptr initial(new Values); initial->insert(x1, Pose2(0.5, 0.0, 0.2)); initial->insert(x2, Pose2(2.3, 0.1,-0.2)); initial->insert(x3, Pose2(4.1, 0.1, 0.1)); diff --git a/examples/Pose2SLAMExample_advanced.cpp b/examples/Pose2SLAMExample_advanced.cpp index bb7f60516..1c196488f 100644 --- a/examples/Pose2SLAMExample_advanced.cpp +++ b/examples/Pose2SLAMExample_advanced.cpp @@ -33,9 +33,6 @@ using namespace boost; using namespace pose2SLAM; int main(int argc, char** argv) { - // create keys for robot positions - PoseKey x1(1), x2(2), x3(3); - /* 1. create graph container and add factors to it */ shared_ptr graph(new Graph); @@ -43,7 +40,7 @@ int main(int argc, char** argv) { // gaussian for prior SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin - graph->addPrior(x1, prior_measurement, prior_model); // add directly to graph + graph->addPrior(1, prior_measurement, prior_model); // add directly to graph /* 2.b add odometry */ // general noisemodel for odometry @@ -51,16 +48,16 @@ int main(int argc, char** argv) { /* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/ Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - graph->addOdometry(x1, x2, odom_measurement, odom_model); - graph->addOdometry(x2, x3, odom_measurement, odom_model); + graph->addOdometry(1, 2, odom_measurement, odom_model); + graph->addOdometry(2, 3, odom_measurement, odom_model); graph->print("full graph"); /* 3. Create the data structure to hold the initial estimate to the solution * initialize to noisy points */ shared_ptr initial(new pose2SLAM::Values); - initial->insert(x1, Pose2(0.5, 0.0, 0.2)); - initial->insert(x2, Pose2(2.3, 0.1,-0.2)); - initial->insert(x3, Pose2(4.1, 0.1, 0.1)); + initial->insertPose(1, Pose2(0.5, 0.0, 0.2)); + initial->insertPose(2, Pose2(2.3, 0.1,-0.2)); + initial->insertPose(3, Pose2(4.1, 0.1, 0.1)); initial->print("initial estimate"); /* 4.2.1 Alternatively, you can go through the process step by step @@ -68,7 +65,7 @@ int main(int argc, char** argv) { Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initial); /* 4.2.2 set up solver and optimize */ - NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15); + NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15); Optimizer optimizer(graph, initial, ordering, params); Optimizer optimizer_result = optimizer.levenbergMarquardt(); @@ -76,8 +73,8 @@ int main(int argc, char** argv) { result.print("final result"); /* Get covariances */ - Matrix covariance1 = optimizer_result.marginalCovariance(x1); - Matrix covariance2 = optimizer_result.marginalCovariance(x2); + Matrix covariance1 = optimizer_result.marginalCovariance(PoseKey(1)); + Matrix covariance2 = optimizer_result.marginalCovariance(PoseKey(1)); print(covariance1, "Covariance1"); print(covariance2, "Covariance2"); diff --git a/examples/Pose2SLAMExample_easy.cpp b/examples/Pose2SLAMExample_easy.cpp index ab23436b0..2593c043c 100644 --- a/examples/Pose2SLAMExample_easy.cpp +++ b/examples/Pose2SLAMExample_easy.cpp @@ -31,8 +31,6 @@ using namespace gtsam; using namespace pose2SLAM; int main(int argc, char** argv) { - // create keys for robot positions - PoseKey x1(1), x2(2), x3(3); /* 1. create graph container and add factors to it */ Graph graph ; @@ -41,7 +39,7 @@ int main(int argc, char** argv) { // gaussian for prior SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1)); Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin - graph.addPrior(x1, prior_measurement, prior_model); // add directly to graph + graph.addPrior(1, prior_measurement, prior_model); // add directly to graph /* 2.b add odometry */ // general noisemodel for odometry @@ -49,21 +47,21 @@ int main(int argc, char** argv) { /* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/ Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) - graph.addOdometry(x1, x2, odom_measurement, odom_model); - graph.addOdometry(x2, x3, odom_measurement, odom_model); + graph.addOdometry(1, 2, odom_measurement, odom_model); + graph.addOdometry(2, 3, odom_measurement, odom_model); graph.print("full graph"); /* 3. Create the data structure to hold the initial estinmate to the solution * initialize to noisy points */ pose2SLAM::Values initial; - initial.insert(x1, Pose2(0.5, 0.0, 0.2)); - initial.insert(x2, Pose2(2.3, 0.1,-0.2)); - initial.insert(x3, Pose2(4.1, 0.1, 0.1)); + initial.insertPose(1, Pose2(0.5, 0.0, 0.2)); + initial.insertPose(2, Pose2(2.3, 0.1,-0.2)); + initial.insertPose(3, Pose2(4.1, 0.1, 0.1)); initial.print("initial estimate"); /* 4 Single Step Optimization * optimize using Levenberg-Marquardt optimization with an ordering from colamd */ - pose2SLAM::Values result = optimize(graph, initial); + pose2SLAM::Values result = optimize(graph, initial); result.print("final result"); diff --git a/examples/Pose2SLAMwSPCG_advanced.cpp b/examples/Pose2SLAMwSPCG_advanced.cpp index a04083986..12d17a6e9 100644 --- a/examples/Pose2SLAMwSPCG_advanced.cpp +++ b/examples/Pose2SLAMwSPCG_advanced.cpp @@ -31,17 +31,17 @@ using namespace gtsam; using namespace pose2SLAM; typedef boost::shared_ptr sharedGraph ; -typedef boost::shared_ptr sharedValue ; +typedef boost::shared_ptr sharedValue ; //typedef NonlinearOptimizer > SPCGOptimizer; -typedef SubgraphSolver Solver; +typedef SubgraphSolver Solver; typedef boost::shared_ptr sharedSolver ; -typedef NonlinearOptimizer SPCGOptimizer; +typedef NonlinearOptimizer SPCGOptimizer; sharedGraph graph; sharedValue initial; -pose2SLAM::Values result; +Values result; /* ************************************************************************* */ int main(void) { @@ -51,7 +51,7 @@ int main(void) { Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9); graph = boost::make_shared() ; - initial = boost::make_shared() ; + initial = boost::make_shared() ; // create a 3 by 3 grid // x3 x6 x9 diff --git a/examples/Pose2SLAMwSPCG_easy.cpp b/examples/Pose2SLAMwSPCG_easy.cpp index 131b19443..e610d327c 100644 --- a/examples/Pose2SLAMwSPCG_easy.cpp +++ b/examples/Pose2SLAMwSPCG_easy.cpp @@ -31,7 +31,7 @@ using namespace gtsam; using namespace pose2SLAM; Graph graph; -pose2SLAM::Values initial, result; +Values initial, result; /* ************************************************************************* */ int main(void) { diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index 9d897dd39..71727f750 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -22,8 +22,7 @@ #include #include #include -#include -#include +#include #include #include @@ -36,26 +35,6 @@ using namespace std; using namespace gtsam; -/** - * Step 1: Setup basic types for optimization of a single variable type - * This can be considered to be specifying the domain of the problem we wish - * to solve. In this case, we will create a very simple domain that operates - * on variables of a specific type, in this case, Rot2. - * - * To create a domain: - * - variable types need to have a key defined to act as a label in graphs - * - a "RotValues" structure needs to be defined to store the system state - * - a graph container acting on a given RotValues - * - * In a typical scenario, these typedefs could be placed in a header - * file and reused between projects. Also, RotValues can be combined to - * form a "TupleValues" to enable multiple variable types, such as 2D points - * and 2D poses. - */ -typedef TypedSymbol Key; /// Variable labels for a specific type -typedef Values RotValues; /// Class to store values - acts as a state for the system -typedef NonlinearFactorGraph Graph; /// Graph container for constraints - needs to know type of variables - const double degree = M_PI / 180; int main() { @@ -66,7 +45,7 @@ int main() { */ /** - * Step 2: create a factor on to express a unary constraint + * Step 1: create a factor on to express a unary constraint * The "prior" in this case is the measurement from a sensor, * with a model of the noise on the measurement. * @@ -82,11 +61,11 @@ int main() { Rot2 prior = Rot2::fromAngle(30 * degree); prior.print("goal angle"); SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 1 * degree); - Key key(1); - PriorFactor factor(key, prior, model); + Symbol key('x',1); + PriorFactor factor(key, prior, model); /** - * Step 3: create a graph container and add the factor to it + * Step 2: create a graph container and add the factor to it * Before optimizing, all factors need to be added to a Graph container, * which provides the necessary top-level functionality for defining a * system of constraints. @@ -94,12 +73,12 @@ int main() { * In this case, there is only one factor, but in a practical scenario, * many more factors would be added. */ - Graph graph; + NonlinearFactorGraph graph; graph.add(factor); graph.print("full graph"); /** - * Step 4: create an initial estimate + * Step 3: create an initial estimate * An initial estimate of the solution for the system is necessary to * start optimization. This system state is the "RotValues" structure, * which is similar in structure to a STL map, in that it maps @@ -114,19 +93,19 @@ int main() { * on the type of key used to find the appropriate value map if there * are multiple types of variables. */ - RotValues initial; + Values initial; initial.insert(key, Rot2::fromAngle(20 * degree)); initial.print("initial estimate"); /** - * Step 5: optimize + * Step 4: optimize * After formulating the problem with a graph of constraints * and an initial estimate, executing optimization is as simple * as calling a general optimization function with the graph and * initial estimate. This will yield a new RotValues structure * with the final state of the optimization. */ - RotValues result = optimize(graph, initial); + Values result = optimize(graph, initial); result.print("final result"); return 0; diff --git a/examples/easyPoint2KalmanFilter.cpp b/examples/easyPoint2KalmanFilter.cpp index f22d65f95..b13058a07 100644 --- a/examples/easyPoint2KalmanFilter.cpp +++ b/examples/easyPoint2KalmanFilter.cpp @@ -24,15 +24,12 @@ #include #include #include -#include #include using namespace std; using namespace gtsam; // Define Types for Linear System Test -typedef TypedSymbol LinearKey; -typedef Values LinearValues; typedef Point2 LinearMeasurement; int main() { @@ -42,7 +39,7 @@ int main() { SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); // Create an ExtendedKalmanFilter object - ExtendedKalmanFilter ekf(x_initial, P_initial); + ExtendedKalmanFilter ekf(x_initial, P_initial); @@ -63,11 +60,11 @@ int main() { // This simple motion can be modeled with a BetweenFactor // Create Keys - LinearKey x0(0), x1(1); + Symbol x0('x',0), x1('x',1); // Predict delta based on controls Point2 difference(1,0); // Create Factor - BetweenFactor factor1(x0, x1, difference, Q); + BetweenFactor factor1(x0, x1, difference, Q); // Predict the new value with the EKF class Point2 x1_predict = ekf.predict(factor1); @@ -88,7 +85,7 @@ int main() { // This simple measurement can be modeled with a PriorFactor Point2 z1(1.0, 0.0); - PriorFactor factor2(x1, z1, R); + PriorFactor factor2(x1, z1, R); // Update the Kalman Filter with the measurement Point2 x1_update = ekf.update(factor2); @@ -98,15 +95,15 @@ int main() { // Do the same thing two more times... // Predict - LinearKey x2(2); + Symbol x2('x',2); difference = Point2(1,0); - BetweenFactor factor3(x1, x2, difference, Q); + BetweenFactor factor3(x1, x2, difference, Q); Point2 x2_predict = ekf.predict(factor1); x2_predict.print("X2 Predict"); // Update Point2 z2(2.0, 0.0); - PriorFactor factor4(x2, z2, R); + PriorFactor factor4(x2, z2, R); Point2 x2_update = ekf.update(factor4); x2_update.print("X2 Update"); @@ -114,15 +111,15 @@ int main() { // Do the same thing one more time... // Predict - LinearKey x3(3); + Symbol x3('x',3); difference = Point2(1,0); - BetweenFactor factor5(x2, x3, difference, Q); + BetweenFactor factor5(x2, x3, difference, Q); Point2 x3_predict = ekf.predict(factor5); x3_predict.print("X3 Predict"); // Update Point2 z3(3.0, 0.0); - PriorFactor factor6(x3, z3, R); + PriorFactor factor6(x3, z3, R); Point2 x3_update = ekf.update(factor6); x3_update.print("X3 Update"); diff --git a/examples/elaboratePoint2KalmanFilter.cpp b/examples/elaboratePoint2KalmanFilter.cpp index 8c8cbe147..ee0d82501 100644 --- a/examples/elaboratePoint2KalmanFilter.cpp +++ b/examples/elaboratePoint2KalmanFilter.cpp @@ -22,9 +22,8 @@ #include #include -#include #include -#include +#include #include #include #include @@ -34,9 +33,6 @@ using namespace std; using namespace gtsam; -typedef TypedSymbol Key; /// Variable labels for a specific type -typedef Values KalmanValues; /// Class to store values - acts as a state for the system - int main() { // [code below basically does SRIF with LDL] @@ -48,7 +44,7 @@ int main() { Ordering::shared_ptr ordering(new Ordering); // Create a structure to hold the linearization points - KalmanValues linearizationPoints; + Values linearizationPoints; // Ground truth example // Start at origin, move to the right (x-axis): 0,0 0,1 0,2 @@ -57,14 +53,14 @@ int main() { // i.e., we should get 0,0 0,1 0,2 if there is no noise // Create new state variable - Key x0(0); + Symbol x0('x',0); ordering->insert(x0, 0); // Initialize state x0 (2D point) at origin by adding a prior factor, i.e., Bayes net P(x0) // This is equivalent to x_0 and P_0 Point2 x_initial(0,0); SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); - PriorFactor factor1(x0, x_initial, P_initial); + PriorFactor factor1(x0, x_initial, P_initial); // Linearize the factor and add it to the linear factor graph linearizationPoints.insert(x0, x_initial); linearFactorGraph->push_back(factor1.linearize(linearizationPoints, *ordering)); @@ -90,12 +86,12 @@ int main() { // so, difference = x_{t+1} - x_{t} = F*x_{t} + B*u_{t} - I*x_{t} // = (F - I)*x_{t} + B*u_{t} // = B*u_{t} (for our example) - Key x1(1); + Symbol x1('x',1); ordering->insert(x1, 1); Point2 difference(1,0); SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); - BetweenFactor factor2(x0, x1, difference, Q); + BetweenFactor factor2(x0, x1, difference, Q); // Linearize the factor and add it to the linear factor graph linearizationPoints.insert(x1, x_initial); linearFactorGraph->push_back(factor2.linearize(linearizationPoints, *ordering)); @@ -118,7 +114,7 @@ int main() { // Extract the current estimate of x1,P1 from the Bayes Network VectorValues result = optimize(*linearBayesNet); - Point2 x1_predict = linearizationPoints[x1].retract(result[ordering->at(x1)]); + Point2 x1_predict = linearizationPoints.at(x1).retract(result[ordering->at(x1)]); x1_predict.print("X1 Predict"); // Update the new linearization point to the new estimate @@ -173,7 +169,7 @@ int main() { // This can be modeled using the PriorFactor, where the mean is z_{t} and the covariance is R. Point2 z1(1.0, 0.0); SharedDiagonal R1 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25)); - PriorFactor factor4(x1, z1, R1); + PriorFactor factor4(x1, z1, R1); // Linearize the factor and add it to the linear factor graph linearFactorGraph->push_back(factor4.linearize(linearizationPoints, *ordering)); @@ -190,7 +186,7 @@ int main() { // Extract the current estimate of x1 from the Bayes Network result = optimize(*linearBayesNet); - Point2 x1_update = linearizationPoints[x1].retract(result[ordering->at(x1)]); + Point2 x1_update = linearizationPoints.at(x1).retract(result[ordering->at(x1)]); x1_update.print("X1 Update"); // Update the linearization point to the new estimate @@ -215,7 +211,7 @@ int main() { linearFactorGraph->add(0, tmpPrior1.getA(tmpPrior1.begin()), tmpPrior1.getb() - tmpPrior1.getA(tmpPrior1.begin()) * result[ordering->at(x1)], tmpPrior1.get_model()); // Create a key for the new state - Key x2(2); + Symbol x2('x',2); // Create the desired ordering ordering = Ordering::shared_ptr(new Ordering); @@ -225,7 +221,7 @@ int main() { // Create a nonlinear factor describing the motion model difference = Point2(1,0); Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); - BetweenFactor factor6(x1, x2, difference, Q); + BetweenFactor factor6(x1, x2, difference, Q); // Linearize the factor and add it to the linear factor graph linearizationPoints.insert(x2, x1_update); @@ -237,7 +233,7 @@ int main() { // Extract the current estimate of x2 from the Bayes Network result = optimize(*linearBayesNet); - Point2 x2_predict = linearizationPoints[x2].retract(result[ordering->at(x2)]); + Point2 x2_predict = linearizationPoints.at(x2).retract(result[ordering->at(x2)]); x2_predict.print("X2 Predict"); // Update the linearization point to the new estimate @@ -263,7 +259,7 @@ int main() { // And update using z2 ... Point2 z2(2.0, 0.0); SharedDiagonal R2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25)); - PriorFactor factor8(x2, z2, R2); + PriorFactor factor8(x2, z2, R2); // Linearize the factor and add it to the linear factor graph linearFactorGraph->push_back(factor8.linearize(linearizationPoints, *ordering)); @@ -281,7 +277,7 @@ int main() { // Extract the current estimate of x2 from the Bayes Network result = optimize(*linearBayesNet); - Point2 x2_update = linearizationPoints[x2].retract(result[ordering->at(x2)]); + Point2 x2_update = linearizationPoints.at(x2).retract(result[ordering->at(x2)]); x2_update.print("X2 Update"); // Update the linearization point to the new estimate @@ -304,7 +300,7 @@ int main() { linearFactorGraph->add(0, tmpPrior3.getA(tmpPrior3.begin()), tmpPrior3.getb() - tmpPrior3.getA(tmpPrior3.begin()) * result[ordering->at(x2)], tmpPrior3.get_model()); // Create a key for the new state - Key x3(3); + Symbol x3('x',3); // Create the desired ordering ordering = Ordering::shared_ptr(new Ordering); @@ -314,7 +310,7 @@ int main() { // Create a nonlinear factor describing the motion model difference = Point2(1,0); Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); - BetweenFactor factor10(x2, x3, difference, Q); + BetweenFactor factor10(x2, x3, difference, Q); // Linearize the factor and add it to the linear factor graph linearizationPoints.insert(x3, x2_update); @@ -326,7 +322,7 @@ int main() { // Extract the current estimate of x3 from the Bayes Network result = optimize(*linearBayesNet); - Point2 x3_predict = linearizationPoints[x3].retract(result[ordering->at(x3)]); + Point2 x3_predict = linearizationPoints.at(x3).retract(result[ordering->at(x3)]); x3_predict.print("X3 Predict"); // Update the linearization point to the new estimate @@ -352,7 +348,7 @@ int main() { // And update using z3 ... Point2 z3(3.0, 0.0); SharedDiagonal R3 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25)); - PriorFactor factor12(x3, z3, R3); + PriorFactor factor12(x3, z3, R3); // Linearize the factor and add it to the linear factor graph linearFactorGraph->push_back(factor12.linearize(linearizationPoints, *ordering)); @@ -370,7 +366,7 @@ int main() { // Extract the current estimate of x2 from the Bayes Network result = optimize(*linearBayesNet); - Point2 x3_update = linearizationPoints[x3].retract(result[ordering->at(x3)]); + Point2 x3_update = linearizationPoints.at(x3).retract(result[ordering->at(x3)]); x3_update.print("X3 Update"); // Update the linearization point to the new estimate diff --git a/examples/matlab/PlanarSLAMExample_easy.m b/examples/matlab/PlanarSLAMExample_easy.m index 3e165e975..54462732b 100644 --- a/examples/matlab/PlanarSLAMExample_easy.m +++ b/examples/matlab/PlanarSLAMExample_easy.m @@ -28,20 +28,20 @@ graph = planarSLAMGraph; %% Add prior % gaussian for prior -prior_model = gtsamSharedNoiseModel_sharedSigmas([0.3; 0.3; 0.1]); +prior_model = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]); prior_measurement = gtsamPose2(0.0, 0.0, 0.0); % prior at origin graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph %% Add odometry % general noisemodel for odometry -odom_model = gtsamSharedNoiseModel_sharedSigmas([0.2; 0.2; 0.1]); +odom_model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); odom_measurement = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) graph.addOdometry(x1, x2, odom_measurement, odom_model); graph.addOdometry(x2, x3, odom_measurement, odom_model); %% Add measurements % general noisemodel for measurements -meas_model = gtsamSharedNoiseModel_sharedSigmas([0.1; 0.2]); +meas_model = gtsamSharedNoiseModel_Sigmas([0.1; 0.2]); % create the measurement values - indices are (pose id, landmark id) degrees = pi/180; diff --git a/examples/matlab/Pose2SLAMExample_advanced.m b/examples/matlab/Pose2SLAMExample_advanced.m index 7a7e10802..3edb34782 100644 --- a/examples/matlab/Pose2SLAMExample_advanced.m +++ b/examples/matlab/Pose2SLAMExample_advanced.m @@ -28,20 +28,20 @@ graph = pose2SLAMGraph; %% Add prior % gaussian for prior -prior_model = gtsamSharedNoiseModel_sharedSigmas([0.3; 0.3; 0.1]); +prior_model = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]); prior_measurement = gtsamPose2(0.0, 0.0, 0.0); % prior at origin graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph %% Add odometry % general noisemodel for odometry -odom_model = gtsamSharedNoiseModel_sharedSigmas([0.2; 0.2; 0.1]); +odom_model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); odom_measurement = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) graph.addOdometry(x1, x2, odom_measurement, odom_model); graph.addOdometry(x2, x3, odom_measurement, odom_model); %% Add measurements % general noisemodel for measurements -meas_model = gtsamSharedNoiseModel_sharedSigmas([0.1; 0.2]); +meas_model = gtsamSharedNoiseModel_Sigmas([0.1; 0.2]); % print graph.print('full graph'); @@ -55,19 +55,19 @@ initialEstimate.insertPose(x3, gtsamPose2(4.1, 0.1, 0.1)); initialEstimate.print('initial estimate'); %% set up solver, choose ordering and optimize -params = NonlinearOptimizationParameters_newDrecreaseThresholds(1e-15, 1e-15); +params = gtsamNonlinearOptimizationParameters_newDecreaseThresholds(1e-15, 1e-15); ord = graph.orderingCOLAMD(initialEstimate); -result = pose2SLAMOptimizer(graph,initialEstimate,ord,params); - -result.print('final result'); +%result = pose2SLAMOptimizer(graph,initialEstimate,ord,params); +%result.print('final result'); + % % % % disp('\\\'); % % % % %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd -% % result = graph.optimize(initialEstimate); -% % result.print('final result'); +result = graph.optimize(initialEstimate); +result.print('final result'); %% Get the corresponding dense matrix ord = graph.orderingCOLAMD(result); diff --git a/examples/matlab/Pose2SLAMExample_easy.m b/examples/matlab/Pose2SLAMExample_easy.m index 8df9c1c0e..bbafba99d 100644 --- a/examples/matlab/Pose2SLAMExample_easy.m +++ b/examples/matlab/Pose2SLAMExample_easy.m @@ -26,13 +26,13 @@ graph = pose2SLAMGraph; %% Add prior % gaussian for prior -prior_model = gtsamSharedNoiseModel_sharedSigmas([0.3; 0.3; 0.1]); +prior_model = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]); prior_measurement = gtsamPose2(0.0, 0.0, 0.0); % prior at origin graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph %% Add odometry % general noisemodel for odometry -odom_model = gtsamSharedNoiseModel_sharedSigmas([0.2; 0.2; 0.1]); +odom_model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]); odom_measurement = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case) graph.addOdometry(x1, x2, odom_measurement, odom_model); graph.addOdometry(x2, x3, odom_measurement, odom_model); diff --git a/examples/vSLAMexample/Makefile.am b/examples/vSLAMexample/Makefile.am deleted file mode 100644 index 625c57564..000000000 --- a/examples/vSLAMexample/Makefile.am +++ /dev/null @@ -1,39 +0,0 @@ -#---------------------------------------------------------------------------------------------------- -# GTSAM Examples -#---------------------------------------------------------------------------------------------------- - -# use nostdinc to turn off -I. and -I.., we do not need them because -# header files are qualified so they can be included in external projects. -AUTOMAKE_OPTIONS = nostdinc - -headers = -sources = -check_PROGRAMS = - -# Examples -noinst_PROGRAMS = vSFMexample -vSFMexample_SOURCES = vSFMexample.cpp vSLAMutils.cpp - -noinst_PROGRAMS += vISAMexample -vISAMexample_SOURCES = vISAMexample.cpp vSLAMutils.cpp - -headers += Feature2D.h vSLAMutils.h - -noinst_HEADERS = $(headers) - -#---------------------------------------------------------------------------------------------------- -# rules to build local programs -#---------------------------------------------------------------------------------------------------- -AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) -AM_LDFLAGS = $(BOOST_LDFLAGS) -LDADD = ../../gtsam/libgtsam.la -AM_DEFAULT_SOURCE_EXT = .cpp - -# rule to run an executable -%.run: % $(LDADD) - ./% - -# rule to run executable with valgrind -%.valgrind: % $(LDADD) - valgrind ./$^ -#---------------------------------------------------------------------------------------------------- diff --git a/examples/vSLAMexample/vISAMexample.cpp b/examples/vSLAMexample/vISAMexample.cpp index 8027f3088..71d5516a2 100644 --- a/examples/vSLAMexample/vISAMexample.cpp +++ b/examples/vSLAMexample/vISAMexample.cpp @@ -20,9 +20,6 @@ #include using namespace boost; -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include #include #include @@ -79,7 +76,7 @@ void readAllDataISAM() { /** * Setup newFactors and initialValues for each new pose and set of measurements at each frame. */ -void createNewFactors(shared_ptr& newFactors, boost::shared_ptr& initialValues, +void createNewFactors(shared_ptr& newFactors, boost::shared_ptr& initialValues, int pose_id, const Pose3& pose, const std::vector& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) { // Create a graph of newFactors with new measurements @@ -100,10 +97,10 @@ void createNewFactors(shared_ptr& newFactors, boost::shared_ptr (new visualSLAM::Values()); - initialValues->insert(pose_id, pose); + initialValues = shared_ptr (new Values()); + initialValues->insert(PoseKey(pose_id), pose); for (size_t i = 0; i < measurements.size(); i++) { - initialValues->insert(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]); + initialValues->insert(PointKey(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]); } } @@ -123,7 +120,7 @@ int main(int argc, char* argv[]) { // Create a NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates int relinearizeInterval = 3; - NonlinearISAM isam(relinearizeInterval); + NonlinearISAM<> isam(relinearizeInterval); // At each frame (poseId) with new camera pose and set of associated measurements, // create a graph of new factors and update ISAM @@ -131,12 +128,12 @@ int main(int argc, char* argv[]) { BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) { const int poseId = features.first; shared_ptr newFactors; - shared_ptr initialValues; + shared_ptr initialValues; createNewFactors(newFactors, initialValues, poseId, g_poses[poseId], features.second, measurementSigma, g_calib); isam.update(*newFactors, *initialValues); - visualSLAM::Values currentEstimate = isam.estimate(); + Values currentEstimate = isam.estimate(); cout << "****************************************************" << endl; currentEstimate.print("Current estimate: "); } diff --git a/examples/vSLAMexample/vSFMexample.cpp b/examples/vSLAMexample/vSFMexample.cpp index 8aaf7be0a..c7e0df05c 100644 --- a/examples/vSLAMexample/vSFMexample.cpp +++ b/examples/vSLAMexample/vSFMexample.cpp @@ -19,9 +19,6 @@ #include using namespace boost; -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include #include #include @@ -102,17 +99,17 @@ Graph setupGraph(std::vector& measurements, SharedNoiseModel measurem * Create a structure of initial estimates for all nodes (landmarks and poses) in the graph. * The returned Values structure contains all initial values for all nodes. */ -visualSLAM::Values initialize(std::map landmarks, std::map poses) { +Values initialize(std::map landmarks, std::map poses) { - visualSLAM::Values initValues; + Values initValues; // Initialize landmarks 3D positions. for (map::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++) - initValues.insert(lmit->first, lmit->second); + initValues.insert(PointKey(lmit->first), lmit->second); // Initialize camera poses. for (map::iterator poseit = poses.begin(); poseit != poses.end(); poseit++) - initValues.insert(poseit->first, poseit->second); + initValues.insert(PoseKey(poseit->first), poseit->second); return initValues; } @@ -135,7 +132,7 @@ int main(int argc, char* argv[]) { boost::shared_ptr graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib))); // Create an initial Values structure using groundtruth values as the initial estimates - boost::shared_ptr initialEstimates(new visualSLAM::Values(initialize(g_landmarks, g_poses))); + boost::shared_ptr initialEstimates(new Values(initialize(g_landmarks, g_poses))); cout << "*******************************************************" << endl; initialEstimates->print("INITIAL ESTIMATES: "); diff --git a/gtsam.h b/gtsam.h index c17906df1..688fc4fa7 100644 --- a/gtsam.h +++ b/gtsam.h @@ -251,13 +251,13 @@ class SharedDiagonal { }; class SharedNoiseModel { - static gtsam::SharedNoiseModel sharedSigmas(Vector sigmas); - static gtsam::SharedNoiseModel sharedSigma(size_t dim, double sigma); - static gtsam::SharedNoiseModel sharedPrecisions(Vector precisions); - static gtsam::SharedNoiseModel sharedPrecision(size_t dim, double precision); - static gtsam::SharedNoiseModel sharedUnit(size_t dim); - static gtsam::SharedNoiseModel sharedSqrtInformation(Matrix R); - static gtsam::SharedNoiseModel sharedCovariance(Matrix covariance); + static gtsam::SharedNoiseModel Sigmas(Vector sigmas); + static gtsam::SharedNoiseModel Sigma(size_t dim, double sigma); + static gtsam::SharedNoiseModel Precisions(Vector precisions); + static gtsam::SharedNoiseModel Precision(size_t dim, double precision); + static gtsam::SharedNoiseModel Unit(size_t dim); + static gtsam::SharedNoiseModel SqrtInformation(Matrix R); + static gtsam::SharedNoiseModel Covariance(Matrix covariance); void print(string s) const; }; @@ -411,6 +411,8 @@ class NonlinearOptimizationParameters { NonlinearOptimizationParameters(double absDecrease, double relDecrease, double sumError, int iIters, double lambda, double lambdaFactor); void print(string s) const; + static gtsam::NonlinearOptimizationParameters* newDecreaseThresholds(double absDecrease, + double relDecrease); }; @@ -659,8 +661,8 @@ class Graph { // GaussianFactor* linearize(const gtsam::Pose2Values& config) const; //}; // -//class gtsam::Pose2Graph{ -// Pose2Graph(); +//class gtsam::pose2SLAM::Graph{ +// pose2SLAM::Graph(); // void print(string s) const; // GaussianFactorGraph* linearize_(const gtsam::Pose2Values& config) const; // void push_back(Pose2Factor* factor); diff --git a/gtsam/3rdparty/Makefile.am b/gtsam/3rdparty/Makefile.am deleted file mode 100644 index e5a930ca5..000000000 --- a/gtsam/3rdparty/Makefile.am +++ /dev/null @@ -1,287 +0,0 @@ -# 3rd Party libraries to be built and installed along with gtsam - -# use nostdinc to turn off -I. and -I.., we do not need them because -# header files are qualified so they can be included in external projects. -AUTOMAKE_OPTIONS = nostdinc - -# set up the folders for includes -3rdpartydir = $(pkgincludedir)/3rdparty -3rdparty_includedir = $(includedir)/gtsam/3rdparty -nobase_3rdparty_HEADERS = - -# CCOLAMD (with UFconfig files included) -# FIXME: ccolamd requires a -I setting for every header file -ccolamd_inc = $(top_srcdir)/gtsam/3rdparty/CCOLAMD/Include -ufconfig_inc = $(top_srcdir)/gtsam/3rdparty/UFconfig -headers = CCOLAMD/Include/ccolamd.h UFconfig/UFconfig.h -sources = CCOLAMD/Source/ccolamd.c CCOLAMD/Source/ccolamd_global.c UFconfig/UFconfig.c - -#---------------------------------------------------------------------------------------------------- -# Create a libtool library that is not installed -# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am -# The headers are installed in $(includedir)/gtsam/3rdparty: -#---------------------------------------------------------------------------------------------------- -nobase_3rdparty_HEADERS += $(headers) -noinst_LTLIBRARIES = libccolamd.la -libccolamd_la_SOURCES = $(sources) - -AM_CPPFLAGS = -AM_CPPFLAGS += -I$(ccolamd_inc) -I$(ufconfig_inc) $(BOOST_CPPFLAGS) -I$(top_srcdir) -AM_LDFLAGS = $(BOOST_LDFLAGS) - -# Eigen Installation - just copies the headers -eigen_path = -eigen_path += Eigen/Eigen -nobase_3rdparty_HEADERS += $(eigen_path)/Array $(eigen_path)/LeastSquares -nobase_3rdparty_HEADERS += $(eigen_path)/Cholesky $(eigen_path)/LU -nobase_3rdparty_HEADERS += $(eigen_path)/Core $(eigen_path)/QR -nobase_3rdparty_HEADERS += $(eigen_path)/Dense $(eigen_path)/QtAlignedMalloc -nobase_3rdparty_HEADERS += $(eigen_path)/Eigen $(eigen_path)/Sparse -nobase_3rdparty_HEADERS += $(eigen_path)/Eigen2Support $(eigen_path)/StdDeque -nobase_3rdparty_HEADERS += $(eigen_path)/Eigenvalues $(eigen_path)/StdList -nobase_3rdparty_HEADERS += $(eigen_path)/Geometry $(eigen_path)/StdVector -nobase_3rdparty_HEADERS += $(eigen_path)/Householder $(eigen_path)/SVD -nobase_3rdparty_HEADERS += $(eigen_path)/Jacobi - -##./src/Cholesky: -nobase_3rdparty_HEADERS += $(eigen_path)/src/Cholesky/LDLT.h $(eigen_path)/src/Cholesky/LLT.h - -##./src/Core: -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/ArrayBase.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Array.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/ArrayWrapper.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Assign.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/BandMatrix.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Block.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/BooleanRedux.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/CommaInitializer.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/CwiseBinaryOp.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/CwiseNullaryOp.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/CwiseUnaryOp.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/CwiseUnaryView.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/DenseBase.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/DenseCoeffsBase.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/DenseStorage.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Diagonal.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/DiagonalMatrix.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/DiagonalProduct.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Dot.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/EigenBase.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Flagged.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/ForceAlignedAccess.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Functors.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Fuzzy.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/GenericPacketMath.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/GlobalFunctions.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/IO.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/MapBase.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Map.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/MathFunctions.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/MatrixBase.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Matrix.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/NestByValue.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/NoAlias.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/NumTraits.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/PermutationMatrix.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/PlainObjectBase.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/ProductBase.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Product.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Random.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Redux.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Replicate.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/ReturnByValue.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Reverse.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Select.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/SelfAdjointView.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/SelfCwiseBinaryOp.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/SolveTriangular.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/StableNorm.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Stride.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Swap.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Transpose.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Transpositions.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/TriangularMatrix.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/VectorBlock.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/VectorwiseOp.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Visitor.h - -##./src/Core/arch/AltiVec: -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/AltiVec/Complex.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/AltiVec/PacketMath.h - -##./src/Core/arch/Default: -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/Default/Settings.h - -##./src/Core/arch/NEON: -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/NEON/Complex.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/NEON/PacketMath.h - -##./src/Core/arch/SSE: -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/SSE/Complex.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/SSE/MathFunctions.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/SSE/PacketMath.h - -##./src/Core/products: -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/CoeffBasedProduct.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/GeneralBlockPanelKernel.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/GeneralMatrixMatrix.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/GeneralMatrixMatrixTriangular.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/GeneralMatrixVector.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/Parallelizer.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/SelfadjointMatrixMatrix.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/SelfadjointMatrixVector.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/SelfadjointProduct.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/SelfadjointRank2Update.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/TriangularMatrixMatrix.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/TriangularMatrixVector.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/TriangularSolverMatrix.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/TriangularSolverVector.h - -##./src/Core/util: -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/BlasUtil.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/Constants.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/DisableStupidWarnings.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/ForwardDeclarations.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/Macros.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/Memory.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/Meta.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/ReenableStupidWarnings.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/StaticAssert.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/XprHelper.h - -##./src/Eigen2Support: -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Block.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Cwise.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/CwiseOperators.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Lazy.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/LeastSquares.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/LU.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Macros.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/MathFunctions.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Memory.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Meta.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Minor.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/QR.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/SVD.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/TriangularSolver.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/VectorBlock.h - -##./src/Eigen2Support/Geometry: -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/AlignedBox.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/All.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/AngleAxis.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/Hyperplane.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/ParametrizedLine.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/Quaternion.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/Rotation2D.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/RotationBase.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/Scaling.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/Transform.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/Translation.h - -##./src/Eigenvalues: -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/ComplexEigenSolver.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/ComplexSchur.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/EigenSolver.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/EigenvaluesCommon.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/HessenbergDecomposition.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/MatrixBaseEigenvalues.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/RealSchur.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/SelfAdjointEigenSolver.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/Tridiagonalization.h - -##./src/Geometry: -nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/AlignedBox.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/AngleAxis.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/EulerAngles.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Homogeneous.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Hyperplane.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/OrthoMethods.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/ParametrizedLine.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Quaternion.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Rotation2D.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/RotationBase.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Scaling.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Transform.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Translation.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Umeyama.h - -##./src/Geometry/arch: -nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/arch/Geometry_SSE.h - -##./src/Householder: -nobase_3rdparty_HEADERS += $(eigen_path)/src/Householder/BlockHouseholder.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Householder/Householder.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Householder/HouseholderSequence.h - -##./src/Jacobi: -nobase_3rdparty_HEADERS += $(eigen_path)/src/Jacobi/Jacobi.h - -##./src/LU: -nobase_3rdparty_HEADERS += $(eigen_path)/src/LU/Determinant.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/LU/FullPivLU.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/LU/Inverse.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/LU/PartialPivLU.h - -##./src/LU/arch: -nobase_3rdparty_HEADERS += $(eigen_path)/src/LU/arch/Inverse_SSE.h - -##./src/misc: -nobase_3rdparty_HEADERS += $(eigen_path)/src/misc/Image.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/misc/Kernel.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/misc/Solve.h - -##./src/plugins: -nobase_3rdparty_HEADERS += $(eigen_path)/src/plugins/ArrayCwiseBinaryOps.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/plugins/ArrayCwiseUnaryOps.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/plugins/BlockMethods.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/plugins/CommonCwiseBinaryOps.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/plugins/CommonCwiseUnaryOps.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/plugins/MatrixCwiseBinaryOps.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/plugins/MatrixCwiseUnaryOps.h - -##./src/QR: -nobase_3rdparty_HEADERS += $(eigen_path)/src/QR/ColPivHouseholderQR.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/QR/FullPivHouseholderQR.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/QR/HouseholderQR.h - -##./src/Sparse: -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/AmbiVector.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/CompressedStorage.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/CoreIterators.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/DynamicSparseMatrix.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/MappedSparseMatrix.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseAssign.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseBlock.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseCwiseBinaryOp.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseCwiseUnaryOp.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseDenseProduct.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseDiagonalProduct.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseDot.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseFuzzy.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseMatrixBase.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseMatrix.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseProduct.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseRedux.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseSelfAdjointView.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseSparseProduct.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseTranspose.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseTriangularView.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseUtil.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseVector.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseView.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/TriangularSolver.h - -##./src/StlSupport: -nobase_3rdparty_HEADERS += $(eigen_path)/src/StlSupport/details.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/StlSupport/StdDeque.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/StlSupport/StdList.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/StlSupport/StdVector.h - -##./src/SVD: -nobase_3rdparty_HEADERS += $(eigen_path)/src/SVD/JacobiSVD.h -nobase_3rdparty_HEADERS += $(eigen_path)/src/SVD/UpperBidiagonalization.h - - - diff --git a/gtsam/Makefile.am b/gtsam/Makefile.am deleted file mode 100644 index 1a09b5b63..000000000 --- a/gtsam/Makefile.am +++ /dev/null @@ -1,15 +0,0 @@ - -# All the sub-directories that need to be built -SUBDIRS = 3rdparty base geometry inference linear nonlinear slam - -# And the corresponding libraries produced -SUBLIBS = 3rdparty/libccolamd.la base/libbase.la geometry/libgeometry.la \ - inference/libinference.la linear/liblinear.la nonlinear/libnonlinear.la \ - slam/libslam.la - -# The following lines specify the actual shared library to be built with libtool -lib_LTLIBRARIES = libgtsam.la -libgtsam_la_SOURCES = -nodist_EXTRA_libgtsam_la_SOURCES = dummy.cxx -libgtsam_la_LIBADD = $(SUBLIBS) $(BOOST_LDFLAGS) -libgtsam_la_LDFLAGS = -no-undefined -version-info 0:0:0 diff --git a/gtsam/base/DerivedValue.h b/gtsam/base/DerivedValue.h new file mode 100644 index 000000000..3cac7c7dc --- /dev/null +++ b/gtsam/base/DerivedValue.h @@ -0,0 +1,106 @@ +/* + * DerivedValue.h + * + * Created on: Jan 26, 2012 + * Author: thduynguyen + */ + +#pragma once + +#include +#include + +namespace gtsam { + +template +class DerivedValue : public Value { +private: + /// Fake Tag struct for singleton pool allocator. In fact, it is never used! + struct PoolTag { }; + +protected: + DerivedValue() {} + +public: + + virtual ~DerivedValue() {} + + /** + * Create a duplicate object returned as a pointer to the generic Value interface. + * For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator + */ + virtual Value* clone_() const { + void *place = boost::singleton_pool::malloc(); + DERIVED* ptr = new(place) DERIVED(static_cast(*this)); + return ptr; + } + + /** + * Destroy and deallocate this object, only if it was originally allocated using clone_(). + */ + virtual void deallocate_() const { + this->~Value(); + boost::singleton_pool::free((void*)this); + } + + /// equals implementing generic Value interface + virtual bool equals_(const Value& p, double tol = 1e-9) const { + // Cast the base class Value pointer to a derived class pointer + const DERIVED& derivedValue2 = dynamic_cast(p); + + // Return the result of calling equals on the derived class + return (static_cast(this))->equals(derivedValue2, tol); + } + + /// Generic Value interface version of retract + virtual Value* retract_(const Vector& delta) const { + // Call retract on the derived class + const DERIVED retractResult = (static_cast(this))->retract(delta); + + // Create a Value pointer copy of the result + void* resultAsValuePlace = boost::singleton_pool::malloc(); + Value* resultAsValue = new(resultAsValuePlace) DERIVED(retractResult); + + // Return the pointer to the Value base class + return resultAsValue; + } + + /// Generic Value interface version of localCoordinates + virtual Vector localCoordinates_(const Value& value2) const { + // Cast the base class Value pointer to a derived class pointer + const DERIVED& derivedValue2 = dynamic_cast(value2); + + // Return the result of calling localCoordinates on the derived class + return (static_cast(this))->localCoordinates(derivedValue2); + } + + /// Assignment operator + virtual Value& operator=(const Value& rhs) { + // Cast the base class Value pointer to a derived class pointer + const DERIVED& derivedRhs = dynamic_cast(rhs); + + // Do the assignment and return the result + return (static_cast(this))->operator=(derivedRhs); + } + + /// Conversion to the derived class + operator const DERIVED& () const { + return static_cast(*this); + } + + /// Conversion to the derived class + operator DERIVED& () { + return static_cast(*this); + } + +protected: + /// Assignment operator, protected because only the Value or DERIVED + /// assignment operators should be used. + DerivedValue& operator=(const DerivedValue& rhs) { + // Nothing to do, do not call base class assignment operator + return *this; + } + +}; + +} /* namespace gtsam */ diff --git a/gtsam/base/FastList.h b/gtsam/base/FastList.h index b34d1fad9..02f8734f6 100644 --- a/gtsam/base/FastList.h +++ b/gtsam/base/FastList.h @@ -20,6 +20,8 @@ #include #include +#include +#include namespace gtsam { @@ -51,6 +53,14 @@ public: /** Copy constructor from the base map class */ FastList(const Base& x) : Base(x) {} +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } + }; } diff --git a/gtsam/base/FastMap.h b/gtsam/base/FastMap.h index cc5a3b4d6..c9fe30646 100644 --- a/gtsam/base/FastMap.h +++ b/gtsam/base/FastMap.h @@ -20,7 +20,8 @@ #include #include -#include +#include +#include namespace gtsam { @@ -52,12 +53,12 @@ public: /** Copy constructor from the base map class */ FastMap(const Base& x) : Base(x) {} - private: +private: /** Serialization function */ friend class boost::serialization::access; template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::base_object(*this); + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); } }; diff --git a/gtsam/base/FastSet.h b/gtsam/base/FastSet.h index 7d9084e5f..05fb879a1 100644 --- a/gtsam/base/FastSet.h +++ b/gtsam/base/FastSet.h @@ -25,6 +25,8 @@ #include #include #include +#include +#include BOOST_MPL_HAS_XXX_TRAIT_DEF(print) @@ -74,6 +76,14 @@ public: /** Check for equality within tolerance to implement Testable */ bool equals(const FastSet& other, double tol = 1e-9) const { return FastSetTestableHelper::equals(*this, other, tol); } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } }; // This is the default Testable interface for *non*Testable elements, which diff --git a/gtsam/base/FastVector.h b/gtsam/base/FastVector.h index 57b5bb4cc..18af63006 100644 --- a/gtsam/base/FastVector.h +++ b/gtsam/base/FastVector.h @@ -20,6 +20,8 @@ #include #include +#include +#include namespace gtsam { @@ -48,14 +50,37 @@ public: /** Constructor from a range, passes through to base class */ template - explicit FastVector(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {} + explicit FastVector(INPUTITERATOR first, INPUTITERATOR last) { + // This if statement works around a bug in boost pool allocator and/or + // STL vector where if the size is zero, the pool allocator will allocate + // huge amounts of memory. + if(first != last) + Base::assign(first, last); + } /** Copy constructor from another FastSet */ FastVector(const FastVector& x) : Base(x) {} - /** Copy constructor from the base map class */ + /** Copy constructor from the base class */ FastVector(const Base& x) : Base(x) {} + /** Copy constructor from a standard STL container */ + FastVector(const std::vector& x) { + // This if statement works around a bug in boost pool allocator and/or + // STL vector where if the size is zero, the pool allocator will allocate + // huge amounts of memory. + if(x.size() > 0) + Base::assign(x.begin(), x.end()); + } + +private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + } + }; } diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index bc530a9f5..a770d9975 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -133,7 +133,7 @@ template Matrix wedge(const Vector& x); template T expm(const Vector& x, int K=7) { Matrix xhat = wedge(x); - return expm(xhat,K); + return T(expm(xhat,K)); } } // namespace gtsam diff --git a/gtsam/base/LieVector.h b/gtsam/base/LieVector.h index 30c007349..9a8e5480f 100644 --- a/gtsam/base/LieVector.h +++ b/gtsam/base/LieVector.h @@ -19,13 +19,14 @@ #include #include +#include namespace gtsam { /** * LieVector is a wrapper around vector to allow it to be a Lie type */ -struct LieVector : public Vector { +struct LieVector : public Vector, public DerivedValue { /** default constructor - should be unnecessary */ LieVector() {} diff --git a/gtsam/base/Makefile.am b/gtsam/base/Makefile.am deleted file mode 100644 index 0d4189dd7..000000000 --- a/gtsam/base/Makefile.am +++ /dev/null @@ -1,74 +0,0 @@ -#---------------------------------------------------------------------------------------------------- -# GTSAM base -# provides some base Math and data structures, as well as test-related utilities -#---------------------------------------------------------------------------------------------------- - -# use nostdinc to turn off -I. and -I.., we do not need them because -# header files are qualified so they can be included in external projects. -AUTOMAKE_OPTIONS = nostdinc - -headers = -sources = -check_PROGRAMS = - -# base Math - -headers += FixedVector.h types.h blockMatrices.h -sources += Vector.cpp Matrix.cpp -sources += cholesky.cpp -check_PROGRAMS += tests/testFixedVector tests/testVector tests/testMatrix tests/testBlockMatrices -check_PROGRAMS += tests/testCholesky -check_PROGRAMS += tests/testNumericalDerivative - -# Testing -headers += Testable.h TestableAssertions.h numericalDerivative.h -sources += timing.cpp debug.cpp -check_PROGRAMS += tests/testDebug tests/testTestableAssertions - -# Manifolds and Lie Groups -headers += Manifold.h Group.h -headers += Lie.h Lie-inl.h lieProxies.h LieScalar.h -sources += LieVector.cpp -check_PROGRAMS += tests/testLieVector tests/testLieScalar - -# Data structures -headers += BTree.h DSF.h FastMap.h FastSet.h FastList.h FastVector.h -sources += DSFVector.cpp -check_PROGRAMS += tests/testBTree tests/testDSF tests/testDSFVector - -# Timing tests -noinst_PROGRAMS = tests/timeMatrix tests/timeVirtual tests/timeVirtual2 tests/timeTest -noinst_PROGRAMS += tests/timeMatrixOps - -#---------------------------------------------------------------------------------------------------- -# Create a libtool library that is not installed -# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am -# The headers are installed in $(includedir)/gtsam: -#---------------------------------------------------------------------------------------------------- -headers += $(sources:.cpp=.h) -basedir = $(pkgincludedir)/base -base_HEADERS = $(headers) -noinst_LTLIBRARIES = libbase.la -libbase_la_SOURCES = $(sources) - -AM_CPPFLAGS = - -AM_CPPFLAGS += $(BOOST_CPPFLAGS) -I$(top_srcdir) -AM_LDFLAGS = $(BOOST_LDFLAGS) - -#---------------------------------------------------------------------------------------------------- -# rules to build local programs -#---------------------------------------------------------------------------------------------------- -TESTS = $(check_PROGRAMS) -AM_DEFAULT_SOURCE_EXT = .cpp -AM_LDFLAGS += $(boost_serialization) -LDADD = libbase.la ../../CppUnitLite/libCppUnitLite.a - -# rule to run an executable -%.run: % $(LDADD) - ./$^ - -# rule to run executable with valgrind -%.valgrind: % $(LDADD) - valgrind ./$^ -#---------------------------------------------------------------------------------------------------- diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index d8829798b..bcc6c8957 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -23,7 +23,7 @@ * void print(const std::string& name) const = 0; * * equality up to tolerance - * tricky to implement, see NonlinearFactor1 for an example + * tricky to implement, see NoiseModelFactor1 for an example * equals is not supposed to print out *anything*, just return true|false * bool equals(const Derived& expected, double tol) const = 0; * @@ -35,6 +35,7 @@ #include #include +#include #define GTSAM_PRINT(x)((x).print(#x)) diff --git a/gtsam/base/Value.h b/gtsam/base/Value.h new file mode 100644 index 000000000..394a11e72 --- /dev/null +++ b/gtsam/base/Value.h @@ -0,0 +1,184 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Value.h + * @brief The interface class for any variable that can be optimized or used in a factor. + * @author Richard Roberts + * @date Jan 14, 2012 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + + /** + * This is the interface class for any value that may be used as a variable + * assignment in a factor graph, and which you must derive to create new + * variable types to use with gtsam. Examples of built-in classes + * implementing this are mainly in geometry, including Rot3, Pose2, etc. + * + * This interface specifies pure virtual retract_(), localCoordinates_() and + * equals_() functions that work with pointers and references to this interface + * class, i.e. the base class. These functions allow containers, such as + * Values can operate generically on Value objects, retracting or computing + * local coordinates for many Value objects of different types. + * + * When you implement retract_(), localCoordinates_(), and equals_(), we + * suggest first implementing versions of these functions that work directly + * with derived objects, then using the provided helper functions to + * implement the generic Value versions. This makes your implementation + * easier, and also improves performance in situations where the derived type + * is in fact known, such as in most implementations of \c evaluateError() in + * classes derived from NonlinearFactor. + * + * Using the above practice, here is an example of implementing a typical + * class derived from Value: + * \code + class Rot3 : public Value { + public: + // Constructor, there is never a need to call the Value base class constructor. + Rot3() { ... } + + // Print for unit tests and debugging (virtual, implements Value::print()) + virtual void print(const std::string& str = "") const; + + // Equals working directly with Rot3 objects (non-virtual, non-overriding!) + bool equals(const Rot3& other, double tol = 1e-9) const; + + // Tangent space dimensionality (virtual, implements Value::dim()) + virtual size_t dim() const { + return 3; + } + + // retract working directly with Rot3 objects (non-virtual, non-overriding!) + Rot3 retract(const Vector& delta) const { + // Math to implement a 3D rotation retraction e.g. exponential map + return Rot3(result); + } + + // localCoordinates working directly with Rot3 objects (non-virtual, non-overriding!) + Vector localCoordinates(const Rot3& r2) const { + // Math to implement 3D rotation localCoordinates, e.g. logarithm map + return Vector(result); + } + + // Equals implementing the generic Value interface (virtual, implements Value::equals_()) + virtual bool equals_(const Value& other, double tol = 1e-9) const { + // Call our provided helper function to call your Rot3-specific + // equals with appropriate casting. + return CallDerivedEquals(this, other, tol); + } + + // retract implementing the generic Value interface (virtual, implements Value::retract_()) + virtual std::auto_ptr retract_(const Vector& delta) const { + // Call our provided helper function to call your Rot3-specific + // retract and do the appropriate casting and allocation. + return CallDerivedRetract(this, delta); + } + + // localCoordinates implementing the generic Value interface (virtual, implements Value::localCoordinates_()) + virtual Vector localCoordinates_(const Value& value) const { + // Call our provided helper function to call your Rot3-specific + // localCoordinates and do the appropriate casting. + return CallDerivedLocalCoordinates(this, value); + } + }; + \endcode + */ + class Value { + public: + + /** Allocate and construct a clone of this value */ + virtual Value* clone_() const = 0; + + /** Deallocate a raw pointer of this value */ + virtual void deallocate_() const = 0; + + /** Compare this Value with another for equality. */ + virtual bool equals_(const Value& other, double tol = 1e-9) const = 0; + + /** Print this value, for debugging and unit tests */ + virtual void print(const std::string& str = "") const = 0; + + /** Return the dimensionality of the tangent space of this value. This is + * the dimensionality of \c delta passed into retract() and of the vector + * returned by localCoordinates(). + * @return The dimensionality of the tangent space + */ + virtual size_t dim() const = 0; + + /** Increment the value, by mapping from the vector delta in the tangent + * space of the current value back to the manifold to produce a new, + * incremented value. + * @param delta The delta vector in the tangent space of this value, by + * which to increment this value. + */ + virtual Value* retract_(const Vector& delta) const = 0; + + /** Compute the coordinates in the tangent space of this value that + * retract() would map to \c value. + * @param value The value whose coordinates should be determined in the + * tangent space of the value on which this function is called. + * @return The coordinates of \c value in the tangent space of \c this. + */ + virtual Vector localCoordinates_(const Value& value) const = 0; + + /** Assignment operator */ + virtual Value& operator=(const Value& rhs) = 0; + + /** Virutal destructor */ + virtual ~Value() {} + + private: + /** Empty serialization function. + * + * There are two important things that users need to do to serialize derived objects in Values successfully: + * (Those derived objects are stored in Values as pointer to this abstract base class Value) + * + * 1. All DERIVED classes derived from Value must put the following line in their serialization function: + * \code + ar & boost::serialization::make_nvp("DERIVED", boost::serialization::base_object(*this)); + \endcode + * or, alternatively + * \code + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value); + \endcode + * See: http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#runtimecasting + * + * 2. The source module that includes archive class headers to serialize objects of derived classes + * (boost/archive/text_oarchive.h, for example) must *export* all derived classes, using either + * BOOST_CLASS_EXPORT or BOOST_CLASS_EXPORT_GUID macros: + \code + BOOST_CLASS_EXPORT(DERIVED_CLASS_1) + BOOST_CLASS_EXPORT_GUID(DERIVED_CLASS_2, "DERIVED_CLASS_2_ID_STRING") + \endcode + * See: http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#derivedpointers + * http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#export + * http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#instantiation\ + * http://www.boost.org/doc/libs/release/libs/serialization/doc/special.html#export + * http://www.boost.org/doc/libs/release/libs/serialization/doc/traits.html#export + * The last two links explain why these export lines have to be in the same source module that includes + * any of the archive class headers. + * */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) {} + + }; + +} /* namespace gtsam */ + +BOOST_SERIALIZATION_ASSUME_ABSTRACT(gtsam::Value) diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 432ae5d77..5367cec09 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -42,15 +42,14 @@ namespace gtsam { /* ************************************************************************* */ void odprintf_(const char *format, ostream& stream, ...) { - char buf[4096], *p = buf; - int n; + char buf[4096], *p = buf; va_list args; va_start(args, stream); #ifdef WIN32 - n = _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL + _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL #else - n = vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL + vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL #endif va_end(args); @@ -63,17 +62,15 @@ void odprintf_(const char *format, ostream& stream, ...) { /* ************************************************************************* */ -void odprintf(const char *format, ...) -{ - char buf[4096], *p = buf; - int n; +void odprintf(const char *format, ...) { + char buf[4096], *p = buf; va_list args; va_start(args, format); #ifdef WIN32 - n = _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL + _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL #else - n = vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL + vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL #endif va_end(args); @@ -86,11 +83,6 @@ void odprintf(const char *format, ...) /* ************************************************************************* */ Vector Vector_( size_t m, const double* const data) { -// Vector v(m); -// for (size_t i=0; i +#include + +// includes for standard serialization types +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +// whether to print the serialized text to stdout +const bool verbose = false; + +namespace gtsam { namespace serializationTestHelpers { + + /* ************************************************************************* */ + // Serialization testing code. + /* ************************************************************************* */ + +template +std::string serialize(const T& input) { + std::ostringstream out_archive_stream; + boost::archive::text_oarchive out_archive(out_archive_stream); + out_archive << input; + return out_archive_stream.str(); +} + +template +void deserialize(const std::string& serialized, T& output) { + std::istringstream in_archive_stream(serialized); + boost::archive::text_iarchive in_archive(in_archive_stream); + in_archive >> output; +} + +// Templated round-trip serialization +template +void roundtrip(const T& input, T& output) { + // Serialize + std::string serialized = serialize(input); + if (verbose) std::cout << serialized << std::endl << std::endl; + + deserialize(serialized, output); +} + +// This version requires equality operator +template +bool equality(const T& input = T()) { + T output; + roundtrip(input,output); + return input==output; +} + +// This version requires equals +template +bool equalsObj(const T& input = T()) { + T output; + roundtrip(input,output); + return input.equals(output); +} + +// De-referenced version for pointers +template +bool equalsDereferenced(const T& input) { + T output; + roundtrip(input,output); + return input->equals(*output); +} + +/* ************************************************************************* */ +template +std::string serializeXML(const T& input) { + std::ostringstream out_archive_stream; + boost::archive::xml_oarchive out_archive(out_archive_stream); + out_archive << boost::serialization::make_nvp("data", input); + return out_archive_stream.str(); +} + +template +void deserializeXML(const std::string& serialized, T& output) { + std::istringstream in_archive_stream(serialized); + boost::archive::xml_iarchive in_archive(in_archive_stream); + in_archive >> boost::serialization::make_nvp("data", output); +} + +// Templated round-trip serialization using XML +template +void roundtripXML(const T& input, T& output) { + // Serialize + std::string serialized = serializeXML(input); + if (verbose) std::cout << serialized << std::endl << std::endl; + + // De-serialize + deserializeXML(serialized, output); +} + +// This version requires equality operator +template +bool equalityXML(const T& input = T()) { + T output; + roundtripXML(input,output); + return input==output; +} + +// This version requires equals +template +bool equalsXML(const T& input = T()) { + T output; + roundtripXML(input,output); + return input.equals(output); +} + +// This version is for pointers +template +bool equalsDereferencedXML(const T& input = T()) { + T output; + roundtripXML(input,output); + return input->equals(*output); +} + +} } diff --git a/gtsam/base/tests/testSerializationBase.cpp b/gtsam/base/tests/testSerializationBase.cpp new file mode 100644 index 000000000..e144b3576 --- /dev/null +++ b/gtsam/base/tests/testSerializationBase.cpp @@ -0,0 +1,95 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSerializationBase.cpp + * @brief + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + +Vector v1 = Vector_(2, 1.0, 2.0); +Vector v2 = Vector_(2, 3.0, 4.0); +Vector v3 = Vector_(2, 5.0, 6.0); + +/* ************************************************************************* */ +TEST (Serialization, FastList) { + FastList list; + list.push_back(v1); + list.push_back(v2); + list.push_back(v3); + + EXPECT(equality(list)); + EXPECT(equalityXML(list)); +} + +/* ************************************************************************* */ +TEST (Serialization, FastMap) { + FastMap map; + map.insert(make_pair(1, v1)); + map.insert(make_pair(2, v2)); + map.insert(make_pair(3, v3)); + + EXPECT(equality(map)); + EXPECT(equalityXML(map)); +} + +/* ************************************************************************* */ +TEST (Serialization, FastSet) { + FastSet set; + set.insert(1.0); + set.insert(2.0); + set.insert(3.0); + + EXPECT(equality(set)); + EXPECT(equalityXML(set)); +} + +/* ************************************************************************* */ +TEST (Serialization, FastVector) { + FastVector vector; + vector.push_back(v1); + vector.push_back(v2); + vector.push_back(v3); + + EXPECT(equality(vector)); + EXPECT(equalityXML(vector)); +} + +/* ************************************************************************* */ +TEST (Serialization, matrix_vector) { + EXPECT(equality(Vector_(4, 1.0, 2.0, 3.0, 4.0))); + EXPECT(equality(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0))); + + EXPECT(equalityXML(Vector_(4, 1.0, 2.0, 3.0, 4.0))); + EXPECT(equalityXML(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0))); +} + +/* ************************************************************************* */ + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h index 9c7561f61..259a07c9b 100644 --- a/gtsam/geometry/Cal3Bundler.h +++ b/gtsam/geometry/Cal3Bundler.h @@ -18,6 +18,7 @@ #pragma once +#include #include namespace gtsam { @@ -27,7 +28,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ -class Cal3Bundler { +class Cal3Bundler : public DerivedValue { private: double f_, k1_, k2_ ; @@ -94,7 +95,7 @@ public: Vector localCoordinates(const Cal3Bundler& T2) const ; ///TODO: comment - int dim() const { return 3 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) + virtual size_t dim() const { return 3 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) ///TODO: comment static size_t Dim() { return 3; } //TODO: make a final dimension variable @@ -110,6 +111,8 @@ private: template void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("Cal3Bundler", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(f_); ar & BOOST_SERIALIZATION_NVP(k1_); ar & BOOST_SERIALIZATION_NVP(k2_); diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 437a68a68..9b94eccb5 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -18,6 +18,7 @@ #pragma once +#include #include namespace gtsam { @@ -27,7 +28,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ -class Cal3DS2 { +class Cal3DS2 : public DerivedValue { private: @@ -97,7 +98,7 @@ public: Vector localCoordinates(const Cal3DS2& T2) const ; ///TODO: comment - int dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) + virtual size_t dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) ///TODO: comment static size_t Dim() { return 9; } //TODO: make a final dimension variable @@ -113,6 +114,8 @@ private: template void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("Cal3DS2", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fy_); ar & BOOST_SERIALIZATION_NVP(s_); diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 25337deb0..021b1e366 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -21,6 +21,7 @@ #pragma once +#include #include namespace gtsam { @@ -30,7 +31,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ - class Cal3_S2 { + class Cal3_S2 : public DerivedValue { private: double fx_, fy_, s_, u0_, v0_; @@ -175,6 +176,8 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("Cal3_S2", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fy_); ar & BOOST_SERIALIZATION_NVP(s_); diff --git a/gtsam/geometry/Cal3_S2Stereo.h b/gtsam/geometry/Cal3_S2Stereo.h index a8366e35f..7bbafd7d5 100644 --- a/gtsam/geometry/Cal3_S2Stereo.h +++ b/gtsam/geometry/Cal3_S2Stereo.h @@ -81,6 +81,8 @@ namespace gtsam { template void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("Cal3_S2Stereo", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(b_); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Cal3_S2); } diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 21fe526a1..95ff66e68 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include @@ -35,7 +36,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ - class CalibratedCamera { + class CalibratedCamera : public DerivedValue { private: Pose3 pose_; // 6DOF pose @@ -48,22 +49,23 @@ namespace gtsam { CalibratedCamera() {} /// construct with pose - CalibratedCamera(const Pose3& pose); + explicit CalibratedCamera(const Pose3& pose); /// @} /// @name Advanced Constructors /// @{ /// construct from vector - CalibratedCamera(const Vector &v); + explicit CalibratedCamera(const Vector &v); /// @} /// @name Testable /// @{ - void print(const std::string& s="") const { - pose_.print(); - } + virtual void print(const std::string& s = "") const { + pose_.print(s); + } + /// check equality to another camera bool equals (const CalibratedCamera &camera, double tol = 1e-9) const { return pose_.equals(camera.pose(), tol) ; @@ -155,6 +157,8 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("CalibratedCamera", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(pose_); } diff --git a/gtsam/geometry/Makefile.am b/gtsam/geometry/Makefile.am deleted file mode 100644 index 5f9b34610..000000000 --- a/gtsam/geometry/Makefile.am +++ /dev/null @@ -1,71 +0,0 @@ -#---------------------------------------------------------------------------------------------------- -# GTSAM geometry -#---------------------------------------------------------------------------------------------------- - -# use nostdinc to turn off -I. and -I.., we do not need them because -# header files are qualified so they can be included in external projects. -AUTOMAKE_OPTIONS = nostdinc - -headers = -sources = -check_PROGRAMS = - -# Concept check -headers += concepts.h - -# Points and poses -sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Pose3.cpp -check_PROGRAMS += tests/testPoint2 tests/testRot2 tests/testPose2 tests/testPoint3 tests/testRot3M tests/testRot3Q tests/testPose3 - -# Cameras -headers += GeneralCameraT.h Cal3_S2Stereo.h PinholeCamera.h -sources += Cal3_S2.cpp Cal3DS2.cpp Cal3Bundler.cpp CalibratedCamera.cpp SimpleCamera.cpp -check_PROGRAMS += tests/testCal3DS2 tests/testCal3_S2 tests/testCal3Bundler tests/testCalibratedCamera tests/testSimpleCamera - -# Stereo -sources += StereoPoint2.cpp StereoCamera.cpp -check_PROGRAMS += tests/testStereoCamera tests/testStereoPoint2 - -# Tensors -headers += tensors.h Tensor1.h Tensor2.h Tensor3.h Tensor4.h Tensor5.h -headers += Tensor1Expression.h Tensor2Expression.h Tensor3Expression.h Tensor5Expression.h -sources += projectiveGeometry.cpp tensorInterface.cpp -check_PROGRAMS += tests/testTensors tests/testHomography2 tests/testFundamental - -# Timing tests -noinst_PROGRAMS = tests/timeRot3 tests/timePose3 tests/timeCalibratedCamera tests/timeStereoCamera - -# Rot3M and Rot3Q both use Rot3.h, they do not have individual header files -allsources = $(sources) -allsources += Rot3M.cpp Rot3Q.cpp -headers += Rot3.h - -#---------------------------------------------------------------------------------------------------- -# Create a libtool library that is not installed -# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am -# The headers are installed in $(includedir)/gtsam: -#---------------------------------------------------------------------------------------------------- -headers += $(sources:.cpp=.h) -geometrydir = $(pkgincludedir)/geometry -geometry_HEADERS = $(headers) -noinst_LTLIBRARIES = libgeometry.la -libgeometry_la_SOURCES = $(allsources) -AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) -AM_LDFLAGS = $(BOOST_LDFLAGS) - -#---------------------------------------------------------------------------------------------------- -# rules to build local programs -#---------------------------------------------------------------------------------------------------- -TESTS = $(check_PROGRAMS) -AM_LDFLAGS += $(boost_serialization) -LDADD = libgeometry.la ../base/libbase.la ../../CppUnitLite/libCppUnitLite.a -AM_DEFAULT_SOURCE_EXT = .cpp - -# rule to run an executable -%.run: % $(LDADD) - ./$^ - -# rule to run executable with valgrind -%.valgrind: % $(LDADD) - valgrind ./$^ -#---------------------------------------------------------------------------------------------------- diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index b8c6428bb..8aee12064 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -20,6 +20,8 @@ #include #include +#include +#include #include #include #include @@ -35,7 +37,7 @@ namespace gtsam { * \nosubgrouping */ template - class PinholeCamera { + class PinholeCamera : public DerivedValue > { private: Pose3 pose_; Calibration k_; @@ -49,7 +51,7 @@ namespace gtsam { PinholeCamera() {} /** constructor with pose */ - PinholeCamera(const Pose3& pose):pose_(pose){} + explicit PinholeCamera(const Pose3& pose):pose_(pose){} /** constructor with pose and calibration */ PinholeCamera(const Pose3& pose, const Calibration& k):pose_(pose),k_(k) {} @@ -61,7 +63,7 @@ namespace gtsam { /// @name Advanced Constructors /// @{ - PinholeCamera(const Vector &v){ + explicit PinholeCamera(const Vector &v){ pose_ = Pose3::Expmap(v.head(Pose3::Dim())); if ( v.size() > Pose3::Dim()) { k_ = Calibration(v.tail(Calibration::Dim())); @@ -278,6 +280,7 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value); ar & BOOST_SERIALIZATION_NVP(pose_); ar & BOOST_SERIALIZATION_NVP(k_); } diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index fe86cc865..fe7792733 100644 --- a/gtsam/geometry/Point2.h +++ b/gtsam/geometry/Point2.h @@ -18,6 +18,8 @@ #pragma once #include + +#include #include #include @@ -30,7 +32,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ -class Point2 { +class Point2 : public DerivedValue { public: /// dimension of the variable - used to autodetect sizes static const size_t dimension = 2; @@ -187,6 +189,8 @@ private: template void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("Point2", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(x_); ar & BOOST_SERIALIZATION_NVP(y_); } diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index c2667d3b4..ad66a845c 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -24,6 +24,7 @@ #include #include +#include #include namespace gtsam { @@ -33,7 +34,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ - class Point3 { + class Point3 : public DerivedValue { public: /// dimension of the variable - used to autodetect sizes static const size_t dimension = 3; @@ -203,6 +204,8 @@ namespace gtsam { template void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("Point3", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(x_); ar & BOOST_SERIALIZATION_NVP(y_); ar & BOOST_SERIALIZATION_NVP(z_); diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 3a8f69899..877eb93a1 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -32,7 +33,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ -class Pose2 { +class Pose2 : public DerivedValue { public: static const size_t dimension = 3; @@ -268,6 +269,8 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("Pose2", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(t_); ar & BOOST_SERIALIZATION_NVP(r_); } diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 640016970..fd4bc08df 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -22,6 +22,7 @@ #define POSE3_DEFAULT_COORDINATES_MODE Pose3::FIRST_ORDER #endif +#include #include #include @@ -34,7 +35,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ - class Pose3 { + class Pose3 : public DerivedValue { public: static const size_t dimension = 6; @@ -232,6 +233,8 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("Pose3", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(R_); ar & BOOST_SERIALIZATION_NVP(t_); } diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index f3062f5c1..8e33794e4 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -19,6 +19,8 @@ #pragma once #include + +#include #include namespace gtsam { @@ -29,7 +31,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ - class Rot2 { + class Rot2 : public DerivedValue { public: /** get the dimension by the type */ @@ -237,6 +239,8 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("Rot2", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(c_); ar & BOOST_SERIALIZATION_NVP(s_); } diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 01361a655..599e6a8e9 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -32,6 +32,7 @@ #endif #endif +#include #include #include @@ -49,7 +50,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ - class Rot3 { + class Rot3 : public DerivedValue { public: static const size_t dimension = 3; @@ -92,6 +93,9 @@ namespace gtsam { */ Rot3(const Quaternion& q); + /** Virtual destructor */ + virtual ~Rot3() {} + /* Static member function to generate some well known rotations */ /// Rotation around X axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis. @@ -358,6 +362,8 @@ namespace gtsam { template void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("Rot3", + boost::serialization::base_object(*this)); #ifndef GTSAM_DEFAULT_QUATERNIONS ar & BOOST_SERIALIZATION_NVP(r1_); ar & BOOST_SERIALIZATION_NVP(r2_); diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index cd3dbd19e..f5e07e66c 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -18,6 +18,8 @@ #pragma once #include + +#include #include #include #include diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h index 0bdf37e0e..43701e4e8 100644 --- a/gtsam/geometry/StereoPoint2.h +++ b/gtsam/geometry/StereoPoint2.h @@ -18,6 +18,7 @@ #pragma once +#include #include namespace gtsam { @@ -27,7 +28,7 @@ namespace gtsam { * @ingroup geometry * \nosubgrouping */ - class StereoPoint2 { + class StereoPoint2 : public DerivedValue { public: static const size_t dimension = 3; private: @@ -147,6 +148,8 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { + ar & boost::serialization::make_nvp("StereoPoint2", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(uL_); ar & BOOST_SERIALIZATION_NVP(uR_); ar & BOOST_SERIALIZATION_NVP(v_); diff --git a/gtsam/geometry/tests/testSerializationGeometry.cpp b/gtsam/geometry/tests/testSerializationGeometry.cpp new file mode 100644 index 000000000..c1fba6b2c --- /dev/null +++ b/gtsam/geometry/tests/testSerializationGeometry.cpp @@ -0,0 +1,115 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSerializationGeometry.cpp + * @brief + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + +/* ************************************************************************* */ +// Export all classes derived from Value +BOOST_CLASS_EXPORT(gtsam::Cal3_S2) +BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo) +BOOST_CLASS_EXPORT(gtsam::Cal3Bundler) +BOOST_CLASS_EXPORT(gtsam::CalibratedCamera) +BOOST_CLASS_EXPORT(gtsam::Point2) +BOOST_CLASS_EXPORT(gtsam::Point3) +BOOST_CLASS_EXPORT(gtsam::Pose2) +BOOST_CLASS_EXPORT(gtsam::Pose3) +BOOST_CLASS_EXPORT(gtsam::Rot2) +BOOST_CLASS_EXPORT(gtsam::Rot3) +BOOST_CLASS_EXPORT(gtsam::PinholeCamera) +BOOST_CLASS_EXPORT(gtsam::PinholeCamera) +BOOST_CLASS_EXPORT(gtsam::PinholeCamera) +BOOST_CLASS_EXPORT(gtsam::StereoPoint2) + +/* ************************************************************************* */ +Point3 pt3(1.0, 2.0, 3.0); +Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); +Pose3 pose3(rt3, pt3); + +Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); +Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); +Cal3Bundler cal3(1.0, 2.0, 3.0); +Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); +Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4)); +CalibratedCamera cal5(Pose3(rt3, pt3)); + +PinholeCamera cam1(pose3, cal1); +StereoCamera cam2(pose3, cal4ptr); +StereoPoint2 spt(1.0, 2.0, 3.0); + +/* ************************************************************************* */ +TEST (Serialization, text_geometry) { + EXPECT(equalsObj(Point2(1.0, 2.0))); + EXPECT(equalsObj(Pose2(1.0, 2.0, 0.3))); + EXPECT(equalsObj(Rot2::fromDegrees(30.0))); + + EXPECT(equalsObj(pt3)); + EXPECT(equalsObj(rt3)); + EXPECT(equalsObj(Pose3(rt3, pt3))); + + EXPECT(equalsObj(cal1)); + EXPECT(equalsObj(cal2)); + EXPECT(equalsObj(cal3)); + EXPECT(equalsObj(cal4)); + EXPECT(equalsObj(cal5)); + + EXPECT(equalsObj(cam1)); + EXPECT(equalsObj(cam2)); + EXPECT(equalsObj(spt)); +} + +/* ************************************************************************* */ +TEST (Serialization, xml_geometry) { + EXPECT(equalsXML(Point2(1.0, 2.0))); + EXPECT(equalsXML(Pose2(1.0, 2.0, 0.3))); + EXPECT(equalsXML(Rot2::fromDegrees(30.0))); + + EXPECT(equalsXML(pt3)); + EXPECT(equalsXML(rt3)); + EXPECT(equalsXML(Pose3(rt3, pt3))); + + EXPECT(equalsXML(cal1)); + EXPECT(equalsXML(cal2)); + EXPECT(equalsXML(cal3)); + EXPECT(equalsXML(cal4)); + EXPECT(equalsXML(cal5)); + + EXPECT(equalsXML(cam1)); + EXPECT(equalsXML(cam2)); + EXPECT(equalsXML(spt)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index 7544587c1..c5301f030 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -21,6 +21,7 @@ #include #include +#include #include #include diff --git a/gtsam/inference/Conditional.h b/gtsam/inference/Conditional.h index 2fa5f691d..0b5fca660 100644 --- a/gtsam/inference/Conditional.h +++ b/gtsam/inference/Conditional.h @@ -46,7 +46,7 @@ private: /** Create keys by adding key in front */ template static std::vector MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) { - std::vector keys((lastParent - firstParent) + 1); + std::vector keys((lastParent - firstParent) + 1); std::copy(firstParent, lastParent, keys.begin() + 1); keys[0] = key; return keys; @@ -62,16 +62,16 @@ protected: public: - typedef KEY Key; - typedef Conditional This; - typedef Factor Base; + typedef KEY KeyType; + typedef Conditional This; + typedef Factor Base; /** * Typedef to the factor type that produces this conditional and that this * conditional can be converted to using a factor constructor. Derived * classes must redefine this. */ - typedef gtsam::Factor FactorType; + typedef gtsam::Factor FactorType; /** A shared_ptr to this class. Derived classes must redefine this. */ typedef boost::shared_ptr shared_ptr; @@ -95,23 +95,23 @@ public: Conditional() : nrFrontals_(0) { assertInvariants(); } /** No parents */ - Conditional(Key key) : FactorType(key), nrFrontals_(1) { assertInvariants(); } + Conditional(KeyType key) : FactorType(key), nrFrontals_(1) { assertInvariants(); } /** Single parent */ - Conditional(Key key, Key parent) : FactorType(key, parent), nrFrontals_(1) { assertInvariants(); } + Conditional(KeyType key, KeyType parent) : FactorType(key, parent), nrFrontals_(1) { assertInvariants(); } /** Two parents */ - Conditional(Key key, Key parent1, Key parent2) : FactorType(key, parent1, parent2), nrFrontals_(1) { assertInvariants(); } + Conditional(KeyType key, KeyType parent1, KeyType parent2) : FactorType(key, parent1, parent2), nrFrontals_(1) { assertInvariants(); } /** Three parents */ - Conditional(Key key, Key parent1, Key parent2, Key parent3) : FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { assertInvariants(); } + Conditional(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3) : FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { assertInvariants(); } /// @} /// @name Advanced Constructors /// @{ /** Constructor from a frontal variable and a vector of parents */ - Conditional(Key key, const std::vector& parents) : + Conditional(KeyType key, const std::vector& parents) : FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_(1) { assertInvariants(); } @@ -145,8 +145,8 @@ public: size_t nrParents() const { return FactorType::size() - nrFrontals_; } /** Special accessor when there is only one frontal variable. */ - Key firstFrontalKey() const { assert(nrFrontals_>0); return FactorType::front(); } - Key lastFrontalKey() const { assert(nrFrontals_>0); return *(endFrontals()-1); } + KeyType firstFrontalKey() const { assert(nrFrontals_>0); return FactorType::front(); } + KeyType lastFrontalKey() const { assert(nrFrontals_>0); return *(endFrontals()-1); } /** return a view of the frontal keys */ Frontals frontals() const { @@ -198,9 +198,9 @@ private: template void Conditional::print(const std::string& s) const { std::cout << s << " P("; - BOOST_FOREACH(Key key, frontals()) std::cout << " " << key; + BOOST_FOREACH(KeyType key, frontals()) std::cout << " " << key; if (nrParents()>0) std::cout << " |"; - BOOST_FOREACH(Key parent, parents()) std::cout << " " << parent; + BOOST_FOREACH(KeyType parent, parents()) std::cout << " " << parent; std::cout << ")" << std::endl; } diff --git a/gtsam/inference/Doxyfile b/gtsam/inference/Doxyfile deleted file mode 100644 index 97b4f769d..000000000 --- a/gtsam/inference/Doxyfile +++ /dev/null @@ -1,1417 +0,0 @@ -# Doxyfile 1.5.6 - -# This file describes the settings to be used by the documentation system -# doxygen (www.doxygen.org) for a project -# -# All text after a hash (#) is considered a comment and will be ignored -# The format is: -# TAG = value [value, ...] -# For lists items can also be appended using: -# TAG += value [value, ...] -# Values that contain spaces should be placed between quotes (" ") - -#--------------------------------------------------------------------------- -# Project related configuration options -#--------------------------------------------------------------------------- - -# This tag specifies the encoding used for all characters in the config file -# that follow. The default is UTF-8 which is also the encoding used for all -# text before the first occurrence of this tag. Doxygen uses libiconv (or the -# iconv built into libc) for the transcoding. See -# http://www.gnu.org/software/libiconv for the list of possible encodings. - -DOXYFILE_ENCODING = UTF-8 - -# The PROJECT_NAME tag is a single word (or a sequence of words surrounded -# by quotes) that should identify the project. - -PROJECT_NAME = gtsam - -# The PROJECT_NUMBER tag can be used to enter a project or revision number. -# This could be handy for archiving the generated documentation or -# if some version control system is used. - -PROJECT_NUMBER = - -# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) -# base path where the generated documentation will be put. -# If a relative path is entered, it will be relative to the location -# where doxygen was started. If left blank the current directory will be used. - -OUTPUT_DIRECTORY = - -# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create -# 4096 sub-directories (in 2 levels) under the output directory of each output -# format and will distribute the generated files over these directories. -# Enabling this option can be useful when feeding doxygen a huge amount of -# source files, where putting all generated files in the same directory would -# otherwise cause performance problems for the file system. - -CREATE_SUBDIRS = NO - -# The OUTPUT_LANGUAGE tag is used to specify the language in which all -# documentation generated by doxygen is written. Doxygen will use this -# information to generate all constant output in the proper language. -# The default language is English, other supported languages are: -# Afrikaans, Arabic, Brazilian, Catalan, Chinese, Chinese-Traditional, -# Croatian, Czech, Danish, Dutch, Farsi, Finnish, French, German, Greek, -# Hungarian, Italian, Japanese, Japanese-en (Japanese with English messages), -# Korean, Korean-en, Lithuanian, Norwegian, Macedonian, Persian, Polish, -# Portuguese, Romanian, Russian, Serbian, Slovak, Slovene, Spanish, Swedish, -# and Ukrainian. - -OUTPUT_LANGUAGE = English - -# If the BRIEF_MEMBER_DESC tag is set to YES (the default) Doxygen will -# include brief member descriptions after the members that are listed in -# the file and class documentation (similar to JavaDoc). -# Set to NO to disable this. - -BRIEF_MEMBER_DESC = YES - -# If the REPEAT_BRIEF tag is set to YES (the default) Doxygen will prepend -# the brief description of a member or function before the detailed description. -# Note: if both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the -# brief descriptions will be completely suppressed. - -REPEAT_BRIEF = YES - -# This tag implements a quasi-intelligent brief description abbreviator -# that is used to form the text in various listings. Each string -# in this list, if found as the leading text of the brief description, will be -# stripped from the text and the result after processing the whole list, is -# used as the annotated text. Otherwise, the brief description is used as-is. -# If left blank, the following values are used ("$name" is automatically -# replaced with the name of the entity): "The $name class" "The $name widget" -# "The $name file" "is" "provides" "specifies" "contains" -# "represents" "a" "an" "the" - -ABBREVIATE_BRIEF = - -# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then -# Doxygen will generate a detailed section even if there is only a brief -# description. - -ALWAYS_DETAILED_SEC = NO - -# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all -# inherited members of a class in the documentation of that class as if those -# members were ordinary class members. Constructors, destructors and assignment -# operators of the base classes will not be shown. - -INLINE_INHERITED_MEMB = NO - -# If the FULL_PATH_NAMES tag is set to YES then Doxygen will prepend the full -# path before files name in the file list and in the header files. If set -# to NO the shortest path that makes the file name unique will be used. - -FULL_PATH_NAMES = YES - -# If the FULL_PATH_NAMES tag is set to YES then the STRIP_FROM_PATH tag -# can be used to strip a user-defined part of the path. Stripping is -# only done if one of the specified strings matches the left-hand part of -# the path. The tag can be used to show relative paths in the file list. -# If left blank the directory from which doxygen is run is used as the -# path to strip. - -STRIP_FROM_PATH = - -# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of -# the path mentioned in the documentation of a class, which tells -# the reader which header file to include in order to use a class. -# If left blank only the name of the header file containing the class -# definition is used. Otherwise one should specify the include paths that -# are normally passed to the compiler using the -I flag. - -STRIP_FROM_INC_PATH = - -# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter -# (but less readable) file names. This can be useful is your file systems -# doesn't support long names like on DOS, Mac, or CD-ROM. - -SHORT_NAMES = NO - -# If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen -# will interpret the first line (until the first dot) of a JavaDoc-style -# comment as the brief description. If set to NO, the JavaDoc -# comments will behave just like regular Qt-style comments -# (thus requiring an explicit @brief command for a brief description.) - -JAVADOC_AUTOBRIEF = NO - -# If the QT_AUTOBRIEF tag is set to YES then Doxygen will -# interpret the first line (until the first dot) of a Qt-style -# comment as the brief description. If set to NO, the comments -# will behave just like regular Qt-style comments (thus requiring -# an explicit \brief command for a brief description.) - -QT_AUTOBRIEF = NO - -# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make Doxygen -# treat a multi-line C++ special comment block (i.e. a block of //! or /// -# comments) as a brief description. This used to be the default behaviour. -# The new default is to treat a multi-line C++ comment block as a detailed -# description. Set this tag to YES if you prefer the old behaviour instead. - -MULTILINE_CPP_IS_BRIEF = NO - -# If the DETAILS_AT_TOP tag is set to YES then Doxygen -# will output the detailed description near the top, like JavaDoc. -# If set to NO, the detailed description appears after the member -# documentation. - -DETAILS_AT_TOP = NO - -# If the INHERIT_DOCS tag is set to YES (the default) then an undocumented -# member inherits the documentation from any documented member that it -# re-implements. - -INHERIT_DOCS = YES - -# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce -# a new page for each member. If set to NO, the documentation of a member will -# be part of the file/class/namespace that contains it. - -SEPARATE_MEMBER_PAGES = NO - -# The TAB_SIZE tag can be used to set the number of spaces in a tab. -# Doxygen uses this value to replace tabs by spaces in code fragments. - -TAB_SIZE = 8 - -# This tag can be used to specify a number of aliases that acts -# as commands in the documentation. An alias has the form "name=value". -# For example adding "sideeffect=\par Side Effects:\n" will allow you to -# put the command \sideeffect (or @sideeffect) in the documentation, which -# will result in a user-defined paragraph with heading "Side Effects:". -# You can put \n's in the value part of an alias to insert newlines. - -ALIASES = - -# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C -# sources only. Doxygen will then generate output that is more tailored for C. -# For instance, some of the names that are used will be different. The list -# of all members will be omitted, etc. - -OPTIMIZE_OUTPUT_FOR_C = NO - -# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java -# sources only. Doxygen will then generate output that is more tailored for -# Java. For instance, namespaces will be presented as packages, qualified -# scopes will look different, etc. - -OPTIMIZE_OUTPUT_JAVA = NO - -# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran -# sources only. Doxygen will then generate output that is more tailored for -# Fortran. - -OPTIMIZE_FOR_FORTRAN = NO - -# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL -# sources. Doxygen will then generate output that is tailored for -# VHDL. - -OPTIMIZE_OUTPUT_VHDL = NO - -# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want -# to include (a tag file for) the STL sources as input, then you should -# set this tag to YES in order to let doxygen match functions declarations and -# definitions whose arguments contain STL classes (e.g. func(std::string); v.s. -# func(std::string) {}). This also make the inheritance and collaboration -# diagrams that involve STL classes more complete and accurate. - -BUILTIN_STL_SUPPORT = NO - -# If you use Microsoft's C++/CLI language, you should set this option to YES to -# enable parsing support. - -CPP_CLI_SUPPORT = NO - -# Set the SIP_SUPPORT tag to YES if your project consists of sip sources only. -# Doxygen will parse them like normal C++ but will assume all classes use public -# instead of private inheritance when no explicit protection keyword is present. - -SIP_SUPPORT = NO - -# For Microsoft's IDL there are propget and propput attributes to indicate getter -# and setter methods for a property. Setting this option to YES (the default) -# will make doxygen to replace the get and set methods by a property in the -# documentation. This will only work if the methods are indeed getting or -# setting a simple type. If this is not the case, or you want to show the -# methods anyway, you should set this option to NO. - -IDL_PROPERTY_SUPPORT = YES - -# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC -# tag is set to YES, then doxygen will reuse the documentation of the first -# member in the group (if any) for the other members of the group. By default -# all members of a group must be documented explicitly. - -DISTRIBUTE_GROUP_DOC = NO - -# Set the SUBGROUPING tag to YES (the default) to allow class member groups of -# the same type (for instance a group of public functions) to be put as a -# subgroup of that type (e.g. under the Public Functions section). Set it to -# NO to prevent subgrouping. Alternatively, this can be done per class using -# the \nosubgrouping command. - -SUBGROUPING = YES - -# When TYPEDEF_HIDES_STRUCT is enabled, a typedef of a struct, union, or enum -# is documented as struct, union, or enum with the name of the typedef. So -# typedef struct TypeS {} TypeT, will appear in the documentation as a struct -# with name TypeT. When disabled the typedef will appear as a member of a file, -# namespace, or class. And the struct will be named TypeS. This can typically -# be useful for C code in case the coding convention dictates that all compound -# types are typedef'ed and only the typedef is referenced, never the tag name. - -TYPEDEF_HIDES_STRUCT = NO - -#--------------------------------------------------------------------------- -# Build related configuration options -#--------------------------------------------------------------------------- - -# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in -# documentation are documented, even if no documentation was available. -# Private class members and static file members will be hidden unless -# the EXTRACT_PRIVATE and EXTRACT_STATIC tags are set to YES - -EXTRACT_ALL = YES - -# If the EXTRACT_PRIVATE tag is set to YES all private members of a class -# will be included in the documentation. - -EXTRACT_PRIVATE = YES - -# If the EXTRACT_STATIC tag is set to YES all static members of a file -# will be included in the documentation. - -EXTRACT_STATIC = YES - -# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) -# defined locally in source files will be included in the documentation. -# If set to NO only classes defined in header files are included. - -EXTRACT_LOCAL_CLASSES = YES - -# This flag is only useful for Objective-C code. When set to YES local -# methods, which are defined in the implementation section but not in -# the interface are included in the documentation. -# If set to NO (the default) only methods in the interface are included. - -EXTRACT_LOCAL_METHODS = NO - -# If this flag is set to YES, the members of anonymous namespaces will be -# extracted and appear in the documentation as a namespace called -# 'anonymous_namespace{file}', where file will be replaced with the base -# name of the file that contains the anonymous namespace. By default -# anonymous namespace are hidden. - -EXTRACT_ANON_NSPACES = NO - -# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all -# undocumented members of documented classes, files or namespaces. -# If set to NO (the default) these members will be included in the -# various overviews, but no documentation section is generated. -# This option has no effect if EXTRACT_ALL is enabled. - -HIDE_UNDOC_MEMBERS = NO - -# If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all -# undocumented classes that are normally visible in the class hierarchy. -# If set to NO (the default) these classes will be included in the various -# overviews. This option has no effect if EXTRACT_ALL is enabled. - -HIDE_UNDOC_CLASSES = NO - -# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, Doxygen will hide all -# friend (class|struct|union) declarations. -# If set to NO (the default) these declarations will be included in the -# documentation. - -HIDE_FRIEND_COMPOUNDS = NO - -# If the HIDE_IN_BODY_DOCS tag is set to YES, Doxygen will hide any -# documentation blocks found inside the body of a function. -# If set to NO (the default) these blocks will be appended to the -# function's detailed documentation block. - -HIDE_IN_BODY_DOCS = NO - -# The INTERNAL_DOCS tag determines if documentation -# that is typed after a \internal command is included. If the tag is set -# to NO (the default) then the documentation will be excluded. -# Set it to YES to include the internal documentation. - -INTERNAL_DOCS = NO - -# If the CASE_SENSE_NAMES tag is set to NO then Doxygen will only generate -# file names in lower-case letters. If set to YES upper-case letters are also -# allowed. This is useful if you have classes or files whose names only differ -# in case and if your file system supports case sensitive file names. Windows -# and Mac users are advised to set this option to NO. - -CASE_SENSE_NAMES = YES - -# If the HIDE_SCOPE_NAMES tag is set to NO (the default) then Doxygen -# will show members with their full class and namespace scopes in the -# documentation. If set to YES the scope will be hidden. - -HIDE_SCOPE_NAMES = NO - -# If the SHOW_INCLUDE_FILES tag is set to YES (the default) then Doxygen -# will put a list of the files that are included by a file in the documentation -# of that file. - -SHOW_INCLUDE_FILES = YES - -# If the INLINE_INFO tag is set to YES (the default) then a tag [inline] -# is inserted in the documentation for inline members. - -INLINE_INFO = YES - -# If the SORT_MEMBER_DOCS tag is set to YES (the default) then doxygen -# will sort the (detailed) documentation of file and class members -# alphabetically by member name. If set to NO the members will appear in -# declaration order. - -SORT_MEMBER_DOCS = YES - -# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the -# brief documentation of file, namespace and class members alphabetically -# by member name. If set to NO (the default) the members will appear in -# declaration order. - -SORT_BRIEF_DOCS = NO - -# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the -# hierarchy of group names into alphabetical order. If set to NO (the default) -# the group names will appear in their defined order. - -SORT_GROUP_NAMES = NO - -# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be -# sorted by fully-qualified names, including namespaces. If set to -# NO (the default), the class list will be sorted only by class name, -# not including the namespace part. -# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. -# Note: This option applies only to the class list, not to the -# alphabetical list. - -SORT_BY_SCOPE_NAME = NO - -# The GENERATE_TODOLIST tag can be used to enable (YES) or -# disable (NO) the todo list. This list is created by putting \todo -# commands in the documentation. - -GENERATE_TODOLIST = YES - -# The GENERATE_TESTLIST tag can be used to enable (YES) or -# disable (NO) the test list. This list is created by putting \test -# commands in the documentation. - -GENERATE_TESTLIST = YES - -# The GENERATE_BUGLIST tag can be used to enable (YES) or -# disable (NO) the bug list. This list is created by putting \bug -# commands in the documentation. - -GENERATE_BUGLIST = YES - -# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or -# disable (NO) the deprecated list. This list is created by putting -# \deprecated commands in the documentation. - -GENERATE_DEPRECATEDLIST = YES - -# The ENABLED_SECTIONS tag can be used to enable conditional -# documentation sections, marked by \if sectionname ... \endif. - -ENABLED_SECTIONS = - -# The MAX_INITIALIZER_LINES tag determines the maximum number of lines -# the initial value of a variable or define consists of for it to appear in -# the documentation. If the initializer consists of more lines than specified -# here it will be hidden. Use a value of 0 to hide initializers completely. -# The appearance of the initializer of individual variables and defines in the -# documentation can be controlled using \showinitializer or \hideinitializer -# command in the documentation regardless of this setting. - -MAX_INITIALIZER_LINES = 30 - -# Set the SHOW_USED_FILES tag to NO to disable the list of files generated -# at the bottom of the documentation of classes and structs. If set to YES the -# list will mention the files that were used to generate the documentation. - -SHOW_USED_FILES = YES - -# If the sources in your project are distributed over multiple directories -# then setting the SHOW_DIRECTORIES tag to YES will show the directory hierarchy -# in the documentation. The default is NO. - -SHOW_DIRECTORIES = NO - -# Set the SHOW_FILES tag to NO to disable the generation of the Files page. -# This will remove the Files entry from the Quick Index and from the -# Folder Tree View (if specified). The default is YES. - -SHOW_FILES = YES - -# Set the SHOW_NAMESPACES tag to NO to disable the generation of the -# Namespaces page. This will remove the Namespaces entry from the Quick Index -# and from the Folder Tree View (if specified). The default is YES. - -SHOW_NAMESPACES = YES - -# The FILE_VERSION_FILTER tag can be used to specify a program or script that -# doxygen should invoke to get the current version for each file (typically from -# the version control system). Doxygen will invoke the program by executing (via -# popen()) the command , where is the value of -# the FILE_VERSION_FILTER tag, and is the name of an input file -# provided by doxygen. Whatever the program writes to standard output -# is used as the file version. See the manual for examples. - -FILE_VERSION_FILTER = - -#--------------------------------------------------------------------------- -# configuration options related to warning and progress messages -#--------------------------------------------------------------------------- - -# The QUIET tag can be used to turn on/off the messages that are generated -# by doxygen. Possible values are YES and NO. If left blank NO is used. - -QUIET = NO - -# The WARNINGS tag can be used to turn on/off the warning messages that are -# generated by doxygen. Possible values are YES and NO. If left blank -# NO is used. - -WARNINGS = YES - -# If WARN_IF_UNDOCUMENTED is set to YES, then doxygen will generate warnings -# for undocumented members. If EXTRACT_ALL is set to YES then this flag will -# automatically be disabled. - -WARN_IF_UNDOCUMENTED = YES - -# If WARN_IF_DOC_ERROR is set to YES, doxygen will generate warnings for -# potential errors in the documentation, such as not documenting some -# parameters in a documented function, or documenting parameters that -# don't exist or using markup commands wrongly. - -WARN_IF_DOC_ERROR = YES - -# This WARN_NO_PARAMDOC option can be abled to get warnings for -# functions that are documented, but have no documentation for their parameters -# or return value. If set to NO (the default) doxygen will only warn about -# wrong or incomplete parameter documentation, but not about the absence of -# documentation. - -WARN_NO_PARAMDOC = NO - -# The WARN_FORMAT tag determines the format of the warning messages that -# doxygen can produce. The string should contain the $file, $line, and $text -# tags, which will be replaced by the file and line number from which the -# warning originated and the warning text. Optionally the format may contain -# $version, which will be replaced by the version of the file (if it could -# be obtained via FILE_VERSION_FILTER) - -WARN_FORMAT = "$file:$line: $text" - -# The WARN_LOGFILE tag can be used to specify a file to which warning -# and error messages should be written. If left blank the output is written -# to stderr. - -WARN_LOGFILE = - -#--------------------------------------------------------------------------- -# configuration options related to the input files -#--------------------------------------------------------------------------- - -# The INPUT tag can be used to specify the files and/or directories that contain -# documented source files. You may enter file names like "myfile.cpp" or -# directories like "/usr/src/myproject". Separate the files or directories -# with spaces. - -INPUT = - -# This tag can be used to specify the character encoding of the source files -# that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is -# also the default input encoding. Doxygen uses libiconv (or the iconv built -# into libc) for the transcoding. See http://www.gnu.org/software/libiconv for -# the list of possible encodings. - -INPUT_ENCODING = UTF-8 - -# If the value of the INPUT tag contains directories, you can use the -# FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp -# and *.h) to filter out the source-files in the directories. If left -# blank the following patterns are tested: -# *.c *.cc *.cxx *.cpp *.c++ *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh *.hxx -# *.hpp *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm *.py *.f90 - -FILE_PATTERNS = - -# The RECURSIVE tag can be used to turn specify whether or not subdirectories -# should be searched for input files as well. Possible values are YES and NO. -# If left blank NO is used. - -RECURSIVE = NO - -# The EXCLUDE tag can be used to specify files and/or directories that should -# excluded from the INPUT source files. This way you can easily exclude a -# subdirectory from a directory tree whose root is specified with the INPUT tag. - -EXCLUDE = - -# The EXCLUDE_SYMLINKS tag can be used select whether or not files or -# directories that are symbolic links (a Unix filesystem feature) are excluded -# from the input. - -EXCLUDE_SYMLINKS = NO - -# If the value of the INPUT tag contains directories, you can use the -# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude -# certain files from those directories. Note that the wildcards are matched -# against the file with absolute path, so to exclude all test directories -# for example use the pattern */test/* - -EXCLUDE_PATTERNS = - -# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names -# (namespaces, classes, functions, etc.) that should be excluded from the -# output. The symbol name can be a fully qualified name, a word, or if the -# wildcard * is used, a substring. Examples: ANamespace, AClass, -# AClass::ANamespace, ANamespace::*Test - -EXCLUDE_SYMBOLS = - -# The EXAMPLE_PATH tag can be used to specify one or more files or -# directories that contain example code fragments that are included (see -# the \include command). - -EXAMPLE_PATH = - -# If the value of the EXAMPLE_PATH tag contains directories, you can use the -# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp -# and *.h) to filter out the source-files in the directories. If left -# blank all files are included. - -EXAMPLE_PATTERNS = - -# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be -# searched for input files to be used with the \include or \dontinclude -# commands irrespective of the value of the RECURSIVE tag. -# Possible values are YES and NO. If left blank NO is used. - -EXAMPLE_RECURSIVE = NO - -# The IMAGE_PATH tag can be used to specify one or more files or -# directories that contain image that are included in the documentation (see -# the \image command). - -IMAGE_PATH = - -# The INPUT_FILTER tag can be used to specify a program that doxygen should -# invoke to filter for each input file. Doxygen will invoke the filter program -# by executing (via popen()) the command , where -# is the value of the INPUT_FILTER tag, and is the name of an -# input file. Doxygen will then use the output that the filter program writes -# to standard output. If FILTER_PATTERNS is specified, this tag will be -# ignored. - -INPUT_FILTER = - -# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern -# basis. Doxygen will compare the file name with each pattern and apply the -# filter if there is a match. The filters are a list of the form: -# pattern=filter (like *.cpp=my_cpp_filter). See INPUT_FILTER for further -# info on how filters are used. If FILTER_PATTERNS is empty, INPUT_FILTER -# is applied to all files. - -FILTER_PATTERNS = - -# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using -# INPUT_FILTER) will be used to filter the input files when producing source -# files to browse (i.e. when SOURCE_BROWSER is set to YES). - -FILTER_SOURCE_FILES = NO - -#--------------------------------------------------------------------------- -# configuration options related to source browsing -#--------------------------------------------------------------------------- - -# If the SOURCE_BROWSER tag is set to YES then a list of source files will -# be generated. Documented entities will be cross-referenced with these sources. -# Note: To get rid of all source code in the generated output, make sure also -# VERBATIM_HEADERS is set to NO. - -SOURCE_BROWSER = NO - -# Setting the INLINE_SOURCES tag to YES will include the body -# of functions and classes directly in the documentation. - -INLINE_SOURCES = YES - -# Setting the STRIP_CODE_COMMENTS tag to YES (the default) will instruct -# doxygen to hide any special comment blocks from generated source code -# fragments. Normal C and C++ comments will always remain visible. - -STRIP_CODE_COMMENTS = YES - -# If the REFERENCED_BY_RELATION tag is set to YES -# then for each documented function all documented -# functions referencing it will be listed. - -REFERENCED_BY_RELATION = NO - -# If the REFERENCES_RELATION tag is set to YES -# then for each documented function all documented entities -# called/used by that function will be listed. - -REFERENCES_RELATION = NO - -# If the REFERENCES_LINK_SOURCE tag is set to YES (the default) -# and SOURCE_BROWSER tag is set to YES, then the hyperlinks from -# functions in REFERENCES_RELATION and REFERENCED_BY_RELATION lists will -# link to the source code. Otherwise they will link to the documentstion. - -REFERENCES_LINK_SOURCE = YES - -# If the USE_HTAGS tag is set to YES then the references to source code -# will point to the HTML generated by the htags(1) tool instead of doxygen -# built-in source browser. The htags tool is part of GNU's global source -# tagging system (see http://www.gnu.org/software/global/global.html). You -# will need version 4.8.6 or higher. - -USE_HTAGS = NO - -# If the VERBATIM_HEADERS tag is set to YES (the default) then Doxygen -# will generate a verbatim copy of the header file for each class for -# which an include is specified. Set to NO to disable this. - -VERBATIM_HEADERS = NO - -#--------------------------------------------------------------------------- -# configuration options related to the alphabetical class index -#--------------------------------------------------------------------------- - -# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index -# of all compounds will be generated. Enable this if the project -# contains a lot of classes, structs, unions or interfaces. - -ALPHABETICAL_INDEX = NO - -# If the alphabetical index is enabled (see ALPHABETICAL_INDEX) then -# the COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns -# in which this list will be split (can be a number in the range [1..20]) - -COLS_IN_ALPHA_INDEX = 5 - -# In case all classes in a project start with a common prefix, all -# classes will be put under the same header in the alphabetical index. -# The IGNORE_PREFIX tag can be used to specify one or more prefixes that -# should be ignored while generating the index headers. - -IGNORE_PREFIX = - -#--------------------------------------------------------------------------- -# configuration options related to the HTML output -#--------------------------------------------------------------------------- - -# If the GENERATE_HTML tag is set to YES (the default) Doxygen will -# generate HTML output. - -GENERATE_HTML = YES - -# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. -# If a relative path is entered the value of OUTPUT_DIRECTORY will be -# put in front of it. If left blank `html' will be used as the default path. - -HTML_OUTPUT = html - -# The HTML_FILE_EXTENSION tag can be used to specify the file extension for -# each generated HTML page (for example: .htm,.php,.asp). If it is left blank -# doxygen will generate files with .html extension. - -HTML_FILE_EXTENSION = .html - -# The HTML_HEADER tag can be used to specify a personal HTML header for -# each generated HTML page. If it is left blank doxygen will generate a -# standard header. - -HTML_HEADER = - -# The HTML_FOOTER tag can be used to specify a personal HTML footer for -# each generated HTML page. If it is left blank doxygen will generate a -# standard footer. - -HTML_FOOTER = - -# The HTML_STYLESHEET tag can be used to specify a user-defined cascading -# style sheet that is used by each HTML page. It can be used to -# fine-tune the look of the HTML output. If the tag is left blank doxygen -# will generate a default style sheet. Note that doxygen will try to copy -# the style sheet file to the HTML output directory, so don't put your own -# stylesheet in the HTML output directory as well, or it will be erased! - -HTML_STYLESHEET = - -# If the HTML_ALIGN_MEMBERS tag is set to YES, the members of classes, -# files or namespaces will be aligned in HTML using tables. If set to -# NO a bullet list will be used. - -HTML_ALIGN_MEMBERS = YES - -# If the GENERATE_HTMLHELP tag is set to YES, additional index files -# will be generated that can be used as input for tools like the -# Microsoft HTML help workshop to generate a compiled HTML help file (.chm) -# of the generated HTML documentation. - -GENERATE_HTMLHELP = NO - -# If the GENERATE_DOCSET tag is set to YES, additional index files -# will be generated that can be used as input for Apple's Xcode 3 -# integrated development environment, introduced with OSX 10.5 (Leopard). -# To create a documentation set, doxygen will generate a Makefile in the -# HTML output directory. Running make will produce the docset in that -# directory and running "make install" will install the docset in -# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find -# it at startup. - -GENERATE_DOCSET = NO - -# When GENERATE_DOCSET tag is set to YES, this tag determines the name of the -# feed. A documentation feed provides an umbrella under which multiple -# documentation sets from a single provider (such as a company or product suite) -# can be grouped. - -DOCSET_FEEDNAME = "Doxygen generated docs" - -# When GENERATE_DOCSET tag is set to YES, this tag specifies a string that -# should uniquely identify the documentation set bundle. This should be a -# reverse domain-name style string, e.g. com.mycompany.MyDocSet. Doxygen -# will append .docset to the name. - -DOCSET_BUNDLE_ID = org.doxygen.Project - -# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML -# documentation will contain sections that can be hidden and shown after the -# page has loaded. For this to work a browser that supports -# JavaScript and DHTML is required (for instance Mozilla 1.0+, Firefox -# Netscape 6.0+, Internet explorer 5.0+, Konqueror, or Safari). - -HTML_DYNAMIC_SECTIONS = NO - -# If the GENERATE_HTMLHELP tag is set to YES, the CHM_FILE tag can -# be used to specify the file name of the resulting .chm file. You -# can add a path in front of the file if the result should not be -# written to the html output directory. - -CHM_FILE = - -# If the GENERATE_HTMLHELP tag is set to YES, the HHC_LOCATION tag can -# be used to specify the location (absolute path including file name) of -# the HTML help compiler (hhc.exe). If non-empty doxygen will try to run -# the HTML help compiler on the generated index.hhp. - -HHC_LOCATION = - -# If the GENERATE_HTMLHELP tag is set to YES, the GENERATE_CHI flag -# controls if a separate .chi index file is generated (YES) or that -# it should be included in the master .chm file (NO). - -GENERATE_CHI = NO - -# If the GENERATE_HTMLHELP tag is set to YES, the CHM_INDEX_ENCODING -# is used to encode HtmlHelp index (hhk), content (hhc) and project file -# content. - -CHM_INDEX_ENCODING = - -# If the GENERATE_HTMLHELP tag is set to YES, the BINARY_TOC flag -# controls whether a binary table of contents is generated (YES) or a -# normal table of contents (NO) in the .chm file. - -BINARY_TOC = NO - -# The TOC_EXPAND flag can be set to YES to add extra items for group members -# to the contents of the HTML help documentation and to the tree view. - -TOC_EXPAND = NO - -# The DISABLE_INDEX tag can be used to turn on/off the condensed index at -# top of each HTML page. The value NO (the default) enables the index and -# the value YES disables it. - -DISABLE_INDEX = NO - -# This tag can be used to set the number of enum values (range [1..20]) -# that doxygen will group on one line in the generated HTML documentation. - -ENUM_VALUES_PER_LINE = 4 - -# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index -# structure should be generated to display hierarchical information. -# If the tag value is set to FRAME, a side panel will be generated -# containing a tree-like index structure (just like the one that -# is generated for HTML Help). For this to work a browser that supports -# JavaScript, DHTML, CSS and frames is required (for instance Mozilla 1.0+, -# Netscape 6.0+, Internet explorer 5.0+, or Konqueror). Windows users are -# probably better off using the HTML help feature. Other possible values -# for this tag are: HIERARCHIES, which will generate the Groups, Directories, -# and Class Hiererachy pages using a tree view instead of an ordered list; -# ALL, which combines the behavior of FRAME and HIERARCHIES; and NONE, which -# disables this behavior completely. For backwards compatibility with previous -# releases of Doxygen, the values YES and NO are equivalent to FRAME and NONE -# respectively. - -GENERATE_TREEVIEW = YES - -# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be -# used to set the initial width (in pixels) of the frame in which the tree -# is shown. - -TREEVIEW_WIDTH = 250 - -# Use this tag to change the font size of Latex formulas included -# as images in the HTML documentation. The default is 10. Note that -# when you change the font size after a successful doxygen run you need -# to manually remove any form_*.png images from the HTML output directory -# to force them to be regenerated. - -FORMULA_FONTSIZE = 10 - -#--------------------------------------------------------------------------- -# configuration options related to the LaTeX output -#--------------------------------------------------------------------------- - -# If the GENERATE_LATEX tag is set to YES (the default) Doxygen will -# generate Latex output. - -GENERATE_LATEX = NO - -# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. -# If a relative path is entered the value of OUTPUT_DIRECTORY will be -# put in front of it. If left blank `latex' will be used as the default path. - -LATEX_OUTPUT = latex - -# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be -# invoked. If left blank `latex' will be used as the default command name. - -LATEX_CMD_NAME = latex - -# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to -# generate index for LaTeX. If left blank `makeindex' will be used as the -# default command name. - -MAKEINDEX_CMD_NAME = makeindex - -# If the COMPACT_LATEX tag is set to YES Doxygen generates more compact -# LaTeX documents. This may be useful for small projects and may help to -# save some trees in general. - -COMPACT_LATEX = NO - -# The PAPER_TYPE tag can be used to set the paper type that is used -# by the printer. Possible values are: a4, a4wide, letter, legal and -# executive. If left blank a4wide will be used. - -PAPER_TYPE = a4wide - -# The EXTRA_PACKAGES tag can be to specify one or more names of LaTeX -# packages that should be included in the LaTeX output. - -EXTRA_PACKAGES = - -# The LATEX_HEADER tag can be used to specify a personal LaTeX header for -# the generated latex document. The header should contain everything until -# the first chapter. If it is left blank doxygen will generate a -# standard header. Notice: only use this tag if you know what you are doing! - -LATEX_HEADER = - -# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated -# is prepared for conversion to pdf (using ps2pdf). The pdf file will -# contain links (just like the HTML output) instead of page references -# This makes the output suitable for online browsing using a pdf viewer. - -PDF_HYPERLINKS = YES - -# If the USE_PDFLATEX tag is set to YES, pdflatex will be used instead of -# plain latex in the generated Makefile. Set this option to YES to get a -# higher quality PDF documentation. - -USE_PDFLATEX = YES - -# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \\batchmode. -# command to the generated LaTeX files. This will instruct LaTeX to keep -# running if errors occur, instead of asking the user for help. -# This option is also used when generating formulas in HTML. - -LATEX_BATCHMODE = NO - -# If LATEX_HIDE_INDICES is set to YES then doxygen will not -# include the index chapters (such as File Index, Compound Index, etc.) -# in the output. - -LATEX_HIDE_INDICES = NO - -#--------------------------------------------------------------------------- -# configuration options related to the RTF output -#--------------------------------------------------------------------------- - -# If the GENERATE_RTF tag is set to YES Doxygen will generate RTF output -# The RTF output is optimized for Word 97 and may not look very pretty with -# other RTF readers or editors. - -GENERATE_RTF = NO - -# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. -# If a relative path is entered the value of OUTPUT_DIRECTORY will be -# put in front of it. If left blank `rtf' will be used as the default path. - -RTF_OUTPUT = rtf - -# If the COMPACT_RTF tag is set to YES Doxygen generates more compact -# RTF documents. This may be useful for small projects and may help to -# save some trees in general. - -COMPACT_RTF = NO - -# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated -# will contain hyperlink fields. The RTF file will -# contain links (just like the HTML output) instead of page references. -# This makes the output suitable for online browsing using WORD or other -# programs which support those fields. -# Note: wordpad (write) and others do not support links. - -RTF_HYPERLINKS = NO - -# Load stylesheet definitions from file. Syntax is similar to doxygen's -# config file, i.e. a series of assignments. You only have to provide -# replacements, missing definitions are set to their default value. - -RTF_STYLESHEET_FILE = - -# Set optional variables used in the generation of an rtf document. -# Syntax is similar to doxygen's config file. - -RTF_EXTENSIONS_FILE = - -#--------------------------------------------------------------------------- -# configuration options related to the man page output -#--------------------------------------------------------------------------- - -# If the GENERATE_MAN tag is set to YES (the default) Doxygen will -# generate man pages - -GENERATE_MAN = NO - -# The MAN_OUTPUT tag is used to specify where the man pages will be put. -# If a relative path is entered the value of OUTPUT_DIRECTORY will be -# put in front of it. If left blank `man' will be used as the default path. - -MAN_OUTPUT = man - -# The MAN_EXTENSION tag determines the extension that is added to -# the generated man pages (default is the subroutine's section .3) - -MAN_EXTENSION = .3 - -# If the MAN_LINKS tag is set to YES and Doxygen generates man output, -# then it will generate one additional man file for each entity -# documented in the real man page(s). These additional files -# only source the real man page, but without them the man command -# would be unable to find the correct page. The default is NO. - -MAN_LINKS = NO - -#--------------------------------------------------------------------------- -# configuration options related to the XML output -#--------------------------------------------------------------------------- - -# If the GENERATE_XML tag is set to YES Doxygen will -# generate an XML file that captures the structure of -# the code including all documentation. - -GENERATE_XML = NO - -# The XML_OUTPUT tag is used to specify where the XML pages will be put. -# If a relative path is entered the value of OUTPUT_DIRECTORY will be -# put in front of it. If left blank `xml' will be used as the default path. - -XML_OUTPUT = xml - -# The XML_SCHEMA tag can be used to specify an XML schema, -# which can be used by a validating XML parser to check the -# syntax of the XML files. - -XML_SCHEMA = - -# The XML_DTD tag can be used to specify an XML DTD, -# which can be used by a validating XML parser to check the -# syntax of the XML files. - -XML_DTD = - -# If the XML_PROGRAMLISTING tag is set to YES Doxygen will -# dump the program listings (including syntax highlighting -# and cross-referencing information) to the XML output. Note that -# enabling this will significantly increase the size of the XML output. - -XML_PROGRAMLISTING = YES - -#--------------------------------------------------------------------------- -# configuration options for the AutoGen Definitions output -#--------------------------------------------------------------------------- - -# If the GENERATE_AUTOGEN_DEF tag is set to YES Doxygen will -# generate an AutoGen Definitions (see autogen.sf.net) file -# that captures the structure of the code including all -# documentation. Note that this feature is still experimental -# and incomplete at the moment. - -GENERATE_AUTOGEN_DEF = NO - -#--------------------------------------------------------------------------- -# configuration options related to the Perl module output -#--------------------------------------------------------------------------- - -# If the GENERATE_PERLMOD tag is set to YES Doxygen will -# generate a Perl module file that captures the structure of -# the code including all documentation. Note that this -# feature is still experimental and incomplete at the -# moment. - -GENERATE_PERLMOD = NO - -# If the PERLMOD_LATEX tag is set to YES Doxygen will generate -# the necessary Makefile rules, Perl scripts and LaTeX code to be able -# to generate PDF and DVI output from the Perl module output. - -PERLMOD_LATEX = NO - -# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be -# nicely formatted so it can be parsed by a human reader. This is useful -# if you want to understand what is going on. On the other hand, if this -# tag is set to NO the size of the Perl module output will be much smaller -# and Perl will parse it just the same. - -PERLMOD_PRETTY = YES - -# The names of the make variables in the generated doxyrules.make file -# are prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. -# This is useful so different doxyrules.make files included by the same -# Makefile don't overwrite each other's variables. - -PERLMOD_MAKEVAR_PREFIX = - -#--------------------------------------------------------------------------- -# Configuration options related to the preprocessor -#--------------------------------------------------------------------------- - -# If the ENABLE_PREPROCESSING tag is set to YES (the default) Doxygen will -# evaluate all C-preprocessor directives found in the sources and include -# files. - -ENABLE_PREPROCESSING = YES - -# If the MACRO_EXPANSION tag is set to YES Doxygen will expand all macro -# names in the source code. If set to NO (the default) only conditional -# compilation will be performed. Macro expansion can be done in a controlled -# way by setting EXPAND_ONLY_PREDEF to YES. - -MACRO_EXPANSION = NO - -# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES -# then the macro expansion is limited to the macros specified with the -# PREDEFINED and EXPAND_AS_DEFINED tags. - -EXPAND_ONLY_PREDEF = NO - -# If the SEARCH_INCLUDES tag is set to YES (the default) the includes files -# in the INCLUDE_PATH (see below) will be search if a #include is found. - -SEARCH_INCLUDES = YES - -# The INCLUDE_PATH tag can be used to specify one or more directories that -# contain include files that are not input files but should be processed by -# the preprocessor. - -INCLUDE_PATH = - -# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard -# patterns (like *.h and *.hpp) to filter out the header-files in the -# directories. If left blank, the patterns specified with FILE_PATTERNS will -# be used. - -INCLUDE_FILE_PATTERNS = - -# The PREDEFINED tag can be used to specify one or more macro names that -# are defined before the preprocessor is started (similar to the -D option of -# gcc). The argument of the tag is a list of macros of the form: name -# or name=definition (no spaces). If the definition and the = are -# omitted =1 is assumed. To prevent a macro definition from being -# undefined via #undef or recursively expanded use the := operator -# instead of the = operator. - -PREDEFINED = - -# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then -# this tag can be used to specify a list of macro names that should be expanded. -# The macro definition that is found in the sources will be used. -# Use the PREDEFINED tag if you want to use a different macro definition. - -EXPAND_AS_DEFINED = - -# If the SKIP_FUNCTION_MACROS tag is set to YES (the default) then -# doxygen's preprocessor will remove all function-like macros that are alone -# on a line, have an all uppercase name, and do not end with a semicolon. Such -# function macros are typically used for boiler-plate code, and will confuse -# the parser if not removed. - -SKIP_FUNCTION_MACROS = YES - -#--------------------------------------------------------------------------- -# Configuration::additions related to external references -#--------------------------------------------------------------------------- - -# The TAGFILES option can be used to specify one or more tagfiles. -# Optionally an initial location of the external documentation -# can be added for each tagfile. The format of a tag file without -# this location is as follows: -# TAGFILES = file1 file2 ... -# Adding location for the tag files is done as follows: -# TAGFILES = file1=loc1 "file2 = loc2" ... -# where "loc1" and "loc2" can be relative or absolute paths or -# URLs. If a location is present for each tag, the installdox tool -# does not have to be run to correct the links. -# Note that each tag file must have a unique name -# (where the name does NOT include the path) -# If a tag file is not located in the directory in which doxygen -# is run, you must also specify the path to the tagfile here. - -TAGFILES = - -# When a file name is specified after GENERATE_TAGFILE, doxygen will create -# a tag file that is based on the input files it reads. - -GENERATE_TAGFILE = - -# If the ALLEXTERNALS tag is set to YES all external classes will be listed -# in the class index. If set to NO only the inherited external classes -# will be listed. - -ALLEXTERNALS = NO - -# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed -# in the modules index. If set to NO, only the current project's groups will -# be listed. - -EXTERNAL_GROUPS = YES - -# The PERL_PATH should be the absolute path and name of the perl script -# interpreter (i.e. the result of `which perl'). - -PERL_PATH = /usr/bin/perl - -#--------------------------------------------------------------------------- -# Configuration options related to the dot tool -#--------------------------------------------------------------------------- - -# If the CLASS_DIAGRAMS tag is set to YES (the default) Doxygen will -# generate a inheritance diagram (in HTML, RTF and LaTeX) for classes with base -# or super classes. Setting the tag to NO turns the diagrams off. Note that -# this option is superseded by the HAVE_DOT option below. This is only a -# fallback. It is recommended to install and use dot, since it yields more -# powerful graphs. - -CLASS_DIAGRAMS = YES - -# You can define message sequence charts within doxygen comments using the \msc -# command. Doxygen will then run the mscgen tool (see -# http://www.mcternan.me.uk/mscgen/) to produce the chart and insert it in the -# documentation. The MSCGEN_PATH tag allows you to specify the directory where -# the mscgen tool resides. If left empty the tool is assumed to be found in the -# default search path. - -MSCGEN_PATH = - -# If set to YES, the inheritance and collaboration graphs will hide -# inheritance and usage relations if the target is undocumented -# or is not a class. - -HIDE_UNDOC_RELATIONS = YES - -# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is -# available from the path. This tool is part of Graphviz, a graph visualization -# toolkit from AT&T and Lucent Bell Labs. The other options in this section -# have no effect if this option is set to NO (the default) - -HAVE_DOT = YES - -# By default doxygen will write a font called FreeSans.ttf to the output -# directory and reference it in all dot files that doxygen generates. This -# font does not include all possible unicode characters however, so when you need -# these (or just want a differently looking font) you can specify the font name -# using DOT_FONTNAME. You need need to make sure dot is able to find the font, -# which can be done by putting it in a standard location or by setting the -# DOTFONTPATH environment variable or by setting DOT_FONTPATH to the directory -# containing the font. - -DOT_FONTNAME = FreeSans - -# By default doxygen will tell dot to use the output directory to look for the -# FreeSans.ttf font (which doxygen will put there itself). If you specify a -# different font using DOT_FONTNAME you can set the path where dot -# can find it using this tag. - -DOT_FONTPATH = - -# If the CLASS_GRAPH and HAVE_DOT tags are set to YES then doxygen -# will generate a graph for each documented class showing the direct and -# indirect inheritance relations. Setting this tag to YES will force the -# the CLASS_DIAGRAMS tag to NO. - -CLASS_GRAPH = YES - -# If the COLLABORATION_GRAPH and HAVE_DOT tags are set to YES then doxygen -# will generate a graph for each documented class showing the direct and -# indirect implementation dependencies (inheritance, containment, and -# class references variables) of the class with other documented classes. - -COLLABORATION_GRAPH = YES - -# If the GROUP_GRAPHS and HAVE_DOT tags are set to YES then doxygen -# will generate a graph for groups, showing the direct groups dependencies - -GROUP_GRAPHS = YES - -# If the UML_LOOK tag is set to YES doxygen will generate inheritance and -# collaboration diagrams in a style similar to the OMG's Unified Modeling -# Language. - -UML_LOOK = NO - -# If set to YES, the inheritance and collaboration graphs will show the -# relations between templates and their instances. - -TEMPLATE_RELATIONS = NO - -# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDE_GRAPH, and HAVE_DOT -# tags are set to YES then doxygen will generate a graph for each documented -# file showing the direct and indirect include dependencies of the file with -# other documented files. - -INCLUDE_GRAPH = YES - -# If the ENABLE_PREPROCESSING, SEARCH_INCLUDES, INCLUDED_BY_GRAPH, and -# HAVE_DOT tags are set to YES then doxygen will generate a graph for each -# documented header file showing the documented files that directly or -# indirectly include this file. - -INCLUDED_BY_GRAPH = YES - -# If the CALL_GRAPH and HAVE_DOT options are set to YES then -# doxygen will generate a call dependency graph for every global function -# or class method. Note that enabling this option will significantly increase -# the time of a run. So in most cases it will be better to enable call graphs -# for selected functions only using the \callgraph command. - -CALL_GRAPH = YES - -# If the CALLER_GRAPH and HAVE_DOT tags are set to YES then -# doxygen will generate a caller dependency graph for every global function -# or class method. Note that enabling this option will significantly increase -# the time of a run. So in most cases it will be better to enable caller -# graphs for selected functions only using the \callergraph command. - -CALLER_GRAPH = NO - -# If the GRAPHICAL_HIERARCHY and HAVE_DOT tags are set to YES then doxygen -# will graphical hierarchy of all classes instead of a textual one. - -GRAPHICAL_HIERARCHY = YES - -# If the DIRECTORY_GRAPH, SHOW_DIRECTORIES and HAVE_DOT tags are set to YES -# then doxygen will show the dependencies a directory has on other directories -# in a graphical way. The dependency relations are determined by the #include -# relations between the files in the directories. - -DIRECTORY_GRAPH = YES - -# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images -# generated by dot. Possible values are png, jpg, or gif -# If left blank png will be used. - -DOT_IMAGE_FORMAT = png - -# The tag DOT_PATH can be used to specify the path where the dot tool can be -# found. If left blank, it is assumed the dot tool can be found in the path. - -DOT_PATH = - -# The DOTFILE_DIRS tag can be used to specify one or more directories that -# contain dot files that are included in the documentation (see the -# \dotfile command). - -DOTFILE_DIRS = - -# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of -# nodes that will be shown in the graph. If the number of nodes in a graph -# becomes larger than this value, doxygen will truncate the graph, which is -# visualized by representing a node as a red box. Note that doxygen if the -# number of direct children of the root node in a graph is already larger than -# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note -# that the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH. - -DOT_GRAPH_MAX_NODES = 50 - -# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the -# graphs generated by dot. A depth value of 3 means that only nodes reachable -# from the root by following a path via at most 3 edges will be shown. Nodes -# that lay further from the root node will be omitted. Note that setting this -# option to 1 or 2 may greatly reduce the computation time needed for large -# code bases. Also note that the size of a graph can be further restricted by -# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction. - -MAX_DOT_GRAPH_DEPTH = 0 - -# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent -# background. This is enabled by default, which results in a transparent -# background. Warning: Depending on the platform used, enabling this option -# may lead to badly anti-aliased labels on the edges of a graph (i.e. they -# become hard to read). - -DOT_TRANSPARENT = YES - -# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output -# files in one run (i.e. multiple -o and -T options on the command line). This -# makes dot run faster, but since only newer versions of dot (>1.8.10) -# support this, this feature is disabled by default. - -DOT_MULTI_TARGETS = NO - -# If the GENERATE_LEGEND tag is set to YES (the default) Doxygen will -# generate a legend page explaining the meaning of the various boxes and -# arrows in the dot generated graphs. - -GENERATE_LEGEND = YES - -# If the DOT_CLEANUP tag is set to YES (the default) Doxygen will -# remove the intermediate dot files that are used to generate -# the various graphs. - -DOT_CLEANUP = YES - -#--------------------------------------------------------------------------- -# Configuration::additions related to the search engine -#--------------------------------------------------------------------------- - -# The SEARCHENGINE tag specifies whether or not a search engine should be -# used. If set to NO the values of all tags below this one will be ignored. - -SEARCHENGINE = NO diff --git a/gtsam/inference/Factor-inl.h b/gtsam/inference/Factor-inl.h index 09336186e..d05cc1de3 100644 --- a/gtsam/inference/Factor-inl.h +++ b/gtsam/inference/Factor-inl.h @@ -45,8 +45,8 @@ namespace gtsam { void Factor::assertInvariants() const { #ifndef NDEBUG // Check that keys are all unique - std::multiset < Key > nonunique(keys_.begin(), keys_.end()); - std::set < Key > unique(keys_.begin(), keys_.end()); + std::multiset < KeyType > nonunique(keys_.begin(), keys_.end()); + std::set < KeyType > unique(keys_.begin(), keys_.end()); bool correct = (nonunique.size() == unique.size()) && std::equal(nonunique.begin(), nonunique.end(), unique.begin()); if (!correct) throw std::logic_error( diff --git a/gtsam/inference/Factor.h b/gtsam/inference/Factor.h index 5a0b1e333..962bf44d1 100644 --- a/gtsam/inference/Factor.h +++ b/gtsam/inference/Factor.h @@ -55,28 +55,28 @@ class Factor { public: - typedef KEY Key; ///< The KEY template parameter - typedef Factor This; ///< This class + typedef KEY KeyType; ///< The KEY template parameter + typedef Factor This; ///< This class /** * Typedef to the conditional type obtained by eliminating this factor, * derived classes must redefine this. */ - typedef Conditional ConditionalType; + typedef Conditional ConditionalType; /// A shared_ptr to this class, derived classes must redefine this. typedef boost::shared_ptr shared_ptr; /// Iterator over keys - typedef typename std::vector::iterator iterator; + typedef typename std::vector::iterator iterator; /// Const iterator over keys - typedef typename std::vector::const_iterator const_iterator; + typedef typename std::vector::const_iterator const_iterator; protected: /// The keys involved in this factor - std::vector keys_; + std::vector keys_; friend class JacobianFactor; friend class HessianFactor; @@ -102,19 +102,19 @@ public: Factor() {} /** Construct unary factor */ - Factor(Key key) : keys_(1) { + Factor(KeyType key) : keys_(1) { keys_[0] = key; assertInvariants(); } /** Construct binary factor */ - Factor(Key key1, Key key2) : keys_(2) { + Factor(KeyType key1, KeyType key2) : keys_(2) { keys_[0] = key1; keys_[1] = key2; assertInvariants(); } /** Construct ternary factor */ - Factor(Key key1, Key key2, Key key3) : keys_(3) { + Factor(KeyType key1, KeyType key2, KeyType key3) : keys_(3) { keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; assertInvariants(); } /** Construct 4-way factor */ - Factor(Key key1, Key key2, Key key3, Key key4) : keys_(4) { + Factor(KeyType key1, KeyType key2, KeyType key3, KeyType key4) : keys_(4) { keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); } /// @} @@ -122,13 +122,13 @@ public: /// @{ /** Construct n-way factor */ - Factor(const std::set& keys) { - BOOST_FOREACH(const Key& key, keys) keys_.push_back(key); + Factor(const std::set& keys) { + BOOST_FOREACH(const KeyType& key, keys) keys_.push_back(key); assertInvariants(); } /** Construct n-way factor */ - Factor(const std::vector& keys) : keys_(keys) { + Factor(const std::vector& keys) : keys_(keys) { assertInvariants(); } @@ -157,16 +157,16 @@ public: /// @{ /// First key - Key front() const { return keys_.front(); } + KeyType front() const { return keys_.front(); } /// Last key - Key back() const { return keys_.back(); } + KeyType back() const { return keys_.back(); } /// find - const_iterator find(Key key) const { return std::find(begin(), end(), key); } + const_iterator find(KeyType key) const { return std::find(begin(), end(), key); } ///TODO: comment - const std::vector& keys() const { return keys_; } + const std::vector& keys() const { return keys_; } /** iterators */ const_iterator begin() const { return keys_.begin(); } ///TODO: comment @@ -194,7 +194,7 @@ public: /** * @return keys involved in this factor */ - std::vector& keys() { return keys_; } + std::vector& keys() { return keys_; } /** mutable iterators */ iterator begin() { return keys_.begin(); } ///TODO: comment diff --git a/gtsam/inference/IndexConditional.cpp b/gtsam/inference/IndexConditional.cpp index eca7a220d..58d5f93d1 100644 --- a/gtsam/inference/IndexConditional.cpp +++ b/gtsam/inference/IndexConditional.cpp @@ -43,11 +43,11 @@ namespace gtsam { /* ************************************************************************* */ bool IndexConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) { #ifndef NDEBUG - BOOST_FOREACH(Key key, frontals()) { assert(key == inversePermutation[key]); } + BOOST_FOREACH(KeyType key, frontals()) { assert(key == inversePermutation[key]); } #endif bool parentChanged = false; - BOOST_FOREACH(Key& parent, parents()) { - Key newParent = inversePermutation[parent]; + BOOST_FOREACH(KeyType& parent, parents()) { + KeyType newParent = inversePermutation[parent]; if(parent != newParent) { parentChanged = true; parent = newParent; @@ -61,8 +61,8 @@ namespace gtsam { void IndexConditional::permuteWithInverse(const Permutation& inversePermutation) { // The permutation may not move the separators into the frontals #ifndef NDEBUG - BOOST_FOREACH(const Key frontal, this->frontals()) { - BOOST_FOREACH(const Key separator, this->parents()) { + BOOST_FOREACH(const KeyType frontal, this->frontals()) { + BOOST_FOREACH(const KeyType separator, this->parents()) { assert(inversePermutation[frontal] < inversePermutation[separator]); } } diff --git a/gtsam/inference/Makefile.am b/gtsam/inference/Makefile.am deleted file mode 100644 index da84df61b..000000000 --- a/gtsam/inference/Makefile.am +++ /dev/null @@ -1,83 +0,0 @@ -#---------------------------------------------------------------------------------------------------- -# GTSAM core functionality: base classes for inference, as well as symbolic and discrete -#---------------------------------------------------------------------------------------------------- - -# use nostdinc to turn off -I. and -I.., we do not need them because -# header files are qualified so they can be included in external projects. -AUTOMAKE_OPTIONS = nostdinc - -headers = -sources = -check_PROGRAMS = - -#---------------------------------------------------------------------------------------------------- -# base -#---------------------------------------------------------------------------------------------------- - -# GTSAM core -headers += Factor.h Factor-inl.h Conditional.h - -# Symbolic Inference -sources += SymbolicFactorGraph.cpp SymbolicMultifrontalSolver.cpp SymbolicSequentialSolver.cpp -check_PROGRAMS += tests/testSymbolicFactor tests/testSymbolicFactorGraph tests/testConditional -check_PROGRAMS += tests/testSymbolicBayesNet tests/testVariableIndex tests/testVariableSlots - -# Inference -headers += GenericMultifrontalSolver.h GenericMultifrontalSolver-inl.h GenericSequentialSolver.h GenericSequentialSolver-inl.h -sources += inference.cpp VariableSlots.cpp Permutation.cpp VariableIndex.cpp -sources += IndexFactor.cpp IndexConditional.cpp -headers += graph.h graph-inl.h -headers += FactorGraph.h FactorGraph-inl.h -headers += ClusterTree.h ClusterTree-inl.h -headers += JunctionTree.h JunctionTree-inl.h -headers += EliminationTree.h EliminationTree-inl.h -headers += BayesNet.h BayesNet-inl.h -headers += BayesTree.h BayesTree-inl.h -headers += BayesTreeCliqueBase.h BayesTreeCliqueBase-inl.h -headers += ISAM.h ISAM-inl.h -check_PROGRAMS += tests/testInference -check_PROGRAMS += tests/testFactorGraph -check_PROGRAMS += tests/testFactorGraph -check_PROGRAMS += tests/testBayesTree -check_PROGRAMS += tests/testISAM -check_PROGRAMS += tests/testEliminationTree -check_PROGRAMS += tests/testClusterTree -check_PROGRAMS += tests/testJunctionTree -check_PROGRAMS += tests/testPermutation - -#---------------------------------------------------------------------------------------------------- -# Create a libtool library that is not installed -# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am -# The headers are installed in $(includedir)/gtsam: -#---------------------------------------------------------------------------------------------------- - -# CCOLAMD include flags are needed due to the bad include paths within the library -# but will not be exposed to users. -ccolamd_inc = -I$(top_srcdir)/gtsam/3rdparty/CCOLAMD/Include -I$(top_srcdir)/gtsam/3rdparty/UFconfig - -headers += $(sources:.cpp=.h) -inferencedir = $(pkgincludedir)/inference -inference_HEADERS = $(headers) -noinst_LTLIBRARIES = libinference.la -libinference_la_SOURCES = $(sources) -AM_CPPFLAGS = $(ccolamd_inc) $(BOOST_CPPFLAGS) -I$(top_srcdir) -AM_LDFLAGS = $(BOOST_LDFLAGS) -AM_CXXFLAGS = - -#---------------------------------------------------------------------------------------------------- -# rules to build local programs -#---------------------------------------------------------------------------------------------------- -TESTS = $(check_PROGRAMS) -AM_LDFLAGS += $(boost_serialization) -LDADD = libinference.la ../base/libbase.la ../3rdparty/libccolamd.la -LDADD += ../../CppUnitLite/libCppUnitLite.a -AM_DEFAULT_SOURCE_EXT = .cpp - -# rule to run an executable -%.run: % $(LDADD) - ./$^ - -# rule to run executable with valgrind -%.valgrind: % $(LDADD) - valgrind ./$^ - diff --git a/gtsam/inference/graph-inl.h b/gtsam/inference/graph-inl.h index 5692d9a92..d8395942a 100644 --- a/gtsam/inference/graph-inl.h +++ b/gtsam/inference/graph-inl.h @@ -139,43 +139,43 @@ predecessorMap2Graph(const PredecessorMap& p_map) { } /* ************************************************************************* */ -template +template class compose_key_visitor : public boost::default_bfs_visitor { private: - boost::shared_ptr config_; + boost::shared_ptr config_; public: - compose_key_visitor(boost::shared_ptr config_in) {config_ = config_in;} + compose_key_visitor(boost::shared_ptr config_in) {config_ = config_in;} template void tree_edge(Edge edge, const Graph& g) const { - typename VALUES::Key key_from = boost::get(boost::vertex_name, g, boost::source(edge, g)); - typename VALUES::Key key_to = boost::get(boost::vertex_name, g, boost::target(edge, g)); + KEY key_from = boost::get(boost::vertex_name, g, boost::source(edge, g)); + KEY key_to = boost::get(boost::vertex_name, g, boost::target(edge, g)); POSE relativePose = boost::get(boost::edge_weight, g, edge); - config_->insert(key_to, (*config_)[key_from].compose(relativePose)); + config_->insert(key_to, config_->at(key_from).compose(relativePose)); } }; /* ************************************************************************* */ -template -boost::shared_ptr composePoses(const G& graph, const PredecessorMap& tree, +template +boost::shared_ptr composePoses(const G& graph, const PredecessorMap& tree, const POSE& rootPose) { //TODO: change edge_weight_t to edge_pose_t typedef typename boost::adjacency_list< boost::vecS, boost::vecS, boost::directedS, - boost::property, + boost::property, boost::property > PoseGraph; typedef typename boost::graph_traits::vertex_descriptor PoseVertex; typedef typename boost::graph_traits::edge_descriptor PoseEdge; PoseGraph g; PoseVertex root; - map key2vertex; + map key2vertex; boost::tie(g, root, key2vertex) = - predecessorMap2Graph(tree); + predecessorMap2Graph(tree); // attach the relative poses to the edges PoseEdge edge12, edge21; @@ -189,8 +189,8 @@ boost::shared_ptr composePoses(const G& graph, const PredecessorMap factor = boost::dynamic_pointer_cast(nl_factor); if (!factor) continue; - typename VALUES::Key key1 = factor->key1(); - typename VALUES::Key key2 = factor->key2(); + KEY key1 = factor->key1(); + KEY key2 = factor->key2(); PoseVertex v1 = key2vertex.find(key1)->second; PoseVertex v2 = key2vertex.find(key2)->second; @@ -207,10 +207,10 @@ boost::shared_ptr composePoses(const G& graph, const PredecessorMap config(new VALUES); - typename VALUES::Key rootKey = boost::get(boost::vertex_name, g, root); + boost::shared_ptr config(new Values); + KEY rootKey = boost::get(boost::vertex_name, g, root); config->insert(rootKey, rootPose); - compose_key_visitor vis(config); + compose_key_visitor vis(config); boost::breadth_first_search(g, root, boost::visitor(vis)); return config; diff --git a/gtsam/inference/graph.h b/gtsam/inference/graph.h index 7a34159ca..98d41940b 100644 --- a/gtsam/inference/graph.h +++ b/gtsam/inference/graph.h @@ -25,6 +25,7 @@ #include #include #include +#include namespace gtsam { @@ -87,9 +88,9 @@ namespace gtsam { /** * Compose the poses by following the chain specified by the spanning tree */ - template - boost::shared_ptr - composePoses(const G& graph, const PredecessorMap& tree, const POSE& rootPose); + template + boost::shared_ptr + composePoses(const G& graph, const PredecessorMap& tree, const POSE& rootPose); /** diff --git a/gtsam/inference/tests/testFactorGraph.cpp b/gtsam/inference/tests/testFactorGraph.cpp index 832d03cea..8669329bb 100644 --- a/gtsam/inference/tests/testFactorGraph.cpp +++ b/gtsam/inference/tests/testFactorGraph.cpp @@ -25,9 +25,6 @@ using namespace boost::assign; #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include using namespace std; diff --git a/gtsam/inference/tests/testISAM.cpp b/gtsam/inference/tests/testISAM.cpp index 55922589c..acb849b8b 100644 --- a/gtsam/inference/tests/testISAM.cpp +++ b/gtsam/inference/tests/testISAM.cpp @@ -21,9 +21,6 @@ using namespace boost::assign; #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include #include #include diff --git a/gtsam/inference/tests/testJunctionTree.cpp b/gtsam/inference/tests/testJunctionTree.cpp index 13f047e88..16309abda 100644 --- a/gtsam/inference/tests/testJunctionTree.cpp +++ b/gtsam/inference/tests/testJunctionTree.cpp @@ -24,10 +24,6 @@ using namespace boost::assign; #include #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - -#include #include #include #include diff --git a/gtsam/inference/tests/testSerializationInference.cpp b/gtsam/inference/tests/testSerializationInference.cpp new file mode 100644 index 000000000..d4c8ea62a --- /dev/null +++ b/gtsam/inference/tests/testSerializationInference.cpp @@ -0,0 +1,85 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSerializationInference.cpp + * @brief + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + +/* ************************************************************************* */ +TEST (Serialization, symbolic_graph) { + // construct expected symbolic graph + SymbolicFactorGraph sfg; + sfg.push_factor(0); + sfg.push_factor(0,1); + sfg.push_factor(0,2); + sfg.push_factor(2,1); + + EXPECT(equalsObj(sfg)); + EXPECT(equalsXML(sfg)); +} + +/* ************************************************************************* */ +TEST (Serialization, symbolic_bn) { + IndexConditional::shared_ptr x2(new IndexConditional(1, 2, 0)); + IndexConditional::shared_ptr l1(new IndexConditional(2, 0)); + IndexConditional::shared_ptr x1(new IndexConditional(0)); + + SymbolicBayesNet sbn; + sbn.push_back(x2); + sbn.push_back(l1); + sbn.push_back(x1); + + EXPECT(equalsObj(sbn)); + EXPECT(equalsXML(sbn)); +} + +/* ************************************************************************* */ +TEST (Serialization, symbolic_bayes_tree ) { + typedef BayesTree SymbolicBayesTree; + static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5; + IndexConditional::shared_ptr + B(new IndexConditional(_B_)), + L(new IndexConditional(_L_, _B_)), + E(new IndexConditional(_E_, _L_, _B_)), + S(new IndexConditional(_S_, _L_, _B_)), + T(new IndexConditional(_T_, _E_, _L_)), + X(new IndexConditional(_X_, _E_)); + + // Bayes Tree for Asia example + SymbolicBayesTree bayesTree; + SymbolicBayesTree::insert(bayesTree, B); + SymbolicBayesTree::insert(bayesTree, L); + SymbolicBayesTree::insert(bayesTree, E); + SymbolicBayesTree::insert(bayesTree, S); + SymbolicBayesTree::insert(bayesTree, T); + SymbolicBayesTree::insert(bayesTree, X); + + EXPECT(equalsObj(bayesTree)); + EXPECT(equalsXML(bayesTree)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/inference/tests/timeSymbolMaps.cpp b/gtsam/inference/tests/timeSymbolMaps.cpp deleted file mode 100644 index 1bf83ca5d..000000000 --- a/gtsam/inference/tests/timeSymbolMaps.cpp +++ /dev/null @@ -1,190 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file timeSymbolMaps.cpp - * @date Jan 20, 2010 - * @author richard - */ - -#include -#include -#include -#include -#include -#include - -#include - -using namespace std; -using namespace boost; -using namespace gtsam; - -template -class SymbolMapExp { -private: - typedef map > Map; - typedef vector Vec; - - Map values_; - -public: - typedef pair value_type; - - SymbolMapExp() {} - - T& at(const Symbol& key) { - typename Map::iterator it = values_.find(key.chr()); - if(it != values_.end()) - return it->second.at(key.index()); - else - throw invalid_argument("Key " + (string)key + " not present"); - } - - void set(const Symbol& key, const T& value) { - Vec& vec(values_[key.chr()]); - //vec.reserve(10000); - if(key.index() >= vec.size()) { - vec.reserve(key.index()+1); - vec.resize(key.index()); - vec.push_back(value); - } else - vec[key.index()] = value; - } -}; - -template -class SymbolMapBinary : public std::map { -private: - typedef std::map Base; -public: - SymbolMapBinary() : std::map() {} - - T& at(const Symbol& key) { - typename Base::iterator it = Base::find(key); - if (it == Base::end()) - throw(std::invalid_argument("SymbolMap::[] invalid key: " + (std::string)key)); - return it->second; - } -}; - -struct SymbolHash : public std::unary_function { - std::size_t operator()(Symbol const& x) const { - std::size_t seed = 0; - boost::hash_combine(seed, x.chr()); - boost::hash_combine(seed, x.index()); - return ((size_t(x.chr()) << 24) & x.index()); - } -}; - -template -class SymbolMapHash : public boost::unordered_map { -public: - SymbolMapHash() : boost::unordered_map(60000) {} -}; - -struct Value { - double v; - Value() : v(0.0) {} - Value(double vi) : v(vi) {} - operator string() { return lexical_cast(v); } - bool operator!=(const Value& vc) { return v != vc.v; } -}; - -#define ELEMS 3000 -#define TIMEAT 300 - -int main(int argc, char *argv[]) { - timer tmr; - - // pre-allocate - cout << "Generating test data ..." << endl; - vector > values; - for(size_t i=0; i binary; - for(size_t i=0; i hash; - for(size_t i=0; i experimental; - for(size_t i=0; ipermuteWithInverse(inversePermutation); - } + const Permutation& inversePermutation) { + BOOST_FOREACH(const sharedFactor& factor, factors_) { + factor->permuteWithInverse(inversePermutation); + } } /* ************************************************************************* */ diff --git a/gtsam/linear/Makefile.am b/gtsam/linear/Makefile.am deleted file mode 100644 index 811ae7458..000000000 --- a/gtsam/linear/Makefile.am +++ /dev/null @@ -1,77 +0,0 @@ -#---------------------------------------------------------------------------------------------------- -# GTSAM linear: inference in Gaussian factor graphs -#---------------------------------------------------------------------------------------------------- - -# use nostdinc to turn off -I. and -I.., we do not need them because -# header files are qualified so they can be included in external projects. -AUTOMAKE_OPTIONS = nostdinc - -headers = -sources = -check_PROGRAMS = - -# Noise Model -headers += SharedGaussian.h SharedDiagonal.h SharedNoiseModel.h -sources += NoiseModel.cpp Errors.cpp Sampler.cpp -check_PROGRAMS += tests/testNoiseModel tests/testErrors tests/testSampler - -# Vector Configurations -sources += VectorValues.cpp -check_PROGRAMS += tests/testVectorValues - -# Solvers -sources += GaussianSequentialSolver.cpp GaussianMultifrontalSolver.cpp - -# Gaussian Factor Graphs -sources += JacobianFactor.cpp HessianFactor.cpp -sources += GaussianFactor.cpp GaussianFactorGraph.cpp -sources += GaussianJunctionTree.cpp -sources += GaussianConditional.cpp GaussianDensity.cpp GaussianBayesNet.cpp -sources += GaussianISAM.cpp -check_PROGRAMS += tests/testHessianFactor tests/testJacobianFactor tests/testGaussianConditional -check_PROGRAMS += tests/testGaussianDensity tests/testGaussianFactorGraph tests/testGaussianJunctionTree - -# Kalman Filter -sources += KalmanFilter.cpp -check_PROGRAMS += tests/testKalmanFilter - -# Iterative Methods -headers += iterative-inl.h -sources += iterative.cpp SubgraphPreconditioner.cpp SubgraphSolver.cpp -headers += IterativeSolver.h IterativeOptimizationParameters.h -headers += SubgraphSolver-inl.h - -# Timing tests -noinst_PROGRAMS = tests/timeGaussianFactor tests/timeFactorOverhead tests/timeSLAMlike - -#---------------------------------------------------------------------------------------------------- -# Create a libtool library that is not installed -# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am -# The headers are installed in $(includedir)/gtsam: -#---------------------------------------------------------------------------------------------------- -headers += $(sources:.cpp=.h) -lineardir = $(pkgincludedir)/linear -linear_HEADERS = $(headers) -noinst_LTLIBRARIES = liblinear.la -liblinear_la_SOURCES = $(sources) -AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) -AM_LDFLAGS = $(BOOST_LDFLAGS) -AM_CXXFLAGS = - -#---------------------------------------------------------------------------------------------------- -# rules to build local programs -#---------------------------------------------------------------------------------------------------- -TESTS = $(check_PROGRAMS) -AM_LDFLAGS += $(boost_serialization) -LDADD = liblinear.la ../inference/libinference.la ../base/libbase.la ../3rdparty/libccolamd.la -LDADD += ../../CppUnitLite/libCppUnitLite.a -AM_DEFAULT_SOURCE_EXT = .cpp - -# rule to run an executable -%.run: % $(LDADD) - ./$^ - -# rule to run executable with valgrind -%.valgrind: % $(LDADD) - valgrind ./$^ - diff --git a/gtsam/linear/SharedNoiseModel.h b/gtsam/linear/SharedNoiseModel.h index 71f2e4014..1d66e3495 100644 --- a/gtsam/linear/SharedNoiseModel.h +++ b/gtsam/linear/SharedNoiseModel.h @@ -50,31 +50,31 @@ namespace gtsam { // note, deliberately not in noiseModel namespace // Static syntactic sugar functions to create noisemodels directly // These should only be used with the Matlab interface - static inline SharedNoiseModel sharedSigmas(const Vector& sigmas, bool smart=false) { + static inline SharedNoiseModel Sigmas(const Vector& sigmas, bool smart=false) { return noiseModel::Diagonal::Sigmas(sigmas, smart); } - static inline SharedNoiseModel sharedSigma(size_t dim, double sigma) { + static inline SharedNoiseModel Sigma(size_t dim, double sigma) { return noiseModel::Isotropic::Sigma(dim, sigma); } - static inline SharedNoiseModel sharedPrecisions(const Vector& precisions) { + static inline SharedNoiseModel Precisions(const Vector& precisions) { return noiseModel::Diagonal::Precisions(precisions); } - static inline SharedNoiseModel sharedPrecision(size_t dim, double precision) { + static inline SharedNoiseModel Precision(size_t dim, double precision) { return noiseModel::Isotropic::Precision(dim, precision); } - static inline SharedNoiseModel sharedUnit(size_t dim) { + static inline SharedNoiseModel Unit(size_t dim) { return noiseModel::Unit::Create(dim); } - static inline SharedNoiseModel sharedSqrtInformation(const Matrix& R) { + static inline SharedNoiseModel SqrtInformation(const Matrix& R) { return noiseModel::Gaussian::SqrtInformation(R); } - static inline SharedNoiseModel sharedCovariance(const Matrix& covariance, bool smart=false) { + static inline SharedNoiseModel Covariance(const Matrix& covariance, bool smart=false) { return noiseModel::Gaussian::Covariance(covariance, smart); } diff --git a/gtsam/linear/SubgraphSolver-inl.h b/gtsam/linear/SubgraphSolver-inl.h index 4aa12532e..d2dc7cee2 100644 --- a/gtsam/linear/SubgraphSolver-inl.h +++ b/gtsam/linear/SubgraphSolver-inl.h @@ -21,8 +21,8 @@ using namespace std; namespace gtsam { -template -void SubgraphSolver::replaceFactors(const typename LINEAR::shared_ptr &graph) { +template +void SubgraphSolver::replaceFactors(const typename LINEAR::shared_ptr &graph) { GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared(); GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared(); @@ -47,8 +47,8 @@ void SubgraphSolver::replaceFactors(const typename LINEAR:: Ab1->dynamicCastFactors >(), Ab2->dynamicCastFactors >(),Rc1,xbar); } -template -VectorValues::shared_ptr SubgraphSolver::optimize() { +template +VectorValues::shared_ptr SubgraphSolver::optimize() const { // preconditioned conjugate gradient VectorValues zeros = pc_->zero(); @@ -60,21 +60,21 @@ VectorValues::shared_ptr SubgraphSolver::optimize() { return xbar; } -template -void SubgraphSolver::initialize(const GRAPH& G, const VALUES& theta0) { +template +void SubgraphSolver::initialize(const GRAPH& G, const Values& theta0) { // generate spanning tree - PredecessorMap tree_ = gtsam::findMinimumSpanningTree(G); + PredecessorMap tree_ = gtsam::findMinimumSpanningTree(G); // make the ordering - list keys = predecessorMap2Keys(tree_); - ordering_ = boost::make_shared(list(keys.begin(), keys.end())); + list keys = predecessorMap2Keys(tree_); + ordering_ = boost::make_shared(list(keys.begin(), keys.end())); // build factor pairs pairs_.clear(); - typedef pair EG ; + typedef pair EG ; BOOST_FOREACH( const EG &eg, tree_ ) { - Symbol key1 = Symbol(eg.first), - key2 = Symbol(eg.second) ; + Key key1 = Key(eg.first), + key2 = Key(eg.second) ; pairs_.insert(pair((*ordering_)[key1], (*ordering_)[key2])) ; } } diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 33351ceb7..0be2cad19 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -175,7 +175,7 @@ namespace gtsam { /// @name Advanced Constructors /// @{ - /** Construct from a container of variable dimensions (in variable order). */ + /** Construct from a container of variable dimensions (in variable order), without initializing any values. */ template VectorValues(const CONTAINER& dimensions) { append(dimensions); } diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp new file mode 100644 index 000000000..8082d3626 --- /dev/null +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -0,0 +1,161 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSerializationLinear.cpp + * @brief + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + +/* ************************************************************************* */ +// Noise model components +/* ************************************************************************* */ + +/* ************************************************************************* */ +// Export Noisemodels +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); + +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +/* ************************************************************************* */ +// example noise models +noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); +noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3)); +noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2); +noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1)); +noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3); + +/* ************************************************************************* */ +TEST (Serialization, noiseModels) { + // tests using pointers to the derived class + EXPECT( equalsDereferenced(diag3)); + EXPECT(equalsDereferencedXML(diag3)); + + EXPECT( equalsDereferenced(gaussian3)); + EXPECT(equalsDereferencedXML(gaussian3)); + + EXPECT( equalsDereferenced(iso3)); + EXPECT(equalsDereferencedXML(iso3)); + + EXPECT( equalsDereferenced(constrained3)); + EXPECT(equalsDereferencedXML(constrained3)); + + EXPECT( equalsDereferenced(unit3)); + EXPECT(equalsDereferencedXML(unit3)); +} + +/* ************************************************************************* */ +TEST (Serialization, SharedNoiseModel_noiseModels) { + SharedNoiseModel diag3_sg = diag3; + EXPECT(equalsDereferenced(diag3_sg)); + EXPECT(equalsDereferencedXML(diag3_sg)); + + EXPECT(equalsDereferenced(diag3)); + EXPECT(equalsDereferencedXML(diag3)); + + EXPECT(equalsDereferenced(iso3)); + EXPECT(equalsDereferencedXML(iso3)); + + EXPECT(equalsDereferenced(gaussian3)); + EXPECT(equalsDereferencedXML(gaussian3)); + + EXPECT(equalsDereferenced(unit3)); + EXPECT(equalsDereferencedXML(unit3)); + + EXPECT(equalsDereferenced(constrained3)); + EXPECT(equalsDereferencedXML(constrained3)); +} + +/* ************************************************************************* */ +TEST (Serialization, SharedDiagonal_noiseModels) { + EXPECT(equalsDereferenced(diag3)); + EXPECT(equalsDereferencedXML(diag3)); + + EXPECT(equalsDereferenced(iso3)); + EXPECT(equalsDereferencedXML(iso3)); + + EXPECT(equalsDereferenced(unit3)); + EXPECT(equalsDereferencedXML(unit3)); + + EXPECT(equalsDereferenced(constrained3)); + EXPECT(equalsDereferencedXML(constrained3)); +} + +/* ************************************************************************* */ +// Linear components +/* ************************************************************************* */ + +/* Create GUIDs for factors */ +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); + +/* ************************************************************************* */ +TEST (Serialization, linear_factors) { + VectorValues values; + values.insert(0, Vector_(1, 1.0)); + values.insert(1, Vector_(2, 2.0,3.0)); + values.insert(2, Vector_(2, 4.0,5.0)); + EXPECT(equalsObj(values)); + EXPECT(equalsXML(values)); + + Index i1 = 4, i2 = 7; + Matrix A1 = eye(3), A2 = -1.0 * eye(3); + Vector b = ones(3); + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1.0, 2.0, 3.0)); + JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); + EXPECT(equalsObj(jacobianfactor)); + EXPECT(equalsXML(jacobianfactor)); + + HessianFactor hessianfactor(jacobianfactor); + EXPECT(equalsObj(hessianfactor)); + EXPECT(equalsXML(hessianfactor)); +} + +/* ************************************************************************* */ +TEST (Serialization, gaussian_conditional) { + Matrix A1 = Matrix_(2,2, 1., 2., 3., 4.); + Matrix A2 = Matrix_(2,2, 6., 0.2, 8., 0.4); + Matrix R = Matrix_(2,2, 0.1, 0.3, 0.0, 0.34); + Vector d(2); d << 0.2, 0.5; + GaussianConditional cg(0, d, R, 1, A1, 2, A2, ones(2)); + + EXPECT(equalsObj(cg)); + EXPECT(equalsXML(cg)); +} + + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/linear/tests/timeGaussianFactor.cpp b/gtsam/linear/tests/timeGaussianFactor.cpp index d39be1061..3895ab520 100644 --- a/gtsam/linear/tests/timeGaussianFactor.cpp +++ b/gtsam/linear/tests/timeGaussianFactor.cpp @@ -15,9 +15,6 @@ * @author Alireza Fathi */ -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include /*STL/C++*/ diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index c4b266192..af1e345b6 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -7,7 +7,7 @@ #include #include // To get optimize(BayesTree) -#include +//#include #include namespace gtsam { diff --git a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h index 823853f6d..06a24d473 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter-inl.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter-inl.h @@ -27,10 +27,10 @@ namespace gtsam { /* ************************************************************************* */ - template - typename ExtendedKalmanFilter::T ExtendedKalmanFilter::solve_( + template + typename ExtendedKalmanFilter::T ExtendedKalmanFilter::solve_( const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering, - const VALUES& linearizationPoints, const KEY& lastKey, + const Values& linearizationPoints, Key lastKey, JacobianFactor::shared_ptr& newPrior) const { // Extract the Index of the provided last key @@ -43,7 +43,7 @@ namespace gtsam { // Extract the current estimate of x1,P1 from the Bayes Network VectorValues result = optimize(*linearBayesNet); - T x = linearizationPoints[lastKey].retract(result[lastIndex]); + T x = linearizationPoints.at(lastKey).retract(result[lastIndex]); // Create a Jacobian Factor from the root node of the produced Bayes Net. // This will act as a prior for the next iteration. @@ -66,8 +66,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - ExtendedKalmanFilter::ExtendedKalmanFilter(T x_initial, + template + ExtendedKalmanFilter::ExtendedKalmanFilter(T x_initial, noiseModel::Gaussian::shared_ptr P_initial) { // Set the initial linearization point to the provided mean @@ -82,8 +82,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - typename ExtendedKalmanFilter::T ExtendedKalmanFilter::predict( + template + typename ExtendedKalmanFilter::T ExtendedKalmanFilter::predict( const MotionFactor& motionFactor) { // TODO: This implementation largely ignores the actual factor symbols. @@ -91,8 +91,8 @@ namespace gtsam { // different keys will still compute as if a common key-set was used // Create Keys - KEY x0 = motionFactor.key1(); - KEY x1 = motionFactor.key2(); + Key x0 = motionFactor.key1(); + Key x1 = motionFactor.key2(); // Create an elimination ordering Ordering ordering; @@ -100,7 +100,7 @@ namespace gtsam { ordering.insert(x1, 1); // Create a set of linearization points - VALUES linearizationPoints; + Values linearizationPoints; linearizationPoints.insert(x0, x_); linearizationPoints.insert(x1, x_); // TODO should this really be x_ ? @@ -122,8 +122,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - typename ExtendedKalmanFilter::T ExtendedKalmanFilter::update( + template + typename ExtendedKalmanFilter::T ExtendedKalmanFilter::update( const MeasurementFactor& measurementFactor) { // TODO: This implementation largely ignores the actual factor symbols. @@ -131,14 +131,14 @@ namespace gtsam { // different keys will still compute as if a common key-set was used // Create Keys - KEY x0 = measurementFactor.key(); + Key x0 = measurementFactor.key(); // Create an elimination ordering Ordering ordering; ordering.insert(x0, 0); // Create a set of linearization points - VALUES linearizationPoints; + Values linearizationPoints; linearizationPoints.insert(x0, x_); // Create a Gaussian Factor Graph diff --git a/gtsam/nonlinear/ExtendedKalmanFilter.h b/gtsam/nonlinear/ExtendedKalmanFilter.h index 8f88979a1..13a612f84 100644 --- a/gtsam/nonlinear/ExtendedKalmanFilter.h +++ b/gtsam/nonlinear/ExtendedKalmanFilter.h @@ -40,22 +40,22 @@ namespace gtsam { * \nosubgrouping */ - template + template class ExtendedKalmanFilter { public: - typedef boost::shared_ptr > shared_ptr; - typedef typename KEY::Value T; - typedef NonlinearFactor2 MotionFactor; - typedef NonlinearFactor1 MeasurementFactor; + typedef boost::shared_ptr > shared_ptr; + typedef VALUE T; + typedef NoiseModelFactor2 MotionFactor; + typedef NoiseModelFactor1 MeasurementFactor; protected: T x_; // linearization point JacobianFactor::shared_ptr priorFactor_; // density T solve_(const GaussianFactorGraph& linearFactorGraph, - const Ordering& ordering, const VALUES& linearizationPoints, - const KEY& x, JacobianFactor::shared_ptr& newPrior) const; + const Ordering& ordering, const Values& linearizationPoints, + Key x, JacobianFactor::shared_ptr& newPrior) const; public: diff --git a/gtsam/nonlinear/GaussianISAM2-inl.h b/gtsam/nonlinear/GaussianISAM2-inl.h index d0d37d886..de3992d28 100644 --- a/gtsam/nonlinear/GaussianISAM2-inl.h +++ b/gtsam/nonlinear/GaussianISAM2-inl.h @@ -121,7 +121,7 @@ namespace gtsam { ///* ************************************************************************* */ //boost::shared_ptr optimize2(const GaussianISAM2::sharedClique& root) { // boost::shared_ptr delta(new VectorValues()); - // set changed; + // set changed; // // starting from the root, call optimize on each conditional // optimize2(root, delta); // return delta; diff --git a/gtsam/nonlinear/GaussianISAM2.h b/gtsam/nonlinear/GaussianISAM2.h index 26049ec69..d1a4bef9f 100644 --- a/gtsam/nonlinear/GaussianISAM2.h +++ b/gtsam/nonlinear/GaussianISAM2.h @@ -36,19 +36,19 @@ namespace gtsam { * variables. * @tparam GRAPH The NonlinearFactorGraph structure to store factors. Defaults to standard NonlinearFactorGraph */ -template > -class GaussianISAM2 : public ISAM2 { - typedef ISAM2 Base; +template +class GaussianISAM2 : public ISAM2 { + typedef ISAM2 Base; public: /// @name Standard Constructors /// @{ /** Create an empty ISAM2 instance */ - GaussianISAM2(const ISAM2Params& params) : ISAM2(params) {} + GaussianISAM2(const ISAM2Params& params) : ISAM2(params) {} /** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */ - GaussianISAM2() : ISAM2() {} + GaussianISAM2() : ISAM2() {} /// @} /// @name Advanced Interface diff --git a/gtsam/nonlinear/ISAM2-impl-inl.h b/gtsam/nonlinear/ISAM2-impl-inl.h index fc48720f9..c2483488c 100644 --- a/gtsam/nonlinear/ISAM2-impl-inl.h +++ b/gtsam/nonlinear/ISAM2-impl-inl.h @@ -19,10 +19,10 @@ namespace gtsam { using namespace std; -template -struct ISAM2::Impl { +template +struct ISAM2::Impl { - typedef ISAM2 ISAM2Type; + typedef ISAM2 ISAM2Type; struct PartialSolveResult { typename ISAM2Type::sharedClique bayesTree; @@ -44,8 +44,10 @@ struct ISAM2::Impl { * @param delta Current linear delta to be augmented with zeros * @param ordering Current ordering to be augmented with new variables * @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables + * @param keyFormatter Formatter for printing nonlinear keys during debugging */ - static void AddVariables(const VALUES& newTheta, VALUES& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes); + static void AddVariables(const Values& newTheta, Values& theta, Permuted& delta, Ordering& ordering, + typename Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** * Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol @@ -61,10 +63,12 @@ struct ISAM2::Impl { * Any variables in the VectorValues delta whose vector magnitude is greater than * or equal to relinearizeThreshold are returned. * @param delta The linear delta to check against the threshold + * @param keyFormatter Formatter for printing nonlinear keys during debugging * @return The set of variable indices in delta whose magnitude is greater than or * equal to relinearizeThreshold */ - static FastSet CheckRelinearization(const Permuted& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold); + static FastSet CheckRelinearization(const Permuted& delta, const Ordering& ordering, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** * Recursively search this clique and its children for marked keys appearing @@ -94,10 +98,12 @@ struct ISAM2::Impl { * expmapped deltas will be set to an invalid value (infinity) to catch bugs * where we might expmap something twice, or expmap it but then not * recalculate its delta. + * @param keyFormatter Formatter for printing nonlinear keys during debugging */ - static void ExpmapMasked(VALUES& values, const Permuted& delta, + static void ExpmapMasked(Values& values, const Permuted& delta, const Ordering& ordering, const std::vector& mask, - boost::optional&> invalidateIfDebug = boost::optional&>()); + boost::optional&> invalidateIfDebug = boost::optional&>(), + const KeyFormatter& keyFormatter = DefaultKeyFormatter); /** * Reorder and eliminate factors. These factors form a subset of the full @@ -118,25 +124,9 @@ struct ISAM2::Impl { }; /* ************************************************************************* */ -struct _VariableAdder { - Ordering& ordering; - Permuted& vconfig; - Index nextVar; - _VariableAdder(Ordering& _ordering, Permuted& _vconfig, Index _nextVar) : ordering(_ordering), vconfig(_vconfig), nextVar(_nextVar){} - template - void operator()(I xIt) { - const bool debug = ISDEBUG("ISAM2 AddVariables"); - vconfig.permutation()[nextVar] = nextVar; - ordering.insert(xIt->first, nextVar); - if(debug) cout << "Adding variable " << (string)xIt->first << " with order " << nextVar << endl; - ++ nextVar; - } -}; - -/* ************************************************************************* */ -template -void ISAM2::Impl::AddVariables( - const VALUES& newTheta, VALUES& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes) { +template +void ISAM2::Impl::AddVariables( + const Values& newTheta, Values& theta, Permuted& delta, Ordering& ordering, typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) { const bool debug = ISDEBUG("ISAM2 AddVariables"); theta.insert(newTheta); @@ -151,8 +141,13 @@ void ISAM2::Impl::AddVariables( delta.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim)); delta.permutation().resize(originalnVars + newTheta.size()); { - _VariableAdder vadder(ordering, delta, originalnVars); - newTheta.apply(vadder); + Index nextVar = originalnVars; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) { + delta.permutation()[nextVar] = nextVar; + ordering.insert(key_value.first, nextVar); + if(debug) cout << "Adding variable " << keyFormatter(key_value.first) << " with order " << nextVar << endl; + ++ nextVar; + } assert(delta.permutation().size() == delta.container().size()); assert(ordering.nVars() == delta.size()); assert(ordering.size() == delta.size()); @@ -162,11 +157,11 @@ void ISAM2::Impl::AddVariables( } /* ************************************************************************* */ -template -FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) { +template +FastSet ISAM2::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) { FastSet indices; - BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) { - BOOST_FOREACH(const Symbol& key, factor->keys()) { + BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) { + BOOST_FOREACH(Key key, factor->keys()) { indices.insert(ordering[key]); } } @@ -174,8 +169,9 @@ FastSet ISAM2::Impl::IndicesFromFactors(const O } /* ************************************************************************* */ -template -FastSet ISAM2::Impl::CheckRelinearization(const Permuted& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) { +template +FastSet ISAM2::Impl::CheckRelinearization(const Permuted& delta, const Ordering& ordering, + const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) { FastSet relinKeys; if(relinearizeThreshold.type() == typeid(double)) { @@ -189,7 +185,7 @@ FastSet ISAM2::Impl::CheckRelinearization(const } else if(relinearizeThreshold.type() == typeid(FastMap)) { const FastMap& thresholds = boost::get >(relinearizeThreshold); BOOST_FOREACH(const Ordering::value_type& key_index, ordering) { - const Vector& threshold = thresholds.find(key_index.first.chr())->second; + const Vector& threshold = thresholds.find(Symbol(key_index.first).chr())->second; Index j = key_index.second; if(threshold.rows() != delta[j].rows()) throw std::invalid_argument("Relinearization threshold vector dimensionality passed into iSAM2 parameters does not match actual variable dimensionality"); @@ -202,8 +198,8 @@ FastSet ISAM2::Impl::CheckRelinearization(const } /* ************************************************************************* */ -template -void ISAM2::Impl::FindAll(typename ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask) { +template +void ISAM2::Impl::FindAll(typename ISAM2Clique::shared_ptr clique, FastSet& keys, const vector& markedMask) { static const bool debug = false; // does the separator contain any of the variables? bool found = false; @@ -223,42 +219,9 @@ void ISAM2::Impl::FindAll(typename ISAM2Clique& delta; - const Ordering& ordering; - const vector& mask; - const boost::optional&> invalidate; - _SelectiveExpmapAndClear(const Permuted& _delta, const Ordering& _ordering, const vector& _mask, boost::optional&> _invalidate) : - delta(_delta), ordering(_ordering), mask(_mask), invalidate(_invalidate) {} - template - void operator()(I it_x) { - Index var = ordering[it_x->first]; - if(ISDEBUG("ISAM2 update verbose")) { - if(mask[var]) - cout << "expmap " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl; - else - cout << " " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl; - } - assert(delta[var].size() == (int)it_x->second.dim()); - assert(delta[var].unaryExpr(&isfinite).all()); - if(mask[var]) { - it_x->second = it_x->second.retract(delta[var]); - if(invalidate) - (*invalidate)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits::infinity())); // Strange syntax to work with clang++ (bug in clang?) - } - } -}; - -/* ************************************************************************* */ -template -void ISAM2::Impl::ExpmapMasked(VALUES& values, const Permuted& delta, - const Ordering& ordering, const vector& mask, boost::optional&> invalidateIfDebug) { +template +void ISAM2::Impl::ExpmapMasked(Values& values, const Permuted& delta, const Ordering& ordering, + const vector& mask, boost::optional&> invalidateIfDebug, const KeyFormatter& keyFormatter) { // If debugging, invalidate if requested, otherwise do not invalidate. // Invalidating means setting expmapped entries to Inf, to trigger assertions // if we try to re-use them. @@ -266,14 +229,35 @@ void ISAM2::Impl::ExpmapMasked(VALUES& values, const invalidateIfDebug = boost::optional&>(); #endif - _SelectiveExpmapAndClear selectiveExpmapper(delta, ordering, mask, invalidateIfDebug); - values.apply(selectiveExpmapper); + assert(values.size() == ordering.size()); + Values::iterator key_value; + Ordering::const_iterator key_index; + for(key_value = values.begin(), key_index = ordering.begin(); + key_value != values.end() && key_index != ordering.end(); ++key_value, ++key_index) { + assert(key_value->first == key_index->first); + const Index var = key_index->second; + if(ISDEBUG("ISAM2 update verbose")) { + if(mask[var]) + cout << "expmap " << keyFormatter(key_value->first) << " (j = " << var << "), delta = " << delta[var].transpose() << endl; + else + cout << " " << keyFormatter(key_value->first) << " (j = " << var << "), delta = " << delta[var].transpose() << endl; + } + assert(delta[var].size() == (int)key_value->second.dim()); + assert(delta[var].unaryExpr(&isfinite).all()); + if(mask[var]) { + Value* retracted = key_value->second.retract_(delta[var]); + key_value->second = *retracted; + retracted->deallocate_(); + if(invalidateIfDebug) + (*invalidateIfDebug)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits::infinity())); // Strange syntax to work with clang++ (bug in clang?) + } + } } /* ************************************************************************* */ -template -typename ISAM2::Impl::PartialSolveResult -ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, +template +typename ISAM2::Impl::PartialSolveResult +ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors, const FastSet& keys, const ReorderingMode& reorderingMode) { static const bool debug = ISDEBUG("ISAM2 recalculate"); diff --git a/gtsam/nonlinear/ISAM2-inl.h b/gtsam/nonlinear/ISAM2-inl.h index d501e30db..6ff22c3a1 100644 --- a/gtsam/nonlinear/ISAM2-inl.h +++ b/gtsam/nonlinear/ISAM2-inl.h @@ -39,8 +39,8 @@ static const bool disableReordering = false; static const double batchThreshold = 0.65; /* ************************************************************************* */ -template -ISAM2::ISAM2(const ISAM2Params& params): +template +ISAM2::ISAM2(const ISAM2Params& params): delta_(Permutation(), deltaUnpermuted_), params_(params) { // See note in gtsam/base/boost_variant_with_workaround.h if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) @@ -48,8 +48,8 @@ ISAM2::ISAM2(const ISAM2Params& params): } /* ************************************************************************* */ -template -ISAM2::ISAM2(): +template +ISAM2::ISAM2(): delta_(Permutation(), deltaUnpermuted_) { // See note in gtsam/base/boost_variant_with_workaround.h if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams)) @@ -57,14 +57,14 @@ ISAM2::ISAM2(): } /* ************************************************************************* */ -template -FastList ISAM2::getAffectedFactors(const FastList& keys) const { +template +FastList ISAM2::getAffectedFactors(const FastList& keys) const { static const bool debug = false; if(debug) cout << "Getting affected factors for "; if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } } if(debug) cout << endl; - FactorGraph > allAffected; + FactorGraph allAffected; FastList indices; BOOST_FOREACH(const Index key, keys) { // const list l = nonlinearFactors_.factors(key); @@ -86,9 +86,9 @@ FastList ISAM2::getAffectedFactors(const Fas /* ************************************************************************* */ // retrieve all factors that ONLY contain the affected variables // (note that the remaining stuff is summarized in the cached factors) -template +template FactorGraph::shared_ptr -ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { +ISAM2::relinearizeAffectedFactors(const FastList& affectedKeys) const { tic(1,"getAffectedFactors"); FastList candidates = getAffectedFactors(affectedKeys); @@ -105,7 +105,7 @@ ISAM2::relinearizeAffectedFactors(const FastListkeys()) { + BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) { Index var = ordering_[key]; if (affectedKeysSet.find(var) == affectedKeysSet.end()) { inside = false; @@ -125,9 +125,9 @@ ISAM2::relinearizeAffectedFactors(const FastList -FactorGraph::CacheFactor> -ISAM2::getCachedBoundaryFactors(Cliques& orphans) { +template +FactorGraph::CacheFactor> +ISAM2::getCachedBoundaryFactors(Cliques& orphans) { static const bool debug = false; @@ -151,8 +151,8 @@ ISAM2::getCachedBoundaryFactors(Cliques& orphans) { return cachedBoundary; } -template -boost::shared_ptr > ISAM2::recalculate( +template +boost::shared_ptr > ISAM2::recalculate( const FastSet& markedKeys, const FastVector& newKeys, const FactorGraph::shared_ptr newFactors, const boost::optional >& constrainKeys, ISAM2Result& result) { @@ -395,10 +395,10 @@ boost::shared_ptr > ISAM2::recalculat } /* ************************************************************************* */ -template -ISAM2Result ISAM2::update( +template +ISAM2Result ISAM2::update( const GRAPH& newFactors, const Values& newTheta, const FastVector& removeFactorIndices, - const boost::optional >& constrainedKeys, bool force_relinearize) { + const boost::optional >& constrainedKeys, bool force_relinearize) { static const bool debug = ISDEBUG("ISAM2 update"); static const bool verbose = ISDEBUG("ISAM2 update verbose"); @@ -519,7 +519,7 @@ ISAM2Result ISAM2::update( boost::optional > constrainedIndices; if(constrainedKeys) { constrainedIndices.reset(FastSet()); - BOOST_FOREACH(const Symbol& key, *constrainedKeys) { + BOOST_FOREACH(Key key, *constrainedKeys) { constrainedIndices->insert(ordering_[key]); } } @@ -581,36 +581,36 @@ ISAM2Result ISAM2::update( } /* ************************************************************************* */ -template -VALUES ISAM2::calculateEstimate() const { +template +Values ISAM2::calculateEstimate() const { // We use ExpmapMasked here instead of regular expmap because the former // handles Permuted - VALUES ret(theta_); + Values ret(theta_); vector mask(ordering_.nVars(), true); Impl::ExpmapMasked(ret, delta_, ordering_, mask); return ret; } /* ************************************************************************* */ -template +template template -typename KEY::Value ISAM2::calculateEstimate(const KEY& key) const { +typename KEY::Value ISAM2::calculateEstimate(const KEY& key) const { const Index index = getOrdering()[key]; const SubVector delta = getDelta()[index]; return getLinearizationPoint()[key].retract(delta); } /* ************************************************************************* */ -template -VALUES ISAM2::calculateBestEstimate() const { +template +Values ISAM2::calculateBestEstimate() const { VectorValues delta(theta_.dims(ordering_)); optimize2(this->root(), delta); return theta_.retract(delta, ordering_); } /* ************************************************************************* */ -template -VectorValues optimize(const ISAM2& isam) { +template +VectorValues optimize(const ISAM2& isam) { VectorValues delta = *allocateVectorValues(isam); optimize2(isam.root(), delta); return delta; diff --git a/gtsam/nonlinear/ISAM2.h b/gtsam/nonlinear/ISAM2.h index a97d9d4ed..4c8df2997 100644 --- a/gtsam/nonlinear/ISAM2.h +++ b/gtsam/nonlinear/ISAM2.h @@ -106,16 +106,19 @@ struct ISAM2Params { bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error before and after the update, to return in ISAM2Result from update() + KeyFormatter keyFormatter; ///< A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter) + /** Specify parameters as constructor arguments */ ISAM2Params( - OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params public variables, ISAM2Params::optimizationParams - RelinearizationThreshold _relinearizeThreshold = 0.1, ///< see ISAM2Params public variables, ISAM2Params::relinearizeThreshold - int _relinearizeSkip = 10, ///< see ISAM2Params public variables, ISAM2Params::relinearizeSkip - bool _enableRelinearization = true, ///< see ISAM2Params public variables, ISAM2Params::enableRelinearization - bool _evaluateNonlinearError = false ///< see ISAM2Params public variables, ISAM2Params::evaluateNonlinearError + OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params::optimizationParams + RelinearizationThreshold _relinearizeThreshold = 0.1, ///< see ISAM2Params::relinearizeThreshold + int _relinearizeSkip = 10, ///< see ISAM2Params::relinearizeSkip + bool _enableRelinearization = true, ///< see ISAM2Params::enableRelinearization + bool _evaluateNonlinearError = false, ///< see ISAM2Params::evaluateNonlinearError + const KeyFormatter& _keyFormatter = DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter ) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold), relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization), - evaluateNonlinearError(_evaluateNonlinearError) {} + evaluateNonlinearError(_evaluateNonlinearError), keyFormatter(_keyFormatter) {} }; /** @@ -266,13 +269,13 @@ private: * estimate of all variables. * */ -template > +template class ISAM2: public BayesTree > { protected: /** The current linearization point */ - VALUES theta_; + Values theta_; /** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */ VariableIndex variableIndex_; @@ -314,8 +317,7 @@ private: public: typedef BayesTree > Base; ///< The BayesTree base class - typedef ISAM2 This; ///< This class - typedef VALUES Values; + typedef ISAM2 This; ///< This class typedef GRAPH Graph; /** Create an empty ISAM2 instance */ @@ -368,19 +370,19 @@ public: * (Params::relinearizeSkip). * @return An ISAM2Result struct containing information about the update */ - ISAM2Result update(const GRAPH& newFactors = GRAPH(), const VALUES& newTheta = VALUES(), + ISAM2Result update(const GRAPH& newFactors = GRAPH(), const Values& newTheta = Values(), const FastVector& removeFactorIndices = FastVector(), - const boost::optional >& constrainedKeys = boost::none, + const boost::optional >& constrainedKeys = boost::none, bool force_relinearize = false); /** Access the current linearization point */ - const VALUES& getLinearizationPoint() const {return theta_;} + const Values& getLinearizationPoint() const {return theta_;} /** Compute an estimate from the incomplete linear delta computed during the last update. * This delta is incomplete because it was not updated below wildfire_threshold. If only * a single variable is needed, it is faster to call calculateEstimate(const KEY&). */ - VALUES calculateEstimate() const; + Values calculateEstimate() const; /** Compute an estimate for a single variable using its incomplete linear delta computed * during the last update. This is faster than calling the no-argument version of @@ -399,7 +401,7 @@ public: /** Compute an estimate using a complete delta computed by a full back-substitution. */ - VALUES calculateBestEstimate() const; + Values calculateBestEstimate() const; /** Access the current delta, computed during the last call to update */ const Permuted& getDelta() const { return delta_; } @@ -435,8 +437,8 @@ private: }; // ISAM2 /** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */ -template -VectorValues optimize(const ISAM2& isam); +template +VectorValues optimize(const ISAM2& isam); } /// namespace gtsam diff --git a/gtsam/nonlinear/Key.cpp b/gtsam/nonlinear/Key.cpp new file mode 100644 index 000000000..821285558 --- /dev/null +++ b/gtsam/nonlinear/Key.cpp @@ -0,0 +1,34 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Key.h + * @brief + * @author Richard Roberts + * @date Feb 20, 2012 + */ + +#include + +#include +#include + +namespace gtsam { + +std::string _defaultKeyFormatter(Key key) { + const Symbol asSymbol(key); + if(asSymbol.chr() > 0) + return (std::string)asSymbol; + else + return boost::lexical_cast(key); + } + +} diff --git a/gtsam/nonlinear/Key.h b/gtsam/nonlinear/Key.h index 1e28f51e6..65a65c6d4 100644 --- a/gtsam/nonlinear/Key.h +++ b/gtsam/nonlinear/Key.h @@ -11,348 +11,30 @@ /** * @file Key.h - * @date Jan 12, 2010 - * @author: Frank Dellaert - * @author: Richard Roberts + * @brief + * @author Richard Roberts + * @date Feb 20, 2012 */ - #pragma once -#include -#include -#include -#include -#include -#ifdef GTSAM_MAGIC_KEY -#include -#endif - -#define ALPHA '\224' +#include +#include namespace gtsam { -/** - * TypedSymbol key class is templated on - * 1) the type T it is supposed to retrieve, for extra type checking - * 2) the character constant used for its string representation - */ -template -class TypedSymbol { + /// Integer nonlinear key type + typedef size_t Key; -protected: - size_t j_; + /// Typedef for a function to format a key, i.e. to convert it to a string + typedef boost::function KeyFormatter; -public: + // Helper function for DefaultKeyFormatter + std::string _defaultKeyFormatter(Key key); - // typedefs - typedef T Value; - typedef boost::mpl::char_ Chr; // to reconstruct the type: use Chr::value + /// The default KeyFormatter, which is used if no KeyFormatter is passed to + /// a nonlinear 'print' function. Automatically detects plain integer keys + /// and Symbol keys. + static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter; - // Constructors: - - TypedSymbol() : - j_(0) { - } - TypedSymbol(size_t j) : - j_(j) { - } - - virtual ~TypedSymbol() {} - - // Get stuff: - ///TODO: comment - size_t index() const { - return j_; - } - static char chr() { - return C; - } - const char* c_str() const { - return ((std::string) (*this)).c_str(); - } - operator std::string() const { - return (boost::format("%c%d") % C % j_).str(); - } - std::string latex() const { - return (boost::format("%c_{%d}") % C % j_).str(); - } - - // logic: - - bool operator<(const TypedSymbol& compare) const { - return j_ < compare.j_; - } - bool operator==(const TypedSymbol& compare) const { - return j_ == compare.j_; - } - bool operator!=(const TypedSymbol& compare) const { - return j_ != compare.j_; - } - int compare(const TypedSymbol& compare) const { - return j_ - compare.j_; - } - - // Testable Requirements - virtual void print(const std::string& s = "") const { - std::cout << s << ": " << (std::string) (*this) << std::endl; - } - bool equals(const TypedSymbol& expected, double tol = 0.0) const { - return (*this) == expected; - } - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(j_); - } -}; - -/** forward declaration to avoid circular dependencies */ -template -class TypedLabeledSymbol; - -/** - * Character and index key used in VectorValues, GaussianFactorGraph, - * GaussianFactor, etc. These keys are generated at runtime from TypedSymbol - * keys when linearizing a nonlinear factor graph. This key is not type - * safe, so cannot be used with any Nonlinear* classes. - */ -class Symbol { -protected: - unsigned char c_; - size_t j_; - -public: - /** Default constructor */ - Symbol() : - c_(0), j_(0) { - } - - /** Copy constructor */ - Symbol(const Symbol& key) : - c_(key.c_), j_(key.j_) { - } - - /** Constructor */ - Symbol(unsigned char c, size_t j) : - c_(c), j_(j) { - } - - /** Casting constructor from TypedSymbol */ - template - Symbol(const TypedSymbol& symbol) : - c_(C), j_(symbol.index()) { - } - - /** Casting constructor from TypedLabeledSymbol */ - template - Symbol(const TypedLabeledSymbol& symbol) : - c_(C), j_(symbol.encode()) { - } - - /** "Magic" key casting constructor from string */ -#ifdef GTSAM_MAGIC_KEY - Symbol(const std::string& str) { - if(str.length() < 1) - throw std::invalid_argument("Cannot parse string key '" + str + "'"); - else { - const char *c_str = str.c_str(); - c_ = c_str[0]; - if(str.length() > 1) - j_ = boost::lexical_cast(c_str+1); - else - j_ = 0; - } - } - - Symbol(const char *c_str) { - std::string str(c_str); - if(str.length() < 1) - throw std::invalid_argument("Cannot parse string key '" + str + "'"); - else { - c_ = c_str[0]; - if(str.length() > 1) - j_ = boost::lexical_cast(c_str+1); - else - j_ = 0; - } - } -#endif - - // Testable Requirements - void print(const std::string& s = "") const { - std::cout << s << ": " << (std::string) (*this) << std::endl; - } - bool equals(const Symbol& expected, double tol = 0.0) const { - return (*this) == expected; - } - - /** Retrieve key character */ - unsigned char chr() const { - return c_; - } - - /** Retrieve key index */ - size_t index() const { - return j_; - } - - /** Create a string from the key */ - operator std::string() const { - return str(boost::format("%c%d") % c_ % j_); - } - - /** Comparison for use in maps */ - bool operator<(const Symbol& comp) const { - return c_ < comp.c_ || (comp.c_ == c_ && j_ < comp.j_); - } - bool operator==(const Symbol& comp) const { - return comp.c_ == c_ && comp.j_ == j_; - } - bool operator!=(const Symbol& comp) const { - return comp.c_ != c_ || comp.j_ != j_; - } - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(c_); - ar & BOOST_SERIALIZATION_NVP(j_); - } -}; - -// Conversion utilities - -template Symbol key2symbol(KEY key) { - return Symbol(key); } -template std::list keys2symbols(std::list keys) { - std::list symbols; - std::transform(keys.begin(), keys.end(), std::back_inserter(symbols), - key2symbol ); - return symbols; -} - -/** - * TypedLabeledSymbol is a variation of the TypedSymbol that allows - * for a runtime label to be placed on the label, so as to express - * "Pose 5 for robot 3" - * Labels should be kept to base datatypes (int, char, etc) to - * minimize cost of comparisons - * - * The labels will be compared first when comparing Keys, followed by the - * index - */ -template -class TypedLabeledSymbol: public TypedSymbol { - -protected: - // Label - L label_; - -public: - - typedef TypedSymbol Base; - - // Constructors: - - TypedLabeledSymbol() { - } - TypedLabeledSymbol(size_t j, L label) : - Base(j), label_(label) { - } - - /** Constructor that decodes encoded labels */ - TypedLabeledSymbol(const Symbol& sym) : - TypedSymbol (0) { - size_t shift = (sizeof(size_t) - sizeof(short)) * 8; - this->j_ = (sym.index() << shift) >> shift; // truncate upper bits - label_ = (L) (sym.index() >> shift); // remove lower bits - } - - /** Constructor to upgrade an existing typed label with a label */ - TypedLabeledSymbol(const Base& key, L label) : - Base(key.index()), label_(label) { - } - - // Get stuff: - - L label() const { - return label_; - } - const char* c_str() const { - return ((std::string)(*this)).c_str(); - } - operator std::string() const { - std::string label_s = (boost::format("%1%") % label_).str(); - return (boost::format("%c%s_%d") % C % label_s % this->j_).str(); - } - std::string latex() const { - std::string label_s = (boost::format("%1%") % label_).str(); - return (boost::format("%c%s_{%d}") % C % label_s % this->j_).str(); - } - - // Needed for conversion to LabeledSymbol - size_t convertLabel() const { - return label_; - } - - /** - * Encoding two numbers into a single size_t for conversion to Symbol - * Stores the label in the upper bytes of the index - */ - size_t encode() const { - short label = (short) label_; //bound size of label to 2 bytes - size_t shift = (sizeof(size_t) - sizeof(short)) * 8; - size_t modifier = ((size_t) label) << shift; - return this->j_ + modifier; - } - - // logic: - - bool operator<(const TypedLabeledSymbol& compare) const { - if (label_ == compare.label_) // sort by label first - return this->j_ < compare.j_; - else - return label_ < compare.label_; - } - bool operator==(const TypedLabeledSymbol& compare) const { - return this->j_ == compare.j_ && label_ == compare.label_; - } - int compare(const TypedLabeledSymbol& compare) const { - if (label_ == compare.label_) // sort by label first - return this->j_ - compare.j_; - else - return label_ - compare.label_; - } - - // Testable Requirements - void print(const std::string& s = "") const { - std::cout << s << ": " << (std::string) (*this) << std::endl; - } - bool equals(const TypedLabeledSymbol& expected, double tol = 0.0) const { - return (*this) == expected; - } - -private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - typedef TypedSymbol Base; - ar & boost::serialization::make_nvp("TypedLabeledSymbol", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(label_); - } -}; - -} // namespace gtsam - diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am deleted file mode 100644 index 6d5f126c8..000000000 --- a/gtsam/nonlinear/Makefile.am +++ /dev/null @@ -1,76 +0,0 @@ -#---------------------------------------------------------------------------------------------------- -# GTSAM nonlinear -# Non-linear optimization -#---------------------------------------------------------------------------------------------------- - -# use nostdinc to turn off -I. and -I.., we do not need them because -# header files are qualified so they can be included in external projects. -AUTOMAKE_OPTIONS = nostdinc - -headers = -sources = -check_PROGRAMS = - -#---------------------------------------------------------------------------------------------------- -# nonlinear -#---------------------------------------------------------------------------------------------------- - -# Lie Groups -headers += Values.h Values-inl.h TupleValues.h TupleValues-inl.h -check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering - -# Nonlinear nonlinear -headers += Key.h -headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h -headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h -headers += NonlinearFactor.h -sources += NonlinearOptimizer.cpp Ordering.cpp DoglegOptimizerImpl.cpp NonlinearOptimizationParameters.cpp -headers += DoglegOptimizer.h DoglegOptimizer-inl.h - -# Nonlinear iSAM(2) -headers += NonlinearISAM.h NonlinearISAM-inl.h -headers += ISAM2.h ISAM2-inl.h ISAM2-impl-inl.h -sources += GaussianISAM2.cpp -headers += GaussianISAM2-inl.h - -# Nonlinear constraints -headers += NonlinearEquality.h - -# White noise factor -headers += WhiteNoiseFactor.h -#check_PROGRAMS += tests/testWhiteFactor - -# Kalman Filter -headers += ExtendedKalmanFilter.h ExtendedKalmanFilter-inl.h - -#---------------------------------------------------------------------------------------------------- -# Create a libtool library that is not installed -# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am -# The headers are installed in $(includedir)/gtsam: -#---------------------------------------------------------------------------------------------------- -headers += $(sources:.cpp=.h) -nonlineardir = $(pkgincludedir)/nonlinear -nonlinear_HEADERS = $(headers) -noinst_LTLIBRARIES = libnonlinear.la -libnonlinear_la_SOURCES = $(sources) -AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) -AM_LDFLAGS = $(BOOST_LDFLAGS) -AM_CXXFLAGS = - -#---------------------------------------------------------------------------------------------------- -# rules to build local programs -#---------------------------------------------------------------------------------------------------- -TESTS = $(check_PROGRAMS) -AM_LDFLAGS += $(boost_serialization) -LDADD = libnonlinear.la ../linear/liblinear.la ../inference/libinference.la ../base/libbase.la ../3rdparty/libccolamd.la -LDADD += ../../CppUnitLite/libCppUnitLite.a -AM_DEFAULT_SOURCE_EXT = .cpp - -# rule to run an executable -%.run: % $(LDADD) - ./$^ - -# rule to run executable with valgrind -%.valgrind: % $(LDADD) - valgrind ./$^ - diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 7537ea192..a1c685668 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -47,11 +47,11 @@ namespace gtsam { * * \nosubgrouping */ - template - class NonlinearEquality: public NonlinearFactor1 { + template + class NonlinearEquality: public NoiseModelFactor1 { public: - typedef typename KEY::Value T; + typedef VALUE T; private: @@ -64,6 +64,12 @@ namespace gtsam { // error gain in allow error case double error_gain_; + // typedef to this class + typedef NonlinearEquality This; + + // typedef to base class + typedef NoiseModelFactor1 Base; + public: /** @@ -71,7 +77,6 @@ namespace gtsam { */ bool (*compare_)(const T& a, const T& b); - typedef NonlinearFactor1 Base; /** default constructor - only for serialization */ NonlinearEquality() {} @@ -84,7 +89,7 @@ namespace gtsam { /** * Constructor - forces exact evaluation */ - NonlinearEquality(const KEY& j, const T& feasible, bool (*_compare)(const T&, const T&) = compare) : + NonlinearEquality(Key j, const T& feasible, bool (*_compare)(const T&, const T&) = compare) : Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible), allow_error_(false), error_gain_(0.0), compare_(_compare) { @@ -93,7 +98,7 @@ namespace gtsam { /** * Constructor - allows inexact evaluation */ - NonlinearEquality(const KEY& j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare) : + NonlinearEquality(Key j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare) : Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible), allow_error_(true), error_gain_(error_gain), compare_(_compare) { @@ -103,17 +108,17 @@ namespace gtsam { /// @name Testable /// @{ - void print(const std::string& s = "") const { - std::cout << "Constraint: " << s << " on [" << (std::string)(this->key_) << "]\n"; + virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << "Constraint: " << s << " on [" << keyFormatter(this->key()) << "]\n"; gtsam::print(feasible_,"Feasible Point"); std::cout << "Variable Dimension: " << feasible_.dim() << std::endl; } /** Check if two factors are equal */ - bool equals(const NonlinearEquality& f, double tol = 1e-9) const { - if (!Base::equals(f)) return false; - return feasible_.equals(f.feasible_, tol) && - fabs(error_gain_ - f.error_gain_) < tol; + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { + const This* e = dynamic_cast(&f); + return e && Base::equals(f) && feasible_.equals(e->feasible_, tol) && + fabs(error_gain_ - e->error_gain_) < tol; } /// @} @@ -121,8 +126,8 @@ namespace gtsam { /// @{ /** actual error function calculation */ - virtual double error(const VALUES& c) const { - const T& xj = c[this->key_]; + virtual double error(const Values& c) const { + const T& xj = c.at(this->key()); Vector e = this->unwhitenedError(c); if (allow_error_ || !compare_(xj, feasible_)) { return error_gain_ * dot(e,e); @@ -132,7 +137,7 @@ namespace gtsam { } /** error function */ - inline Vector evaluateError(const T& xj, boost::optional H = boost::none) const { + Vector evaluateError(const T& xj, boost::optional H = boost::none) const { size_t nj = feasible_.dim(); if (allow_error_) { if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare @@ -142,18 +147,18 @@ namespace gtsam { return zero(nj); // set error to zero if equal } else { if (H) throw std::invalid_argument( - "Linearization point not feasible for " + (std::string)(this->key_) + "!"); + "Linearization point not feasible for " + DefaultKeyFormatter(this->key()) + "!"); return repeat(nj, std::numeric_limits::infinity()); // set error to infinity if not equal } } // Linearize is over-written, because base linearization tries to whiten - virtual GaussianFactor::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const { - const T& xj = x[this->key_]; + virtual GaussianFactor::shared_ptr linearize(const Values& x, const Ordering& ordering) const { + const T& xj = x.at(this->key()); Matrix A; Vector b = evaluateError(xj, A); SharedDiagonal model = noiseModel::Constrained::All(b.size()); - return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key_], A, b, model)); + return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key()], A, b, model)); } /// @} @@ -164,7 +169,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor1", + ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(feasible_); ar & BOOST_SERIALIZATION_NVP(allow_error_); @@ -177,14 +182,14 @@ namespace gtsam { /** * Simple unary equality constraint - fixes a value for a variable */ - template - class NonlinearEquality1 : public NonlinearFactor1 { + template + class NonlinearEquality1 : public NoiseModelFactor1 { public: - typedef typename KEY::Value X; + typedef VALUE X; protected: - typedef NonlinearFactor1 Base; + typedef NoiseModelFactor1 Base; /** default constructor to allow for serialization */ NonlinearEquality1() {} @@ -196,10 +201,10 @@ namespace gtsam { public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; ///TODO: comment - NonlinearEquality1(const X& value, const KEY& key1, double mu = 1000.0) + NonlinearEquality1(const X& value, Key key1, double mu = 1000.0) : Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {} virtual ~NonlinearEquality1() {} @@ -212,9 +217,9 @@ namespace gtsam { } /** Print */ - virtual void print(const std::string& s = "") const { + virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << ": NonlinearEquality1(" - << (std::string) this->key_ << "),"<< "\n"; + << keyFormatter(this->key()) << "),"<< "\n"; this->noiseModel_->print(); value_.print("Value"); } @@ -225,7 +230,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor1", + ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(value_); } @@ -236,13 +241,13 @@ namespace gtsam { * Simple binary equality constraint - this constraint forces two factors to * be the same. */ - template - class NonlinearEquality2 : public NonlinearFactor2 { + template + class NonlinearEquality2 : public NoiseModelFactor2 { public: - typedef typename KEY::Value X; + typedef VALUE X; protected: - typedef NonlinearFactor2 Base; + typedef NoiseModelFactor2 Base; GTSAM_CONCEPT_MANIFOLD_TYPE(X); @@ -251,10 +256,10 @@ namespace gtsam { public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; ///TODO: comment - NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0) + NonlinearEquality2(Key key1, Key key2, double mu = 1000.0) : Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {} virtual ~NonlinearEquality2() {} @@ -274,7 +279,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor2", + ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); } }; // \NonlinearEquality2 diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index ad137a5d9..e260f24db 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -24,30 +24,19 @@ #include #include -#include +#include #include #include #include #include +#include #include +#include namespace gtsam { -using boost::make_tuple; - -// Helper function to fill a vector from a tuple function of any length -template -inline void __fill_from_tuple(std::vector& vector, size_t position, const CONS& tuple) { - vector[position] = tuple.get_head(); - __fill_from_tuple(vector, position+1, tuple.get_tail()); -} -template<> -inline void __fill_from_tuple(std::vector& vector, size_t position, const boost::tuples::null_type& tuple) { - // Do nothing -} - /* ************************************************************************* */ /** * Nonlinear factor base class @@ -57,18 +46,17 @@ inline void __fill_from_tuple(std::vector& vec * which are objects in non-linear manifolds (Lie groups). * \nosubgrouping */ -template -class NonlinearFactor: public Factor { +class NonlinearFactor: public Factor { protected: // Some handy typedefs - typedef Factor Base; - typedef NonlinearFactor This; + typedef Factor Base; + typedef NonlinearFactor This; public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr shared_ptr; /// @name Standard Constructors /// @{ @@ -77,18 +65,6 @@ public: NonlinearFactor() { } - /** - * Constructor - * @param keys A boost::tuple containing the variables involved in this factor, - * example: NonlinearFactor(make_tuple(symbol1, symbol2, symbol3)) - */ - template - NonlinearFactor(const boost::tuples::cons& keys) { - this->keys_.resize(boost::tuples::length >::value); - // Use helper function to fill key vector, using 'cons' representation of tuple - __fill_from_tuple(this->keys(), 0, keys); - } - /** * Constructor * @param keys The variables involved in this factor @@ -103,8 +79,15 @@ public: /// @{ /** print */ - virtual void print(const std::string& s = "") const { - std::cout << s << ": NonlinearFactor\n"; + virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "keys = { "; + BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; } + std::cout << "}" << endl; + } + + /** Check if two factors are equal */ + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { + return Base::equals(f); } /// @} @@ -120,7 +103,7 @@ public: * This is typically equal to log-likelihood, e.g. 0.5(h(x)-z)^2/sigma^2 in case of Gaussian. * You can override this for systems with unusual noise models. */ - virtual double error(const VALUES& c) const = 0; + virtual double error(const Values& c) const = 0; /** get the dimension of the factor (number of rows on linearization) */ virtual size_t dim() const = 0; @@ -135,11 +118,11 @@ public: * when the constraint is *NOT* fulfilled. * @return true if the constraint is active */ - virtual bool active(const VALUES& c) const { return true; } + virtual bool active(const Values& c) const { return true; } /** linearize to a GaussianFactor */ virtual boost::shared_ptr - linearize(const VALUES& c, const Ordering& ordering) const = 0; + linearize(const Values& c, const Ordering& ordering) const = 0; /** * Create a symbolic factor using the given ordering to determine the @@ -165,20 +148,19 @@ public: * The noise model is typically Gaussian, but robust and constrained error models are also supported. */ -template -class NoiseModelFactor: public NonlinearFactor { +class NoiseModelFactor: public NonlinearFactor { protected: // handy typedefs - typedef NonlinearFactor Base; - typedef NoiseModelFactor This; + typedef NonlinearFactor Base; + typedef NoiseModelFactor This; SharedNoiseModel noiseModel_; /** Noise model */ public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr shared_ptr; /** Default constructor for I/O only */ NoiseModelFactor() { @@ -187,16 +169,6 @@ public: /** Destructor */ virtual ~NoiseModelFactor() {} - /** - * Constructor - * @param keys A boost::tuple containing the variables involved in this factor, - * example: NoiseModelFactor(noiseModel, make_tuple(symbol1, symbol2, symbol3) - */ - template - NoiseModelFactor(const SharedNoiseModel& noiseModel, const boost::tuples::cons& keys) - : Base(keys), noiseModel_(noiseModel) { - } - /** * Constructor * @param keys The variables involved in this factor @@ -216,17 +188,15 @@ protected: public: /** Print */ - virtual void print(const std::string& s = "") const { - std::cout << s << ": NoiseModelFactor\n"; - std::cout << " "; - BOOST_FOREACH(const Symbol& key, this->keys()) { std::cout << (std::string)key << " "; } - std::cout << "\n"; + virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s, keyFormatter); this->noiseModel_->print(" noise model: "); } /** Check if two factors are equal */ - virtual bool equals(const NoiseModelFactor& f, double tol = 1e-9) const { - return noiseModel_->equals(*f.noiseModel_, tol) && Base::equals(f, tol); + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { + const NoiseModelFactor* e = dynamic_cast(&f); + return e && Base::equals(f, tol) && noiseModel_->equals(*e->noiseModel_, tol); } /** get the dimension of the factor (number of rows on linearization) */ @@ -245,13 +215,13 @@ public: * If any of the optional Matrix reference arguments are specified, it should compute * both the function evaluation and its derivative(s) in X1 (and/or X2, X3...). */ - virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const = 0; + virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const = 0; /** * Vector of errors, whitened * This is the raw error, i.e., i.e. \f$ (h(x)-z)/\sigma \f$ in case of a Gaussian */ - Vector whitenedError(const VALUES& c) const { + Vector whitenedError(const Values& c) const { return noiseModel_->whiten(unwhitenedError(c)); } @@ -261,7 +231,7 @@ public: * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. */ - virtual double error(const VALUES& c) const { + virtual double error(const Values& c) const { if (this->active(c)) return 0.5 * noiseModel_->distance(unwhitenedError(c)); else @@ -273,7 +243,7 @@ public: * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ */ - boost::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const { + boost::shared_ptr linearize(const Values& x, const Ordering& ordering) const { // Only linearize if the factor is active if (!this->active(x)) return boost::shared_ptr(); @@ -321,45 +291,44 @@ private: /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 1 * variable. To derive from this class, implement evaluateError(). */ -template -class NonlinearFactor1: public NoiseModelFactor { +template +class NoiseModelFactor1: public NoiseModelFactor { public: // typedefs for value types pulled from keys - typedef typename KEY::Value X; + typedef VALUE X; protected: - // The value of the key. Not const to allow serialization - KEY key_; - - typedef NoiseModelFactor Base; - typedef NonlinearFactor1 This; + typedef NoiseModelFactor Base; + typedef NoiseModelFactor1 This; public: /** Default constructor for I/O only */ - NonlinearFactor1() {} + NoiseModelFactor1() {} - virtual ~NonlinearFactor1() {} + virtual ~NoiseModelFactor1() {} - inline const KEY& key() const { return key_; } + inline Key key() const { return keys_[0]; } /** * Constructor * @param z measurement * @param key by which to look up X value in Values */ - NonlinearFactor1(const SharedNoiseModel& noiseModel, const KEY& key1) : - Base(noiseModel, make_tuple(key1)), key_(key1) { + NoiseModelFactor1(const SharedNoiseModel& noiseModel, Key key1) : + Base(noiseModel) { + keys_.resize(1); + keys_[0] = key1; } /** Calls the 1-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const { + virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if(this->active(x)) { - const X& x1 = x[key_]; + const X& x1 = x.at(keys_[0]); if(H) { return evaluateError(x1, (*H)[0]); } else { @@ -370,12 +339,6 @@ public: } } - /** Print */ - virtual void print(const std::string& s = "") const { - std::cout << s << ": NonlinearFactor1(" << (std::string) this->key_ << ")\n"; - this->noiseModel_->print(" noise model: "); - } - /** * Override this method to finish implementing a unary factor. * If the optional Matrix reference argument is specified, it should compute @@ -392,59 +355,58 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(key_); } -};// \class NonlinearFactor1 +};// \class NoiseModelFactor1 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 2 * variables. To derive from this class, implement evaluateError(). */ -template -class NonlinearFactor2: public NoiseModelFactor { +template +class NoiseModelFactor2: public NoiseModelFactor { public: // typedefs for value types pulled from keys - typedef typename KEY1::Value X1; - typedef typename KEY2::Value X2; + typedef VALUE1 X1; + typedef VALUE2 X2; protected: - // The values of the keys. Not const to allow serialization - KEY1 key1_; - KEY2 key2_; - - typedef NoiseModelFactor Base; - typedef NonlinearFactor2 This; + typedef NoiseModelFactor Base; + typedef NoiseModelFactor2 This; public: /** * Default Constructor for I/O */ - NonlinearFactor2() {} + NoiseModelFactor2() {} /** * Constructor * @param j1 key of the first variable * @param j2 key of the second variable */ - NonlinearFactor2(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2) : - Base(noiseModel, make_tuple(j1,j2)), key1_(j1), key2_(j2) {} + NoiseModelFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) : + Base(noiseModel) { + keys_.resize(2); + keys_[0] = j1; + keys_[1] = j2; + } - virtual ~NonlinearFactor2() {} + virtual ~NoiseModelFactor2() {} /** methods to retrieve both keys */ - inline const KEY1& key1() const { return key1_; } - inline const KEY2& key2() const { return key2_; } + inline Key key1() const { return keys_[0]; } + inline Key key2() const { return keys_[1]; } /** Calls the 2-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const { + virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if(this->active(x)) { - const X1& x1 = x[key1_]; - const X2& x2 = x[key2_]; + const X1& x1 = x.at(keys_[0]); + const X2& x2 = x.at(keys_[1]); if(H) { return evaluateError(x1, x2, (*H)[0], (*H)[1]); } else { @@ -455,14 +417,6 @@ public: } } - /** Print */ - virtual void print(const std::string& s = "") const { - std::cout << s << ": NonlinearFactor2(" - << (std::string) this->key1_ << "," - << (std::string) this->key2_ << ")\n"; - this->noiseModel_->print(" noise model: "); - } - /** * Override this method to finish implementing a binary factor. * If any of the optional Matrix reference arguments are specified, it should compute @@ -480,40 +434,33 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(key1_); - ar & BOOST_SERIALIZATION_NVP(key2_); } -}; // \class NonlinearFactor2 +}; // \class NoiseModelFactor2 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 3 * variables. To derive from this class, implement evaluateError(). */ -template -class NonlinearFactor3: public NoiseModelFactor { +template +class NoiseModelFactor3: public NoiseModelFactor { public: // typedefs for value types pulled from keys - typedef typename KEY1::Value X1; - typedef typename KEY2::Value X2; - typedef typename KEY3::Value X3; + typedef VALUE1 X1; + typedef VALUE2 X2; + typedef VALUE3 X3; protected: - // The values of the keys. Not const to allow serialization - KEY1 key1_; - KEY2 key2_; - KEY3 key3_; - - typedef NoiseModelFactor Base; - typedef NonlinearFactor3 This; + typedef NoiseModelFactor Base; + typedef NoiseModelFactor3 This; public: /** * Default Constructor for I/O */ - NonlinearFactor3() {} + NoiseModelFactor3() {} /** * Constructor @@ -521,39 +468,34 @@ public: * @param j2 key of the second variable * @param j3 key of the third variable */ - NonlinearFactor3(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3) : - Base(noiseModel, make_tuple(j1,j2,j3)), key1_(j1), key2_(j2), key3_(j3) {} + NoiseModelFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3) : + Base(noiseModel) { + keys_.resize(3); + keys_[0] = j1; + keys_[1] = j2; + keys_[2] = j3; + } - virtual ~NonlinearFactor3() {} + virtual ~NoiseModelFactor3() {} /** methods to retrieve keys */ - inline const KEY1& key1() const { return key1_; } - inline const KEY2& key2() const { return key2_; } - inline const KEY3& key3() const { return key3_; } + inline Key key1() const { return keys_[0]; } + inline Key key2() const { return keys_[1]; } + inline Key key3() const { return keys_[2]; } /** Calls the 3-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const { + virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if(this->active(x)) { if(H) - return evaluateError(x[key1_], x[key2_], x[key3_], (*H)[0], (*H)[1], (*H)[2]); + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), (*H)[0], (*H)[1], (*H)[2]); else - return evaluateError(x[key1_], x[key2_], x[key3_]); + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2])); } else { return zero(this->dim()); } } - /** Print */ - virtual void print(const std::string& s = "") const { - std::cout << s << ": NonlinearFactor3(" - << (std::string) this->key1_ << "," - << (std::string) this->key2_ << "," - << (std::string) this->key3_ << ")\n"; - this->noiseModel_->print(" noise model: "); - } - - /** * Override this method to finish implementing a trinary factor. * If any of the optional Matrix reference arguments are specified, it should compute @@ -573,43 +515,34 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(key1_); - ar & BOOST_SERIALIZATION_NVP(key2_); - ar & BOOST_SERIALIZATION_NVP(key3_); } -}; // \class NonlinearFactor3 +}; // \class NoiseModelFactor3 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 4 * variables. To derive from this class, implement evaluateError(). */ -template -class NonlinearFactor4: public NoiseModelFactor { +template +class NoiseModelFactor4: public NoiseModelFactor { public: // typedefs for value types pulled from keys - typedef typename KEY1::Value X1; - typedef typename KEY2::Value X2; - typedef typename KEY3::Value X3; - typedef typename KEY4::Value X4; + typedef VALUE1 X1; + typedef VALUE2 X2; + typedef VALUE3 X3; + typedef VALUE4 X4; protected: - // The values of the keys. Not const to allow serialization - KEY1 key1_; - KEY2 key2_; - KEY3 key3_; - KEY4 key4_; - - typedef NoiseModelFactor Base; - typedef NonlinearFactor4 This; + typedef NoiseModelFactor Base; + typedef NoiseModelFactor4 This; public: /** * Default Constructor for I/O */ - NonlinearFactor4() {} + NoiseModelFactor4() {} /** * Constructor @@ -618,40 +551,36 @@ public: * @param j3 key of the third variable * @param j4 key of the fourth variable */ - NonlinearFactor4(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4) : - Base(noiseModel, make_tuple(j1,j2,j3,j4)), key1_(j1), key2_(j2), key3_(j3), key4_(j4) {} + NoiseModelFactor4(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4) : + Base(noiseModel) { + keys_.resize(4); + keys_[0] = j1; + keys_[1] = j2; + keys_[2] = j3; + keys_[3] = j4; + } - virtual ~NonlinearFactor4() {} + virtual ~NoiseModelFactor4() {} /** methods to retrieve keys */ - inline const KEY1& key1() const { return key1_; } - inline const KEY2& key2() const { return key2_; } - inline const KEY3& key3() const { return key3_; } - inline const KEY4& key4() const { return key4_; } + inline Key key1() const { return keys_[0]; } + inline Key key2() const { return keys_[1]; } + inline Key key3() const { return keys_[2]; } + inline Key key4() const { return keys_[3]; } /** Calls the 4-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const { + virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if(this->active(x)) { if(H) - return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], (*H)[0], (*H)[1], (*H)[2], (*H)[3]); + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]); else - return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_]); + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3])); } else { return zero(this->dim()); } } - /** Print */ - virtual void print(const std::string& s = "") const { - std::cout << s << ": NonlinearFactor4(" - << (std::string) this->key1_ << "," - << (std::string) this->key2_ << "," - << (std::string) this->key3_ << "," - << (std::string) this->key4_ << ")\n"; - this->noiseModel_->print(" noise model: "); - } - /** * Override this method to finish implementing a 4-way factor. * If any of the optional Matrix reference arguments are specified, it should compute @@ -672,46 +601,35 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(key1_); - ar & BOOST_SERIALIZATION_NVP(key2_); - ar & BOOST_SERIALIZATION_NVP(key3_); - ar & BOOST_SERIALIZATION_NVP(key4_); } -}; // \class NonlinearFactor4 +}; // \class NoiseModelFactor4 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 5 * variables. To derive from this class, implement evaluateError(). */ -template -class NonlinearFactor5: public NoiseModelFactor { +template +class NoiseModelFactor5: public NoiseModelFactor { public: // typedefs for value types pulled from keys - typedef typename KEY1::Value X1; - typedef typename KEY2::Value X2; - typedef typename KEY3::Value X3; - typedef typename KEY4::Value X4; - typedef typename KEY5::Value X5; + typedef VALUE1 X1; + typedef VALUE2 X2; + typedef VALUE3 X3; + typedef VALUE4 X4; + typedef VALUE5 X5; protected: - // The values of the keys. Not const to allow serialization - KEY1 key1_; - KEY2 key2_; - KEY3 key3_; - KEY4 key4_; - KEY5 key5_; - - typedef NoiseModelFactor Base; - typedef NonlinearFactor5 This; + typedef NoiseModelFactor Base; + typedef NoiseModelFactor5 This; public: /** * Default Constructor for I/O */ - NonlinearFactor5() {} + NoiseModelFactor5() {} /** * Constructor @@ -721,42 +639,38 @@ public: * @param j4 key of the fourth variable * @param j5 key of the fifth variable */ - NonlinearFactor5(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4, const KEY5& j5) : - Base(noiseModel, make_tuple(j1,j2,j3,j4,j5)), key1_(j1), key2_(j2), key3_(j3), key4_(j4), key5_(j5) {} + NoiseModelFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5) : + Base(noiseModel) { + keys_.resize(5); + keys_[0] = j1; + keys_[1] = j2; + keys_[2] = j3; + keys_[3] = j4; + keys_[4] = j5; + } - virtual ~NonlinearFactor5() {} + virtual ~NoiseModelFactor5() {} /** methods to retrieve keys */ - inline const KEY1& key1() const { return key1_; } - inline const KEY2& key2() const { return key2_; } - inline const KEY3& key3() const { return key3_; } - inline const KEY4& key4() const { return key4_; } - inline const KEY5& key5() const { return key5_; } + inline Key key1() const { return keys_[0]; } + inline Key key2() const { return keys_[1]; } + inline Key key3() const { return keys_[2]; } + inline Key key4() const { return keys_[3]; } + inline Key key5() const { return keys_[4]; } /** Calls the 5-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const { + virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if(this->active(x)) { if(H) - return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]); + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]); else - return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_]); + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4])); } else { return zero(this->dim()); } } - /** Print */ - virtual void print(const std::string& s = "") const { - std::cout << s << ": NonlinearFactor5(" - << (std::string) this->key1_ << "," - << (std::string) this->key2_ << "," - << (std::string) this->key3_ << "," - << (std::string) this->key4_ << "," - << (std::string) this->key5_ << ")\n"; - this->noiseModel_->print(" noise model: "); - } - /** * Override this method to finish implementing a 5-way factor. * If any of the optional Matrix reference arguments are specified, it should compute @@ -778,49 +692,36 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(key1_); - ar & BOOST_SERIALIZATION_NVP(key2_); - ar & BOOST_SERIALIZATION_NVP(key3_); - ar & BOOST_SERIALIZATION_NVP(key4_); - ar & BOOST_SERIALIZATION_NVP(key5_); } -}; // \class NonlinearFactor5 +}; // \class NoiseModelFactor5 /* ************************************************************************* */ /** A convenient base class for creating your own NoiseModelFactor with 6 * variables. To derive from this class, implement evaluateError(). */ -template -class NonlinearFactor6: public NoiseModelFactor { +template +class NoiseModelFactor6: public NoiseModelFactor { public: // typedefs for value types pulled from keys - typedef typename KEY1::Value X1; - typedef typename KEY2::Value X2; - typedef typename KEY3::Value X3; - typedef typename KEY4::Value X4; - typedef typename KEY5::Value X5; - typedef typename KEY6::Value X6; + typedef VALUE1 X1; + typedef VALUE2 X2; + typedef VALUE3 X3; + typedef VALUE4 X4; + typedef VALUE5 X5; + typedef VALUE6 X6; protected: - // The values of the keys. Not const to allow serialization - KEY1 key1_; - KEY2 key2_; - KEY3 key3_; - KEY4 key4_; - KEY5 key5_; - KEY6 key6_; - - typedef NoiseModelFactor Base; - typedef NonlinearFactor6 This; + typedef NoiseModelFactor Base; + typedef NoiseModelFactor6 This; public: /** * Default Constructor for I/O */ - NonlinearFactor6() {} + NoiseModelFactor6() {} /** * Constructor @@ -831,44 +732,40 @@ public: * @param j5 key of the fifth variable * @param j6 key of the fifth variable */ - NonlinearFactor6(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4, const KEY5& j5, const KEY6& j6) : - Base(noiseModel, make_tuple(j1,j2,j3,j4,j5,j6)), key1_(j1), key2_(j2), key3_(j3), key4_(j4), key5_(j5), key6_(j6) {} + NoiseModelFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) : + Base(noiseModel) { + keys_.resize(6); + keys_[0] = j1; + keys_[1] = j2; + keys_[2] = j3; + keys_[3] = j4; + keys_[4] = j5; + keys_[5] = j6; + } - virtual ~NonlinearFactor6() {} + virtual ~NoiseModelFactor6() {} /** methods to retrieve keys */ - inline const KEY1& key1() const { return key1_; } - inline const KEY2& key2() const { return key2_; } - inline const KEY3& key3() const { return key3_; } - inline const KEY4& key4() const { return key4_; } - inline const KEY5& key5() const { return key5_; } - inline const KEY6& key6() const { return key6_; } + inline Key key1() const { return keys_[0]; } + inline Key key2() const { return keys_[1]; } + inline Key key3() const { return keys_[2]; } + inline Key key4() const { return keys_[3]; } + inline Key key5() const { return keys_[4]; } + inline Key key6() const { return keys_[5]; } /** Calls the 6-key specific version of evaluateError, which is pure virtual * so must be implemented in the derived class. */ - virtual Vector unwhitenedError(const VALUES& x, boost::optional&> H = boost::none) const { + virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { if(this->active(x)) { if(H) - return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]); + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), x.at(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]); else - return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_]); + return evaluateError(x.at(keys_[0]), x.at(keys_[1]), x.at(keys_[2]), x.at(keys_[3]), x.at(keys_[4]), x.at(keys_[5])); } else { return zero(this->dim()); } } - /** Print */ - virtual void print(const std::string& s = "") const { - std::cout << s << ": NonlinearFactor6(" - << (std::string) this->key1_ << "," - << (std::string) this->key2_ << "," - << (std::string) this->key3_ << "," - << (std::string) this->key4_ << "," - << (std::string) this->key5_ << "," - << (std::string) this->key6_ << ")\n"; - this->noiseModel_->print(" noise model: "); - } - /** * Override this method to finish implementing a 6-way factor. * If any of the optional Matrix reference arguments are specified, it should compute @@ -891,14 +788,8 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("NoiseModelFactor", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(key1_); - ar & BOOST_SERIALIZATION_NVP(key2_); - ar & BOOST_SERIALIZATION_NVP(key3_); - ar & BOOST_SERIALIZATION_NVP(key4_); - ar & BOOST_SERIALIZATION_NVP(key5_); - ar & BOOST_SERIALIZATION_NVP(key6_); } -}; // \class NonlinearFactor6 +}; // \class NoiseModelFactor6 /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearFactorGraph-inl.h b/gtsam/nonlinear/NonlinearFactorGraph.cpp similarity index 77% rename from gtsam/nonlinear/NonlinearFactorGraph-inl.h rename to gtsam/nonlinear/NonlinearFactorGraph.cpp index 473107429..647afa2d5 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph-inl.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.cpp @@ -10,39 +10,40 @@ * -------------------------------------------------------------------------- */ /** - * @file NonlinearFactorGraph-inl.h + * @file NonlinearFactorGraph.cpp * @brief Factor Graph Consisting of non-linear factors * @author Frank Dellaert * @author Carlos Nieto * @author Christian Potthast */ -#pragma once - +#include +#include #include #include -#include -#include +#include using namespace std; namespace gtsam { /* ************************************************************************* */ - template - double NonlinearFactorGraph::probPrime(const VALUES& c) const { + double NonlinearFactorGraph::probPrime(const Values& c) const { return exp(-0.5 * error(c)); } /* ************************************************************************* */ - template - void NonlinearFactorGraph::print(const std::string& str) const { - Base::print(str); + void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& keyFormatter) const { + cout << str << "size: " << size() << endl; + for (size_t i = 0; i < factors_.size(); i++) { + stringstream ss; + ss << "factor " << i << ": "; + if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter); + } } /* ************************************************************************* */ - template - double NonlinearFactorGraph::error(const VALUES& c) const { + double NonlinearFactorGraph::error(const Values& c) const { double total_error = 0.; // iterate over all the factors_ to accumulate the log probabilities BOOST_FOREACH(const sharedFactor& factor, this->factors_) { @@ -53,9 +54,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - std::set NonlinearFactorGraph::keys() const { - std::set keys; + std::set NonlinearFactorGraph::keys() const { + std::set keys; BOOST_FOREACH(const sharedFactor& factor, this->factors_) { if(factor) keys.insert(factor->begin(), factor->end()); @@ -64,9 +64,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD( - const VALUES& config) const { + Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD( + const Values& config) const { // Create symbolic graph and initial (iterator) ordering SymbolicFactorGraph::shared_ptr symbolic; @@ -91,8 +90,7 @@ namespace gtsam { } /* ************************************************************************* */ - template - SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const { + SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const { // Generate the symbolic factor graph SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph); symbolicfg->reserve(this->size()); @@ -108,21 +106,19 @@ namespace gtsam { } /* ************************************************************************* */ - template - pair NonlinearFactorGraph< - VALUES>::symbolic(const VALUES& config) const { + pair NonlinearFactorGraph::symbolic( + const Values& config) const { // Generate an initial key ordering in iterator order Ordering::shared_ptr ordering(config.orderingArbitrary()); return make_pair(symbolic(*ordering), ordering); } /* ************************************************************************* */ - template - typename GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize( - const VALUES& config, const Ordering& ordering) const { + GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize( + const Values& config, const Ordering& ordering) const { // create an empty linear FG - typename GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph); + GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph); linearFG->reserve(this->size()); // linearize all factors diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index b572c7d84..3caeec680 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -28,42 +28,37 @@ namespace gtsam { /** - * A non-linear factor graph is templated on a values structure, but the factor type - * is fixed as a NonlinearFactor. The values structures are typically (in SAM) more general + * A non-linear factor graph is a graph of non-Gaussian, i.e. non-linear factors, + * which derive from NonlinearFactor. The values structures are typically (in SAM) more general * than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds. * Linearizing the non-linear factor graph creates a linear factor graph on the * tangent vector space at the linearization point. Because the tangent space is a true * vector space, the config type will be an VectorValues in that linearized factor graph. - * \nosubgrouping */ - template - class NonlinearFactorGraph: public FactorGraph > { + class NonlinearFactorGraph: public FactorGraph { public: - typedef VALUES Values; - typedef FactorGraph > Base; - typedef boost::shared_ptr > shared_ptr; - typedef boost::shared_ptr > sharedFactor; - - /// @name Testable - /// @{ + typedef FactorGraph Base; + typedef boost::shared_ptr shared_ptr; + typedef boost::shared_ptr sharedFactor; /** print just calls base class */ - void print(const std::string& str = "NonlinearFactorGraph: ") const; - - /// @} - /// @name Standard Interface - /// @{ + void print(const std::string& str = "NonlinearFactorGraph: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** return keys in some random order */ - std::set keys() const; + std::set keys() const; /** unnormalized error */ - double error(const VALUES& c) const; + double error(const Values& c) const; /** Unnormalized probability. O(n) */ - double probPrime(const VALUES& c) const; + double probPrime(const Values& c) const; + + template + void add(const F& factor) { + this->push_back(boost::shared_ptr(new F(factor))); + } /** * Create a symbolic factor graph using an existing ordering @@ -77,31 +72,20 @@ namespace gtsam { * ordering is found. */ std::pair - symbolic(const VALUES& config) const; + symbolic(const Values& config) const; /** * Compute a fill-reducing ordering using COLAMD. This returns the * ordering and a VariableIndex, which can later be re-used to save * computation. */ - Ordering::shared_ptr orderingCOLAMD(const VALUES& config) const; + Ordering::shared_ptr orderingCOLAMD(const Values& config) const; /** * linearize a nonlinear factor graph */ boost::shared_ptr - linearize(const VALUES& config, const Ordering& ordering) const; - - /// @} - /// @name Advanced Interface - /// @{ - - template - void add(const F& factor) { - this->push_back(boost::shared_ptr(new F(factor))); - } - - /// @} + linearize(const Values& config, const Ordering& ordering) const; private: @@ -116,4 +100,3 @@ namespace gtsam { } // namespace -#include diff --git a/gtsam/nonlinear/NonlinearISAM-inl.h b/gtsam/nonlinear/NonlinearISAM-inl.h index 16ec47247..e054ecacb 100644 --- a/gtsam/nonlinear/NonlinearISAM-inl.h +++ b/gtsam/nonlinear/NonlinearISAM-inl.h @@ -29,8 +29,8 @@ using namespace std; using namespace gtsam; /* ************************************************************************* */ -template -void NonlinearISAM::update(const Factors& newFactors, +template +void NonlinearISAM::update(const Factors& newFactors, const Values& initialValues) { if(newFactors.size() > 0) { @@ -50,7 +50,7 @@ void NonlinearISAM::update(const Factors& newFactors, // Augment ordering // FIXME: should just loop over new values BOOST_FOREACH(const typename Factors::sharedFactor& factor, newFactors) - BOOST_FOREACH(const Symbol& key, factor->keys()) + BOOST_FOREACH(Key key, factor->keys()) ordering_.tryInsert(key, ordering_.nVars()); // will do nothing if already present boost::shared_ptr linearizedNewFactors( @@ -62,8 +62,8 @@ void NonlinearISAM::update(const Factors& newFactors, } /* ************************************************************************* */ -template -void NonlinearISAM::reorder_relinearize() { +template +void NonlinearISAM::reorder_relinearize() { // cout << "Reordering, relinearizing..." << endl; @@ -89,8 +89,8 @@ void NonlinearISAM::reorder_relinearize() { } /* ************************************************************************* */ -template -VALUES NonlinearISAM::estimate() const { +template +Values NonlinearISAM::estimate() const { if(isam_.size() > 0) return linPoint_.retract(optimize(isam_), ordering_); else @@ -98,14 +98,14 @@ VALUES NonlinearISAM::estimate() const { } /* ************************************************************************* */ -template -Matrix NonlinearISAM::marginalCovariance(const Symbol& key) const { +template +Matrix NonlinearISAM::marginalCovariance(Key key) const { return isam_.marginalCovariance(ordering_[key]); } /* ************************************************************************* */ -template -void NonlinearISAM::print(const std::string& s) const { +template +void NonlinearISAM::print(const std::string& s) const { cout << "ISAM - " << s << ":" << endl; cout << " ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl; isam_.print("GaussianISAM"); diff --git a/gtsam/nonlinear/NonlinearISAM.h b/gtsam/nonlinear/NonlinearISAM.h index 9f082330c..16547770b 100644 --- a/gtsam/nonlinear/NonlinearISAM.h +++ b/gtsam/nonlinear/NonlinearISAM.h @@ -24,11 +24,10 @@ namespace gtsam { /** * Wrapper class to manage ISAM in a nonlinear context */ -template > +template class NonlinearISAM { public: - typedef VALUES Values; typedef GRAPH Factors; protected: @@ -71,7 +70,7 @@ public: Values estimate() const; /** find the marginal covariance for a single variable */ - Matrix marginalCovariance(const Symbol& key) const; + Matrix marginalCovariance(Key key) const; // access @@ -105,7 +104,7 @@ public: void reorder_relinearize(); /** manually add a key to the end of the ordering */ - void addKey(const Symbol& key) { ordering_.push_back(key); } + void addKey(Key key) { ordering_.push_back(key); } /** replace the current ordering */ void setOrdering(const Ordering& new_ordering) { ordering_ = new_ordering; } diff --git a/gtsam/nonlinear/NonlinearOptimization-inl.h b/gtsam/nonlinear/NonlinearOptimization-inl.h index 105a9de19..361de59e2 100644 --- a/gtsam/nonlinear/NonlinearOptimization-inl.h +++ b/gtsam/nonlinear/NonlinearOptimization-inl.h @@ -34,17 +34,17 @@ namespace gtsam { /** * The Elimination solver */ - template - T optimizeSequential(const G& graph, const T& initialEstimate, + template + Values optimizeSequential(const G& graph, const Values& initialEstimate, const NonlinearOptimizationParameters& parameters, bool useLM) { // Use a variable ordering from COLAMD Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate); // Create an nonlinear Optimizer that uses a Sequential Solver - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; Optimizer optimizer(boost::make_shared(graph), - boost::make_shared(initialEstimate), ordering, + boost::make_shared(initialEstimate), ordering, boost::make_shared(parameters)); // Now optimize using either LM or GN methods. @@ -57,17 +57,17 @@ namespace gtsam { /** * The multifrontal solver */ - template - T optimizeMultiFrontal(const G& graph, const T& initialEstimate, + template + Values optimizeMultiFrontal(const G& graph, const Values& initialEstimate, const NonlinearOptimizationParameters& parameters, bool useLM) { // Use a variable ordering from COLAMD Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate); // Create an nonlinear Optimizer that uses a Multifrontal Solver - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; Optimizer optimizer(boost::make_shared(graph), - boost::make_shared(initialEstimate), ordering, + boost::make_shared(initialEstimate), ordering, boost::make_shared(parameters)); // now optimize using either LM or GN methods @@ -81,20 +81,20 @@ namespace gtsam { /** * The sparse preconditioned conjugate gradient solver */ - template - T optimizeSPCG(const G& graph, const T& initialEstimate, + template + Values optimizeSPCG(const G& graph, const Values& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(), bool useLM = true) { // initial optimization state is the same in both cases tested - typedef SubgraphSolver Solver; + typedef SubgraphSolver Solver; typedef boost::shared_ptr shared_Solver; - typedef NonlinearOptimizer SPCGOptimizer; + typedef NonlinearOptimizer SPCGOptimizer; shared_Solver solver = boost::make_shared( graph, initialEstimate, IterativeOptimizationParameters()); SPCGOptimizer optimizer( boost::make_shared(graph), - boost::make_shared(initialEstimate), + boost::make_shared(initialEstimate), solver->ordering(), solver, boost::make_shared(parameters)); @@ -110,20 +110,20 @@ namespace gtsam { /** * optimization that returns the values */ - template - T optimize(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters, + template + Values optimize(const G& graph, const Values& initialEstimate, const NonlinearOptimizationParameters& parameters, const LinearSolver& solver, const NonlinearOptimizationMethod& nonlinear_method) { switch (solver) { case SEQUENTIAL: - return optimizeSequential(graph, initialEstimate, parameters, + return optimizeSequential(graph, initialEstimate, parameters, nonlinear_method == LM); case MULTIFRONTAL: - return optimizeMultiFrontal(graph, initialEstimate, parameters, + return optimizeMultiFrontal(graph, initialEstimate, parameters, nonlinear_method == LM); #if ENABLE_SPCG case SPCG: -// return optimizeSPCG(graph, initialEstimate, parameters, +// return optimizeSPCG(graph, initialEstimate, parameters, // nonlinear_method == LM); throw runtime_error("optimize: SPCG not supported yet due to the specific pose constraint"); #endif diff --git a/gtsam/nonlinear/NonlinearOptimization.h b/gtsam/nonlinear/NonlinearOptimization.h index b563796eb..6521ded8e 100644 --- a/gtsam/nonlinear/NonlinearOptimization.h +++ b/gtsam/nonlinear/NonlinearOptimization.h @@ -44,8 +44,8 @@ namespace gtsam { /** * optimization that returns the values */ - template - T optimize(const G& graph, const T& initialEstimate, + template + Values optimize(const G& graph, const Values& initialEstimate, const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(), const LinearSolver& solver = MULTIFRONTAL, const NonlinearOptimizationMethod& nonlinear_method = LM); diff --git a/gtsam/nonlinear/NonlinearOptimizationParameters.cpp b/gtsam/nonlinear/NonlinearOptimizationParameters.cpp index 4da8fc7a5..e80c3b59c 100644 --- a/gtsam/nonlinear/NonlinearOptimizationParameters.cpp +++ b/gtsam/nonlinear/NonlinearOptimizationParameters.cpp @@ -102,7 +102,7 @@ NonlinearOptimizationParameters::newLambda(double lambda) { /* ************************************************************************* */ NonlinearOptimizationParameters::shared_ptr -NonlinearOptimizationParameters::newDrecreaseThresholds(double absDecrease, +NonlinearOptimizationParameters::newDecreaseThresholds(double absDecrease, double relDecrease) { shared_ptr ptr(boost::make_shared()); ptr->absDecrease_ = absDecrease; diff --git a/gtsam/nonlinear/NonlinearOptimizationParameters.h b/gtsam/nonlinear/NonlinearOptimizationParameters.h index 68595b711..07e01ea58 100644 --- a/gtsam/nonlinear/NonlinearOptimizationParameters.h +++ b/gtsam/nonlinear/NonlinearOptimizationParameters.h @@ -110,7 +110,7 @@ namespace gtsam { static shared_ptr newLambda(double lambda); /// a copy of old instance except new thresholds - static shared_ptr newDrecreaseThresholds(double absDecrease, + static shared_ptr newDecreaseThresholds(double absDecrease, double relDecrease); /// a copy of old instance except new QR flag diff --git a/gtsam/nonlinear/NonlinearOptimizer-inl.h b/gtsam/nonlinear/NonlinearOptimizer-inl.h index a98f4b455..f788eac2a 100644 --- a/gtsam/nonlinear/NonlinearOptimizer-inl.h +++ b/gtsam/nonlinear/NonlinearOptimizer-inl.h @@ -30,8 +30,8 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ - template - NonlinearOptimizer::NonlinearOptimizer(shared_graph graph, + template + NonlinearOptimizer::NonlinearOptimizer(shared_graph graph, shared_values values, shared_ordering ordering, shared_parameters parameters) : graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering), parameters_(parameters), iterations_(0), @@ -47,8 +47,8 @@ namespace gtsam { /* ************************************************************************* */ // FIXME: remove this constructor - template - NonlinearOptimizer::NonlinearOptimizer(shared_graph graph, + template + NonlinearOptimizer::NonlinearOptimizer(shared_graph graph, shared_values values, shared_ordering ordering, shared_solver spcg_solver, shared_parameters parameters) : graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering), @@ -66,8 +66,8 @@ namespace gtsam { /* ************************************************************************* */ // One iteration of Gauss Newton /* ************************************************************************* */ - template - NonlinearOptimizer NonlinearOptimizer::iterate() const { + template + NonlinearOptimizer NonlinearOptimizer::iterate() const { Parameters::verbosityLevel verbosity = parameters_->verbosity_ ; @@ -86,7 +86,7 @@ namespace gtsam { if (verbosity >= Parameters::DELTA) delta.print("delta"); // take old values and update it - shared_values newValues(new C(values_->retract(delta, *ordering_))); + shared_values newValues(new Values(values_->retract(delta, *ordering_))); // maybe show output if (verbosity >= Parameters::VALUES) newValues->print("newValues"); @@ -99,8 +99,8 @@ namespace gtsam { } /* ************************************************************************* */ - template - NonlinearOptimizer NonlinearOptimizer::gaussNewton() const { + template + NonlinearOptimizer NonlinearOptimizer::gaussNewton() const { static W writer(error_); if (error_ < parameters_->sumError_ ) { @@ -130,8 +130,8 @@ namespace gtsam { // TODO: in theory we can't infinitely recurse, but maybe we should put a max. // Reminder: the parameters are Graph type $G$, Values class type $T$, // linear system class $L$, the non linear solver type $S$, and the writer type $W$ - template - NonlinearOptimizer NonlinearOptimizer::try_lambda(const L& linearSystem) { + template + NonlinearOptimizer NonlinearOptimizer::try_lambda(const L& linearSystem) { const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ; const Parameters::LambdaMode lambdaMode = parameters_->lambdaMode_ ; @@ -178,7 +178,7 @@ namespace gtsam { if (verbosity >= Parameters::TRYDELTA) delta.print("delta"); // update values - shared_values newValues(new T(values_->retract(delta, *ordering_))); + shared_values newValues(new Values(values_->retract(delta, *ordering_))); // create new optimization state with more adventurous lambda double error = graph_->error(*newValues); @@ -228,8 +228,8 @@ namespace gtsam { // One iteration of Levenberg Marquardt // Reminder: the parameters are Graph type $G$, Values class type $T$, // linear system class $L$, the non linear solver type $S$, and the writer type $W$ - template - NonlinearOptimizer NonlinearOptimizer::iterateLM() { + template + NonlinearOptimizer NonlinearOptimizer::iterateLM() { const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ; const double lambda = parameters_->lambda_ ; @@ -253,8 +253,8 @@ namespace gtsam { /* ************************************************************************* */ // Reminder: the parameters are Graph type $G$, Values class type $T$, // linear system class $L$, the non linear solver type $S$, and the writer type $W$ - template - NonlinearOptimizer NonlinearOptimizer::levenbergMarquardt() { + template + NonlinearOptimizer NonlinearOptimizer::levenbergMarquardt() { // Initialize bool converged = false; @@ -299,20 +299,20 @@ namespace gtsam { } /* ************************************************************************* */ - template - NonlinearOptimizer NonlinearOptimizer::iterateDogLeg() { + template + NonlinearOptimizer NonlinearOptimizer::iterateDogLeg() { S solver(*graph_->linearize(*values_, *ordering_), parameters_->useQR_); DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate( parameters_->lambda_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *solver.eliminate(), *graph_, *values_, *ordering_, error_, parameters_->verbosity_ > Parameters::ERROR); - shared_values newValues(new T(values_->retract(result.dx_d, *ordering_))); + shared_values newValues(new Values(values_->retract(result.dx_d, *ordering_))); return newValuesErrorLambda_(newValues, result.f_error, result.Delta); } /* ************************************************************************* */ - template - NonlinearOptimizer NonlinearOptimizer::dogLeg() { + template + NonlinearOptimizer NonlinearOptimizer::dogLeg() { static W writer(error_); // check if we're already close enough diff --git a/gtsam/nonlinear/NonlinearOptimizer.h b/gtsam/nonlinear/NonlinearOptimizer.h index 00647b2d8..4e41649e6 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.h +++ b/gtsam/nonlinear/NonlinearOptimizer.h @@ -37,20 +37,20 @@ public: * and then one of the optimization routines is called. These iterate * until convergence. All methods are functional and return a new state. * - * The class is parameterized by the Graph type $G$, Values class type $T$, + * The class is parameterized by the Graph type $G$, Values class type $Values$, * linear system class $L$, the non linear solver type $S$, and the writer type $W$ * - * The values class type $T$ is in order to be able to optimize over non-vector values structures. + * The values class type $Values$ is in order to be able to optimize over non-vector values structures. * * A nonlinear system solver $S$ - * Concept NonLinearSolver implements - * linearize: G * T -> L - * solve : L -> T + * Concept NonLinearSolver implements + * linearize: G * Values -> L + * solve : L -> Values * * The writer $W$ generates output to disk or the screen. * - * For example, in a 2D case, $G$ can be Pose2Graph, $T$ can be Pose2Values, - * $L$ can be GaussianFactorGraph and $S$ can be Factorization. + * For example, in a 2D case, $G$ can be pose2SLAM::Graph, $Values$ can be Pose2Values, + * $L$ can be GaussianFactorGraph and $S$ can be Factorization. * The solver class has two main functions: linearize and optimize. The first one * linearizes the nonlinear cost function around the current estimate, and the second * one optimizes the linearized system using various methods. @@ -58,12 +58,12 @@ public: * To use the optimizer in code, include in your cpp file * \nosubgrouping */ -template +template class NonlinearOptimizer { public: // For performance reasons in recursion, we store values in a shared_ptr - typedef boost::shared_ptr shared_values; ///Prevent memory leaks in Values + typedef boost::shared_ptr shared_values; ///Prevent memory leaks in Values typedef boost::shared_ptr shared_graph; /// Prevent memory leaks in Graph typedef boost::shared_ptr shared_linear; /// Not sure typedef boost::shared_ptr shared_ordering; ///ordering parameters @@ -74,7 +74,7 @@ public: private: - typedef NonlinearOptimizer This; + typedef NonlinearOptimizer This; typedef boost::shared_ptr > shared_dimensions; /// keep a reference to const version of the graph @@ -182,7 +182,7 @@ public: /** * Copy constructor */ - NonlinearOptimizer(const NonlinearOptimizer &optimizer) : + NonlinearOptimizer(const NonlinearOptimizer &optimizer) : graph_(optimizer.graph_), values_(optimizer.values_), error_(optimizer.error_), ordering_(optimizer.ordering_), parameters_(optimizer.parameters_), iterations_(optimizer.iterations_), dimensions_(optimizer.dimensions_), structure_(optimizer.structure_) {} @@ -242,13 +242,13 @@ public: /** * Return mean and covariance on a single variable */ - Matrix marginalCovariance(Symbol j) const { + Matrix marginalCovariance(Key j) const { return createSolver()->marginalCovariance((*ordering_)[j]); } /** * linearize and optimize - * This returns an VectorValues, i.e., vectors in tangent space of T + * This returns an VectorValues, i.e., vectors in tangent space of Values */ VectorValues linearizeAndOptimizeForDelta() const { return *createSolver()->optimize(); @@ -340,19 +340,19 @@ public: * Static interface to LM optimization (no shared_ptr arguments) - see above */ static shared_values optimizeLM(const G& graph, - const T& values, + const Values& values, const Parameters parameters = Parameters()) { return optimizeLM(boost::make_shared(graph), - boost::make_shared(values), + boost::make_shared(values), boost::make_shared(parameters)); } ///TODO: comment static shared_values optimizeLM(const G& graph, - const T& values, + const Values& values, Parameters::verbosityLevel verbosity) { return optimizeLM(boost::make_shared(graph), - boost::make_shared(values), + boost::make_shared(values), verbosity); } @@ -392,19 +392,19 @@ public: * Static interface to Dogleg optimization (no shared_ptr arguments) - see above */ static shared_values optimizeDogLeg(const G& graph, - const T& values, + const Values& values, const Parameters parameters = Parameters()) { return optimizeDogLeg(boost::make_shared(graph), - boost::make_shared(values), + boost::make_shared(values), boost::make_shared(parameters)); } ///TODO: comment static shared_values optimizeDogLeg(const G& graph, - const T& values, + const Values& values, Parameters::verbosityLevel verbosity) { return optimizeDogLeg(boost::make_shared(graph), - boost::make_shared(values), + boost::make_shared(values), verbosity); } @@ -431,9 +431,9 @@ public: /** * Static interface to GN optimization (no shared_ptr arguments) - see above */ - static shared_values optimizeGN(const G& graph, const T& values, const Parameters parameters = Parameters()) { + static shared_values optimizeGN(const G& graph, const Values& values, const Parameters parameters = Parameters()) { return optimizeGN(boost::make_shared(graph), - boost::make_shared(values), + boost::make_shared(values), boost::make_shared(parameters)); } }; diff --git a/gtsam/nonlinear/Ordering.cpp b/gtsam/nonlinear/Ordering.cpp index d8b5eac7e..10c3370cf 100644 --- a/gtsam/nonlinear/Ordering.cpp +++ b/gtsam/nonlinear/Ordering.cpp @@ -26,9 +26,9 @@ using namespace std; namespace gtsam { /* ************************************************************************* */ -Ordering::Ordering(const std::list & L):nVars_(0) { +Ordering::Ordering(const std::list & L):nVars_(0) { int i = 0; - BOOST_FOREACH( const Symbol& s, L ) + BOOST_FOREACH( Key s, L ) insert(s, i++) ; } @@ -40,12 +40,12 @@ void Ordering::permuteWithInverse(const Permutation& inversePermutation) { } /* ************************************************************************* */ -void Ordering::print(const string& str) const { +void Ordering::print(const string& str, const KeyFormatter& keyFormatter) const { cout << str << " "; BOOST_FOREACH(const Ordering::value_type& key_order, *this) { if(key_order != *begin()) cout << ", "; - cout << (string)key_order.first << ":" << key_order.second; + cout << keyFormatter(key_order.first) << ":" << key_order.second; } cout << endl; } @@ -70,7 +70,7 @@ Ordering::value_type Ordering::pop_back() { } /* ************************************************************************* */ -Index Ordering::pop_back(const Symbol& key) { +Index Ordering::pop_back(Key key) { Map::iterator item = order_.find(key); if(item == order_.end()) { throw invalid_argument("Attempting to remove a key from an ordering that does not contain that key"); diff --git a/gtsam/nonlinear/Ordering.h b/gtsam/nonlinear/Ordering.h index 4d10b15b5..fcb3d5bfb 100644 --- a/gtsam/nonlinear/Ordering.h +++ b/gtsam/nonlinear/Ordering.h @@ -17,10 +17,10 @@ #pragma once -#include #include -#include +#include #include +#include #include #include @@ -34,8 +34,7 @@ namespace gtsam { */ class Ordering { protected: - typedef boost::fast_pool_allocator > Allocator; - typedef std::map, Allocator> Map; + typedef FastMap Map; Map order_; Index nVars_; @@ -43,7 +42,7 @@ public: typedef boost::shared_ptr shared_ptr; - typedef std::pair value_type; + typedef std::pair value_type; typedef Map::iterator iterator; typedef Map::const_iterator const_iterator; @@ -54,7 +53,7 @@ public: Ordering() : nVars_(0) {} /// Construct from list, assigns order indices sequentially to list items. - Ordering(const std::list & L) ; + Ordering(const std::list & L) ; /// @} /// @name Standard Interface @@ -69,7 +68,7 @@ public: const_iterator begin() const { return order_.begin(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */ const_iterator end() const { return order_.end(); } /**< Iterator in order of sorted symbols, not in elimination/index order! */ - Index at(const Symbol& key) const { return operator[](key); } ///< Synonym for operator[](const Symbol&) const + Index at(Key key) const { return operator[](key); } ///< Synonym for operator[](Key) const /** Assigns the ordering index of the requested \c key into \c index if the symbol * is present in the ordering, otherwise does not modify \c index. The @@ -79,7 +78,7 @@ public: * @param [out] index Reference into which to write the index of the requested key, if the key is present. * @return true if the key is present and \c index was modified, false otherwise. */ - bool tryAt(const Symbol& key, Index& index) const { + bool tryAt(Key key, Index& index) const { const_iterator i = order_.find(key); if(i != order_.end()) { index = i->second; @@ -91,7 +90,7 @@ public: /// Access the index for the requested key, throws std::out_of_range if the /// key is not present in the ordering (note that this differs from the /// behavior of std::map) - Index& operator[](const Symbol& key) { + Index& operator[](Key key) { iterator i=order_.find(key); if(i == order_.end()) throw std::out_of_range(std::string()); else return i->second; } @@ -99,7 +98,7 @@ public: /// Access the index for the requested key, throws std::out_of_range if the /// key is not present in the ordering (note that this differs from the /// behavior of std::map) - Index operator[](const Symbol& key) const { + Index operator[](Key key) const { const_iterator i=order_.find(key); if(i == order_.end()) throw std::out_of_range(std::string()); else return i->second; } @@ -110,7 +109,7 @@ public: * @return An iterator pointing to the symbol/index pair with the requested, * or the end iterator if it does not exist. */ - iterator find(const Symbol& key) { return order_.find(key); } + iterator find(Key key) { return order_.find(key); } /** Returns an iterator pointing to the symbol/index pair with the requested, * or the end iterator if it does not exist. @@ -118,7 +117,7 @@ public: * @return An iterator pointing to the symbol/index pair with the requested, * or the end iterator if it does not exist. */ - const_iterator find(const Symbol& key) const { return order_.find(key); } + const_iterator find(Key key) const { return order_.find(key); } /** * Attempts to insert a symbol/order pair with same semantics as stl::Map::insert(), @@ -153,22 +152,22 @@ public: iterator end() { return order_.end(); } /// Test if the key exists in the ordering. - bool exists(const Symbol& key) const { return order_.count(key); } + bool exists(Key key) const { return order_.count(key); } ///TODO: comment - std::pair tryInsert(const Symbol& key, Index order) { return tryInsert(std::make_pair(key,order)); } + std::pair tryInsert(Key key, Index order) { return tryInsert(std::make_pair(key,order)); } ///TODO: comment - iterator insert(const Symbol& key, Index order) { return insert(std::make_pair(key,order)); } + iterator insert(Key key, Index order) { return insert(std::make_pair(key,order)); } /// Adds a new key to the ordering with an index of one greater than the current highest index. - Index push_back(const Symbol& key) { return insert(std::make_pair(key, nVars_))->second; } + Index push_back(Key key) { return insert(std::make_pair(key, nVars_))->second; } /** Remove the last (last-ordered, not highest-sorting key) symbol/index pair * from the ordering (this version is \f$ O(n) \f$, use it when you do not * know the last-ordered key). * - * If you already know the last-ordered symbol, call popback(const Symbol&) + * If you already know the last-ordered symbol, call popback(Key) * that accepts this symbol as an argument. * * @return The symbol and index that were removed. @@ -183,15 +182,15 @@ public: * * @return The index of the symbol that was removed. */ - Index pop_back(const Symbol& key); + Index pop_back(Key key); /** * += operator allows statements like 'ordering += x0,x1,x2,x3;', which are * very useful for unit tests. This functionality is courtesy of * boost::assign. */ - inline boost::assign::list_inserter, Symbol> - operator+=(const Symbol& key) { + inline boost::assign::list_inserter, Key> + operator+=(Key key) { return boost::assign::make_list_inserter(boost::assign_detail::call_push_back(*this))(key); } /** @@ -201,15 +200,15 @@ public: */ void permuteWithInverse(const Permutation& inversePermutation); - /// Synonym for operator[](const Symbol&) - Index& at(const Symbol& key) { return operator[](key); } + /// Synonym for operator[](Key) + Index& at(Key key) { return operator[](key); } /// @} /// @name Testable /// @{ /** print (from Testable) for testing and debugging */ - void print(const std::string& str = "Ordering:") const; + void print(const std::string& str = "Ordering:", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; /** equals (from Testable) for testing and debugging */ bool equals(const Ordering& rhs, double tol = 0.0) const; diff --git a/gtsam/nonlinear/Symbol.h b/gtsam/nonlinear/Symbol.h new file mode 100644 index 000000000..79d6a049b --- /dev/null +++ b/gtsam/nonlinear/Symbol.h @@ -0,0 +1,128 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Symbol.h + * @date Jan 12, 2010 + * @author: Frank Dellaert + * @author: Richard Roberts + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include + +namespace gtsam { + +/** + * Character and index key used in VectorValues, GaussianFactorGraph, + * GaussianFactor, etc. These keys are generated at runtime from TypedSymbol + * keys when linearizing a nonlinear factor graph. This key is not type + * safe, so cannot be used with any Nonlinear* classes. + */ +class Symbol { +protected: + unsigned char c_; + size_t j_; + +public: + /** Default constructor */ + Symbol() : + c_(0), j_(0) { + } + + /** Copy constructor */ + Symbol(const Symbol& key) : + c_(key.c_), j_(key.j_) { + } + + /** Constructor */ + Symbol(unsigned char c, size_t j) : + c_(c), j_(j) { + } + + /** Constructor that decodes an integer Key */ + Symbol(Key key) { + const size_t keyBytes = sizeof(Key); + const size_t chrBytes = sizeof(unsigned char); + const size_t indexBytes = keyBytes - chrBytes; + const Key chrMask = std::numeric_limits::max() << indexBytes; + const Key indexMask = ~chrMask; + c_ = (key & chrMask) >> indexBytes; + j_ = key & indexMask; + } + + /** Cast to integer */ + operator Key() const { + const size_t keyBytes = sizeof(Key); + const size_t chrBytes = sizeof(unsigned char); + const size_t indexBytes = keyBytes - chrBytes; + const Key chrMask = std::numeric_limits::max() << indexBytes; + const Key indexMask = ~chrMask; + if(j_ > indexMask) + throw std::invalid_argument("Symbol index is too large"); + Key key = (c_ << indexBytes) | j_; + return key; + } + + // Testable Requirements + void print(const std::string& s = "") const { + std::cout << s << ": " << (std::string) (*this) << std::endl; + } + bool equals(const Symbol& expected, double tol = 0.0) const { + return (*this) == expected; + } + + /** Retrieve key character */ + unsigned char chr() const { + return c_; + } + + /** Retrieve key index */ + size_t index() const { + return j_; + } + + /** Create a string from the key */ + operator std::string() const { + return str(boost::format("%c%d") % c_ % j_); + } + + /** Comparison for use in maps */ + bool operator<(const Symbol& comp) const { + return c_ < comp.c_ || (comp.c_ == c_ && j_ < comp.j_); + } + bool operator==(const Symbol& comp) const { + return comp.c_ == c_ && comp.j_ == j_; + } + bool operator!=(const Symbol& comp) const { + return comp.c_ != c_ || comp.j_ != j_; + } + +private: + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(c_); + ar & BOOST_SERIALIZATION_NVP(j_); + } +}; + +} // namespace gtsam + diff --git a/gtsam/nonlinear/TupleValues-inl.h b/gtsam/nonlinear/TupleValues-inl.h deleted file mode 100644 index a9d6300c1..000000000 --- a/gtsam/nonlinear/TupleValues-inl.h +++ /dev/null @@ -1,165 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file TupleValues-inl.h - * @author Richard Roberts - * @author Manohar Paluri - * @author Alex Cunningham - */ - -#pragma once - -namespace gtsam { - -/* ************************************************************************* */ -/** TupleValuesN Implementations */ -/* ************************************************************************* */ - -/* ************************************************************************* */ -/** TupleValues 1 */ -template -TupleValues1::TupleValues1(const TupleValues1& values) : - TupleValuesEnd (values) {} - -template -TupleValues1::TupleValues1(const VALUES1& cfg1) : - TupleValuesEnd (cfg1) {} - -template -TupleValues1::TupleValues1(const TupleValuesEnd& values) : - TupleValuesEnd(values) {} - -/* ************************************************************************* */ -/** TupleValues 2 */ -template -TupleValues2::TupleValues2(const TupleValues2& values) : - TupleValues >(values) {} - -template -TupleValues2::TupleValues2(const VALUES1& cfg1, const VALUES2& cfg2) : - TupleValues >( - cfg1, TupleValuesEnd(cfg2)) {} - -template -TupleValues2::TupleValues2(const TupleValues >& values) : - TupleValues >(values) {} - -/* ************************************************************************* */ -/** TupleValues 3 */ -template -TupleValues3::TupleValues3( - const TupleValues3& values) : - TupleValues > >(values) {} - -template -TupleValues3::TupleValues3( - const VALUES1& cfg1, const VALUES2& cfg2, const VALUES3& cfg3) : - TupleValues > >( - cfg1, TupleValues >( - cfg2, TupleValuesEnd(cfg3))) {} - -template -TupleValues3::TupleValues3( - const TupleValues > >& values) : - TupleValues > >(values) {} - -/* ************************************************************************* */ -/** TupleValues 4 */ -template -TupleValues4::TupleValues4( - const TupleValues4& values) : - TupleValues > > >(values) {} - -template -TupleValues4::TupleValues4( - const VALUES1& cfg1, const VALUES2& cfg2, - const VALUES3& cfg3,const VALUES4& cfg4) : - TupleValues > > >( - cfg1, TupleValues > >( - cfg2, TupleValues >( - cfg3, TupleValuesEnd(cfg4)))) {} - -template -TupleValues4::TupleValues4( - const TupleValues > > >& values) : - TupleValues > > >(values) {} - -/* ************************************************************************* */ -/** TupleValues 5 */ -template -TupleValues5::TupleValues5( - const TupleValues5& values) : - TupleValues > > > >(values) {} - -template -TupleValues5::TupleValues5( - const VALUES1& cfg1, const VALUES2& cfg2, const VALUES3& cfg3, - const VALUES4& cfg4, const VALUES5& cfg5) : - TupleValues > > > >( - cfg1, TupleValues > > >( - cfg2, TupleValues > >( - cfg3, TupleValues >( - cfg4, TupleValuesEnd(cfg5))))) {} - -template -TupleValues5::TupleValues5( - const TupleValues > > > >& values) : - TupleValues > > > >(values) {} - -/* ************************************************************************* */ -/** TupleValues 6 */ -template -TupleValues6::TupleValues6( - const TupleValues6& values) : - TupleValues > > > > >(values) {} - -template -TupleValues6::TupleValues6( - const VALUES1& cfg1, const VALUES2& cfg2, const VALUES3& cfg3, - const VALUES4& cfg4, const VALUES5& cfg5, const VALUES6& cfg6) : - TupleValues > > > > >( - cfg1, TupleValues > > > >( - cfg2, TupleValues > > >( - cfg3, TupleValues > >( - cfg4, TupleValues >( - cfg5, TupleValuesEnd(cfg6)))))) {} - -template -TupleValues6::TupleValues6( - const TupleValues > > > > >& values) : - TupleValues > > > > >(values) {} - -} diff --git a/gtsam/nonlinear/TupleValues.h b/gtsam/nonlinear/TupleValues.h deleted file mode 100644 index df3eab51b..000000000 --- a/gtsam/nonlinear/TupleValues.h +++ /dev/null @@ -1,550 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file TupleValues.h - * @author Richard Roberts - * @author Manohar Paluri - * @author Alex Cunningham - */ - -#include -#include - -#pragma once - -namespace gtsam { - - /** - * TupleValues are a structure to manage heterogenous Values, so as to - * enable different types of variables/keys to be used simultaneously. We - * do this with recursive templates (instead of blind pointer casting) to - * reduce run-time overhead and keep static type checking. The interface - * mimics that of a single Values. - * - * This uses a recursive structure of values pairs to form a lisp-like - * list, with a special case (TupleValuesEnd) that contains only one values - * at the end. Because this recursion is done at compile time, there is no - * runtime performance hit to using this structure. - * - * For an easy interface, there are TupleValuesN classes, which wrap - * the recursive TupleValues together as a single class, so you can have - * mixed-type classes from 2-6 types. Note that a TupleValues2 is equivalent - * to the previously used PairValues. - * - * Design and extension note: - * To implement a recursively templated data structure, note that most operations - * have two versions: one with templates and one without. The templated one allows - * for the arguments to be passed to the next values, while the specialized one - * operates on the "first" values. TupleValuesEnd contains only the specialized version. - * \nosubgrouping - */ - template - class TupleValues { - - protected: - // Data for internal valuess - VALUES1 first_; /// Arbitrary values - VALUES2 second_; /// A TupleValues or TupleValuesEnd, which wraps an arbitrary values - - /** concept checks */ - GTSAM_CONCEPT_TESTABLE_TYPE(VALUES1) - GTSAM_CONCEPT_TESTABLE_TYPE(VALUES2) - - public: - // typedefs for values subtypes - typedef typename VALUES1::Key Key1; - typedef typename VALUES1::Value Value1; - - /// @name Standard Constructors - /// @{ - - /** default constructor */ - TupleValues() {} - - /** Copy constructor */ - TupleValues(const TupleValues& values) : - first_(values.first_), second_(values.second_) {} - - /** Construct from valuess */ - TupleValues(const VALUES1& cfg1, const VALUES2& cfg2) : - first_(cfg1), second_(cfg2) {} - - /// @} - /// @name Testable - /// @{ - - /** Print */ - void print(const std::string& s = "") const { - first_.print(s); - second_.print(); - } - - /** Equality with tolerance for keys and values */ - bool equals(const TupleValues& c, double tol=1e-9) const { - return first_.equals(c.first_, tol) && second_.equals(c.second_, tol); - } - - /// @} - /// @name Advanced Interface - /// @{ - - /** - * Insert a key/value pair to the values. - * Note: if the key is already in the values, the values will not be changed. - * Use update() to allow for changing existing values. - * @param key is the key - can be an int (second version) if the can can be initialized from an int - * @param value is the value to insert - */ - template - void insert(const KEY& key, const VALUE& value) {second_.insert(key, value);} - void insert(int key, const Value1& value) {first_.insert(Key1(key), value);} /// - void insert(const TupleValues& values) { second_.insert(values); } - void insert(const TupleValues& values) { /// - void update(const TupleValues& values) { second_.update(values); } - void update(const TupleValues& values) { /// - void update(const KEY& key, const VALUE& value) { second_.update(key, value); } - void update(const Key1& key, const Value1& value) { first_.update(key, value); } /// - void insertSub(const CFG& values) { second_.insertSub(values); } - void insertSub(const VALUES1& values) { first_.insert(values); } /// - void erase(const KEY& j) { second_.erase(j); } - void erase(const Key1& j) { first_.erase(j); } /// - bool exists(const KEY& j) const { return second_.exists(j); } - bool exists(const Key1& j) const { return first_.exists(j); } /// - boost::optional exists_(const KEY& j) const { return second_.exists_(j); } - boost::optional exists_(const Key1& j) const { return first_.exists_(j); } /// - const typename KEY::Value & operator[](const KEY& j) const { return second_[j]; } - const Value1& operator[](const Key1& j) const { return first_[j]; } /// - const typename KEY::Value & at(const KEY& j) const { return second_.at(j); } - const Value1& at(const Key1& j) const { return first_.at(j); } ///dims(ordering)); - } - - /** @return number of key/value pairs stored */ - size_t size() const { return first_.size() + second_.size(); } - - /** @return true if values is empty */ - bool empty() const { return first_.empty() && second_.empty(); } - - /** - * Generate a default ordering, simply in key sort order. To instead - * create a fill-reducing ordering, use - * NonlinearFactorGraph::orderingCOLAMD(). Alternatively, you may permute - * this ordering yourself (as orderingCOLAMD() does internally). - */ - Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const { - // Generate an initial key ordering in iterator order - _ValuesKeyOrderer keyOrderer(firstVar); - this->apply(keyOrderer); - return keyOrderer.ordering; - } - - /** - * Apply a class with an application operator() to a const_iterator over - * every pair. The operator must be able to handle such an - * iterator for every type in the Values, (i.e. through templating). - */ - template - void apply(A& operation) { - first_.apply(operation); - second_.apply(operation); - } - - /** - * TODO: comment - */ - template - void apply(A& operation) const { - first_.apply(operation); - second_.apply(operation); - } - - /// @} - /// @name Manifold - /// @{ - - /** @return The dimensionality of the tangent space */ - size_t dim() const { return first_.dim() + second_.dim(); } - - /** Create an array of variable dimensions using the given ordering */ - std::vector dims(const Ordering& ordering) const { - _ValuesDimensionCollector dimCollector(ordering); - this->apply(dimCollector); - return dimCollector.dimensions; - } - - /** Expmap */ - TupleValues retract(const VectorValues& delta, const Ordering& ordering) const { - return TupleValues(first_.retract(delta, ordering), second_.retract(delta, ordering)); - } - - /** logmap each element */ - VectorValues localCoordinates(const TupleValues& cp, const Ordering& ordering) const { - VectorValues delta(this->dims(ordering)); - localCoordinates(cp, ordering, delta); - return delta; - } - - /** logmap each element */ - void localCoordinates(const TupleValues& cp, const Ordering& ordering, VectorValues& delta) const { - first_.localCoordinates(cp.first_, ordering, delta); - second_.localCoordinates(cp.second_, ordering, delta); - } - - /// @} - - private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(first_); - ar & BOOST_SERIALIZATION_NVP(second_); - } - - }; - - /** - * End of a recursive TupleValues - contains only one values - * - * Do not use this class directly - it should only be used as a part - * of a recursive structure - */ - template - class TupleValuesEnd { - - protected: - // Data for internal valuess - VALUES first_; - - public: - // typedefs - typedef typename VALUES::Key Key1; - typedef typename VALUES::Value Value1; - - TupleValuesEnd() {} - - TupleValuesEnd(const TupleValuesEnd& values) : - first_(values.first_) {} - - TupleValuesEnd(const VALUES& cfg) : - first_(cfg) {} - - void print(const std::string& s = "") const { - first_.print(); - } - - bool equals(const TupleValuesEnd& c, double tol=1e-9) const { - return first_.equals(c.first_, tol); - } - - void insert(const Key1& key, const Value1& value) {first_.insert(key, value); } - void insert(int key, const Value1& value) {first_.insert(Key1(key), value);} - - void insert(const TupleValuesEnd& values) {first_.insert(values.first_); } - - void update(const TupleValuesEnd& values) {first_.update(values.first_); } - - void update(const Key1& key, const Value1& value) { first_.update(key, value); } - - void insertSub(const VALUES& values) {first_.insert(values); } - - const Value1& operator[](const Key1& j) const { return first_[j]; } - - const VALUES& values() const { return first_; } - - void erase(const Key1& j) { first_.erase(j); } - - void clear() { first_.clear(); } - - bool empty() const { return first_.empty(); } - - bool exists(const Key1& j) const { return first_.exists(j); } - - boost::optional exists_(const Key1& j) const { return first_.exists_(j); } - - const Value1& at(const Key1& j) const { return first_.at(j); } - - VectorValues zero(const Ordering& ordering) const { - return VectorValues::Zero(this->dims(ordering)); - } - - size_t size() const { return first_.size(); } - - size_t dim() const { return first_.dim(); } - - TupleValuesEnd retract(const VectorValues& delta, const Ordering& ordering) const { - return TupleValuesEnd(first_.retract(delta, ordering)); - } - - VectorValues localCoordinates(const TupleValuesEnd& cp, const Ordering& ordering) const { - VectorValues delta(this->dims(ordering)); - localCoordinates(cp, ordering, delta); - return delta; - } - - void localCoordinates(const TupleValuesEnd& cp, const Ordering& ordering, VectorValues& delta) const { - first_.localCoordinates(cp.first_, ordering, delta); - } - - /** - * Apply a class with an application operator() to a const_iterator over - * every pair. The operator must be able to handle such an - * iterator for every type in the Values, (i.e. through templating). - */ - template - void apply(A& operation) { - first_.apply(operation); - } - template - void apply(A& operation) const { - first_.apply(operation); - } - - /** Create an array of variable dimensions using the given ordering */ - std::vector dims(const Ordering& ordering) const { - _ValuesDimensionCollector dimCollector(ordering); - this->apply(dimCollector); - return dimCollector.dimensions; - } - - /** - * Generate a default ordering, simply in key sort order. To instead - * create a fill-reducing ordering, use - * NonlinearFactorGraph::orderingCOLAMD(). Alternatively, you may permute - * this ordering yourself (as orderingCOLAMD() does internally). - */ - Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const { - // Generate an initial key ordering in iterator order - _ValuesKeyOrderer keyOrderer(firstVar); - this->apply(keyOrderer); - return keyOrderer.ordering; - } - - private: - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(first_); - } - }; - - /** - * Wrapper classes to act as containers for valuess. Note that these can be cascaded - * recursively, as they are TupleValues, and are primarily a short form of the values - * structure to make use of the TupleValues easier. - * - * The interface is designed to mimic PairValues, but for 2-6 values types. - */ - - template - class TupleValues1 : public TupleValuesEnd { - public: - // typedefs - typedef C1 Values1; - - typedef TupleValuesEnd Base; - typedef TupleValues1 This; - - TupleValues1() {} - TupleValues1(const This& values); - TupleValues1(const Base& values); - TupleValues1(const Values1& cfg1); - - // access functions - inline const Values1& first() const { return this->values(); } - }; - - template - class TupleValues2 : public TupleValues > { - public: - // typedefs - typedef C1 Values1; - typedef C2 Values2; - - typedef TupleValues > Base; - typedef TupleValues2 This; - - TupleValues2() {} - TupleValues2(const This& values); - TupleValues2(const Base& values); - TupleValues2(const Values1& cfg1, const Values2& cfg2); - - // access functions - inline const Values1& first() const { return this->values(); } - inline const Values2& second() const { return this->rest().values(); } - }; - - template - class TupleValues3 : public TupleValues > > { - public: - // typedefs - typedef C1 Values1; - typedef C2 Values2; - typedef C3 Values3; - - typedef TupleValues > > Base; - typedef TupleValues3 This; - - TupleValues3() {} - TupleValues3(const Base& values); - TupleValues3(const This& values); - TupleValues3(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3); - - // access functions - inline const Values1& first() const { return this->values(); } - inline const Values2& second() const { return this->rest().values(); } - inline const Values3& third() const { return this->rest().rest().values(); } - }; - - template - class TupleValues4 : public TupleValues > > > { - public: - // typedefs - typedef C1 Values1; - typedef C2 Values2; - typedef C3 Values3; - typedef C4 Values4; - - typedef TupleValues > > > Base; - typedef TupleValues4 This; - - TupleValues4() {} - TupleValues4(const This& values); - TupleValues4(const Base& values); - TupleValues4(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3,const Values4& cfg4); - - // access functions - inline const Values1& first() const { return this->values(); } - inline const Values2& second() const { return this->rest().values(); } - inline const Values3& third() const { return this->rest().rest().values(); } - inline const Values4& fourth() const { return this->rest().rest().rest().values(); } - }; - - template - class TupleValues5 : public TupleValues > > > > { - public: - typedef C1 Values1; - typedef C2 Values2; - typedef C3 Values3; - typedef C4 Values4; - typedef C5 Values5; - - typedef TupleValues > > > > Base; - typedef TupleValues5 This; - - TupleValues5() {} - TupleValues5(const This& values); - TupleValues5(const Base& values); - TupleValues5(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3, - const Values4& cfg4, const Values5& cfg5); - - // access functions - inline const Values1& first() const { return this->values(); } - inline const Values2& second() const { return this->rest().values(); } - inline const Values3& third() const { return this->rest().rest().values(); } - inline const Values4& fourth() const { return this->rest().rest().rest().values(); } - inline const Values5& fifth() const { return this->rest().rest().rest().rest().values(); } - }; - - template - class TupleValues6 : public TupleValues > > > > > { - public: - typedef C1 Values1; - typedef C2 Values2; - typedef C3 Values3; - typedef C4 Values4; - typedef C5 Values5; - typedef C6 Values6; - - typedef TupleValues > > > > > Base; - typedef TupleValues6 This; - - TupleValues6() {} - TupleValues6(const This& values); - TupleValues6(const Base& values); - TupleValues6(const Values1& cfg1, const Values2& cfg2, const Values3& cfg3, - const Values4& cfg4, const Values5& cfg5, const Values6& cfg6); - // access functions - inline const Values1& first() const { return this->values(); } - inline const Values2& second() const { return this->rest().values(); } - inline const Values3& third() const { return this->rest().rest().values(); } - inline const Values4& fourth() const { return this->rest().rest().rest().values(); } - inline const Values5& fifth() const { return this->rest().rest().rest().rest().values(); } - inline const Values6& sixth() const { return this->rest().rest().rest().rest().rest().values(); } - }; - -} - -#include diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 28f08bfb5..dd5b29fbd 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -10,222 +10,116 @@ * -------------------------------------------------------------------------- */ /** - * @file Values.cpp + * @file Values.h * @author Richard Roberts + * + * @brief A non-templated config holding any types of Manifold-group elements + * + * Detailed story: + * A values structure is a map from keys to values. It is used to specify the value of a bunch + * of variables in a factor graph. A Values is a values structure which can hold variables that + * are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type + * which is also a manifold element, and hence supports operations dim, retract, and localCoordinates. */ -#pragma once - #include -#include -#include +#include +#include -#include -#include - -#include -#include -#include -#include - -using namespace std; +#include +#include // Only so Eclipse finds class definition namespace gtsam { -/* ************************************************************************* */ - template - void Values::print(const string &s) const { - cout << "Values " << s << ", size " << values_.size() << "\n"; - BOOST_FOREACH(const KeyValuePair& v, values_) { - gtsam::print(v.second, (string)(v.first)); - } - } - /* ************************************************************************* */ - template - bool Values::equals(const Values& expected, double tol) const { - if (values_.size() != expected.values_.size()) return false; - BOOST_FOREACH(const KeyValuePair& v, values_) { - if (!expected.exists(v.first)) return false; - if(!gtsam::equal(v.second, expected[v.first], tol)) - return false; + class ValueCloneAllocator { + public: + static Value* allocate_clone(const Value& a) { return a.clone_(); } + static void deallocate_clone(const Value* a) { a->deallocate_(); } + private: + ValueCloneAllocator() {} + }; + +#if 0 + /* ************************************************************************* */ + class ValueAutomaticCasting { + Key key_; + const Value& value_; + + public: + ValueAutomaticCasting(Key key, const Value& value) : key_(key), value_(value) {} + + template + class ConvertibleToValue : public ValueType { + }; + + template + operator const ConvertibleToValue& () const { + // Check the type and throw exception if incorrect + if(typeid(ValueType) != typeid(value_)) + throw ValuesIncorrectType(key_, typeid(ValueType), typeid(value_)); + + // We have already checked the type, so do a "blind" static_cast, not dynamic_cast + return static_cast&>(value_); } - return true; + }; +#endif + + /* ************************************************************************* */ + template + const ValueType& Values::at(Key j) const { + // Find the item + KeyValueMap::const_iterator item = values_.find(j); + + // Throw exception if it does not exist + if(item == values_.end()) + throw ValuesKeyDoesNotExist("retrieve", j); + + // Check the type and throw exception if incorrect + if(typeid(*item->second) != typeid(ValueType)) + throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType)); + + // We have already checked the type, so do a "blind" static_cast, not dynamic_cast + return static_cast(*item->second); } +#if 0 /* ************************************************************************* */ - template - const typename J::Value& Values::at(const J& j) const { - const_iterator it = values_.find(j); - if (it == values_.end()) throw KeyDoesNotExist("retrieve", j); - else return it->second; + inline ValueAutomaticCasting Values::at(Key j) const { + // Find the item + KeyValueMap::const_iterator item = values_.find(j); + + // Throw exception if it does not exist + if(item == values_.end()) + throw ValuesKeyDoesNotExist("retrieve", j); + + return ValueAutomaticCasting(item->first, *item->second); } +#endif +#if 0 /* ************************************************************************* */ - template - size_t Values::dim() const { - size_t n = 0; - BOOST_FOREACH(const KeyValuePair& value, values_) - n += value.second.dim(); - return n; + inline ValueAutomaticCasting Values::operator[](Key j) const { + return at(j); } +#endif /* ************************************************************************* */ - template - VectorValues Values::zero(const Ordering& ordering) const { - return VectorValues::Zero(this->dims(ordering)); - } + template + boost::optional Values::exists(Key j) const { + // Find the item + KeyValueMap::const_iterator item = values_.find(j); - /* ************************************************************************* */ - template - void Values::insert(const J& name, const typename J::Value& val) { - if(!values_.insert(make_pair(name, val)).second) - throw KeyAlreadyExists(name, val); - } + if(item != values_.end()) { + // Check the type and throw exception if incorrect + if(typeid(*item->second) != typeid(ValueType)) + throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType)); - /* ************************************************************************* */ - template - void Values::insert(const Values& cfg) { - BOOST_FOREACH(const KeyValuePair& v, cfg.values_) - insert(v.first, v.second); - } - - /* ************************************************************************* */ - template - void Values::update(const Values& cfg) { - BOOST_FOREACH(const KeyValuePair& v, values_) { - boost::optional t = cfg.exists_(v.first); - if (t) values_[v.first] = *t; - } - } - - /* ************************************************************************* */ - template - void Values::update(const J& j, const typename J::Value& val) { - values_[j] = val; - } - - /* ************************************************************************* */ - template - std::list Values::keys() const { - std::list ret; - BOOST_FOREACH(const KeyValuePair& v, values_) - ret.push_back(v.first); - return ret; - } - - /* ************************************************************************* */ - template - void Values::erase(const J& j) { - size_t dim; // unused - erase(j, dim); - } - - /* ************************************************************************* */ - template - void Values::erase(const J& j, size_t& dim) { - iterator it = values_.find(j); - if (it == values_.end()) - throw KeyDoesNotExist("erase", j); - dim = it->second.dim(); - values_.erase(it); - } - - /* ************************************************************************* */ - // todo: insert for every element is inefficient - template - Values Values::retract(const VectorValues& delta, const Ordering& ordering) const { - Values newValues; - typedef pair KeyValue; - BOOST_FOREACH(const KeyValue& value, this->values_) { - const J& j = value.first; - const typename J::Value& pj = value.second; - Index index; - if(ordering.tryAt(j,index)) { - newValues.insert(j, pj.retract(delta[index])); - } else - newValues.insert(j, pj); - } - return newValues; - } - - /* ************************************************************************* */ - template - std::vector Values::dims(const Ordering& ordering) const { - _ValuesDimensionCollector dimCollector(ordering); - this->apply(dimCollector); - return dimCollector.dimensions; - } - - /* ************************************************************************* */ - template - Ordering::shared_ptr Values::orderingArbitrary(Index firstVar) const { - // Generate an initial key ordering in iterator order - _ValuesKeyOrderer keyOrderer(firstVar); - this->apply(keyOrderer); - return keyOrderer.ordering; - } - -// /* ************************************************************************* */ -// // todo: insert for every element is inefficient -// template -// Values Values::retract(const Vector& delta) const { -// if(delta.size() != dim()) { -// cout << "Values::dim (" << dim() << ") <> delta.size (" << delta.size() << ")" << endl; -// throw invalid_argument("Delta vector length does not match config dimensionality."); -// } -// Values newValues; -// int delta_offset = 0; -// typedef pair KeyValue; -// BOOST_FOREACH(const KeyValue& value, this->values_) { -// const J& j = value.first; -// const typename J::Value& pj = value.second; -// int cur_dim = pj.dim(); -// newValues.insert(j,pj.retract(sub(delta, delta_offset, delta_offset+cur_dim))); -// delta_offset += cur_dim; -// } -// return newValues; -// } - - /* ************************************************************************* */ - // todo: insert for every element is inefficient - // todo: currently only logmaps elements in both configs - template - inline VectorValues Values::localCoordinates(const Values& cp, const Ordering& ordering) const { - VectorValues delta(this->dims(ordering)); - localCoordinates(cp, ordering, delta); - return delta; - } - - /* ************************************************************************* */ - template - void Values::localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const { - typedef pair KeyValue; - BOOST_FOREACH(const KeyValue& value, cp) { - assert(this->exists(value.first)); - delta[ordering[value.first]] = this->at(value.first).localCoordinates(value.second); + // We have already checked the type, so do a "blind" static_cast, not dynamic_cast + return static_cast(*item->second); + } else { + return boost::none; } } - /* ************************************************************************* */ - template - const char* KeyDoesNotExist::what() const throw() { - if(message_.empty()) - message_ = - "Attempting to " + std::string(operation_) + " the key \"" + - (std::string)key_ + "\", which does not exist in the Values."; - return message_.c_str(); - } - - /* ************************************************************************* */ - template - const char* KeyAlreadyExists::what() const throw() { - if(message_.empty()) - message_ = - "Attempting to add a key-value pair with key \"" + (std::string)key_ + "\", key already exists."; - return message_.c_str(); - } - } - - diff --git a/gtsam/nonlinear/Values.cpp b/gtsam/nonlinear/Values.cpp new file mode 100644 index 000000000..00b42730b --- /dev/null +++ b/gtsam/nonlinear/Values.cpp @@ -0,0 +1,222 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Values.h + * @author Richard Roberts + * + * @brief A non-templated config holding any types of Manifold-group elements + * + * Detailed story: + * A values structure is a map from keys to values. It is used to specify the value of a bunch + * of variables in a factor graph. A Values is a values structure which can hold variables that + * are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type + * which is also a manifold element, and hence supports operations dim, retract, and localCoordinates. + */ + +#include + +#include + +#include +#include +#include + +using namespace std; + +namespace gtsam { + + /* ************************************************************************* */ + Values::Values(const Values& other) { + this->insert(other); + } + + /* ************************************************************************* */ + void Values::print(const string& str, const KeyFormatter& keyFormatter) const { + cout << str << "Values with " << size() << " values:\n" << endl; + for(const_iterator key_value = begin(); key_value != end(); ++key_value) { + cout << " " << keyFormatter(key_value->first) << ": "; + key_value->second.print(""); + } + } + + /* ************************************************************************* */ + bool Values::equals(const Values& other, double tol) const { + if(this->size() != other.size()) + return false; + for(const_iterator it1=this->begin(), it2=other.begin(); it1!=this->end(); ++it1, ++it2) { + if(typeid(it1->second) != typeid(it2->second)) + return false; + if(it1->first != it2->first) + return false; + if(!it1->second.equals_(it2->second, tol)) + return false; + } + return true; // We return false earlier if we find anything that does not match + } + + /* ************************************************************************* */ + bool Values::exists(Key j) const { + return values_.find(j) != values_.end(); + } + + /* ************************************************************************* */ + VectorValues Values::zeroVectors(const Ordering& ordering) const { + return VectorValues::Zero(this->dims(ordering)); + } + + /* ************************************************************************* */ + Values Values::retract(const VectorValues& delta, const Ordering& ordering) const { + Values result; + + for(const_iterator key_value = begin(); key_value != end(); ++key_value) { + const SubVector& singleDelta = delta[ordering[key_value->first]]; // Delta for this value + Key key = key_value->first; // Non-const duplicate to deal with non-const insert argument + Value* retractedValue(key_value->second.retract_(singleDelta)); // Retract + result.values_.insert(key, retractedValue); // Add retracted result directly to result values + } + + return result; + } + + /* ************************************************************************* */ + VectorValues Values::localCoordinates(const Values& cp, const Ordering& ordering) const { + VectorValues result(this->dims(ordering)); + localCoordinates(cp, ordering, result); + return result; + } + + /* ************************************************************************* */ + void Values::localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& result) const { + if(this->size() != cp.size()) + throw DynamicValuesMismatched(); + for(const_iterator it1=this->begin(), it2=cp.begin(); it1!=this->end(); ++it1, ++it2) { + if(it1->first != it2->first) + throw DynamicValuesMismatched(); // If keys do not match + // Will throw a dynamic_cast exception if types do not match + result.insert(ordering[it1->first], it1->second.localCoordinates_(it2->second)); + } + } + + /* ************************************************************************* */ + void Values::insert(Key j, const Value& val) { + Key key = j; // Non-const duplicate to deal with non-const insert argument + std::pair insertResult = values_.insert(key, val.clone_()); + if(!insertResult.second) + throw ValuesKeyAlreadyExists(j); + } + + /* ************************************************************************* */ + void Values::insert(const Values& values) { + for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { + Key key = key_value->first; // Non-const duplicate to deal with non-const insert argument + insert(key, key_value->second); + } + } + + /* ************************************************************************* */ + void Values::update(Key j, const Value& val) { + // Find the value to update + KeyValueMap::iterator item = values_.find(j); + if(item == values_.end()) + throw ValuesKeyDoesNotExist("update", j); + + // Cast to the derived type + if(typeid(*item->second) != typeid(val)) + throw ValuesIncorrectType(j, typeid(*item->second), typeid(val)); + + values_.replace(item, val.clone_()); + } + + /* ************************************************************************* */ + void Values::update(const Values& values) { + for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) { + this->update(key_value->first, key_value->second); + } + } + + /* ************************************************************************* */ + void Values::erase(Key j) { + KeyValueMap::iterator item = values_.find(j); + if(item == values_.end()) + throw ValuesKeyDoesNotExist("erase", j); + values_.erase(item); + } + + /* ************************************************************************* */ + FastList Values::keys() const { + FastList result; + for(const_iterator key_value = begin(); key_value != end(); ++key_value) + result.push_back(key_value->first); + return result; + } + + /* ************************************************************************* */ + Values& Values::operator=(const Values& rhs) { + this->clear(); + this->insert(rhs); + return *this; + } + + /* ************************************************************************* */ + vector Values::dims(const Ordering& ordering) const { + vector result(values_.size()); + BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) { + result[ordering[key_value.first]] = key_value.second.dim(); + } + return result; + } + + /* ************************************************************************* */ + size_t Values::dim() const { + size_t result = 0; + BOOST_FOREACH(const ConstKeyValuePair& key_value, *this) { + result += key_value.second.dim(); + } + return result; + } + + /* ************************************************************************* */ + Ordering::shared_ptr Values::orderingArbitrary(Index firstVar) const { + Ordering::shared_ptr ordering(new Ordering); + for(const_iterator key_value = begin(); key_value != end(); ++key_value) { + ordering->insert(key_value->first, firstVar++); + } + return ordering; + } + + /* ************************************************************************* */ + const char* ValuesKeyAlreadyExists::what() const throw() { + if(message_.empty()) + message_ = + "Attempting to add a key-value pair with key \"" + DefaultKeyFormatter(key_) + "\", key already exists."; + return message_.c_str(); + } + + /* ************************************************************************* */ + const char* ValuesKeyDoesNotExist::what() const throw() { + if(message_.empty()) + message_ = + "Attempting to " + std::string(operation_) + " the key \"" + + DefaultKeyFormatter(key_) + "\", which does not exist in the Values."; + return message_.c_str(); + } + + /* ************************************************************************* */ + const char* ValuesIncorrectType::what() const throw() { + if(message_.empty()) + message_ = + "Attempting to retrieve value with key \"" + DefaultKeyFormatter(key_) + "\", type stored in Values is " + + std::string(storedTypeId_.name()) + " but requested type was " + std::string(requestedTypeId_.name()); + return message_.c_str(); + } + +} diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index 4c98e8a58..745498c29 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -13,115 +13,191 @@ * @file Values.h * @author Richard Roberts * - * @brief A templated config for Manifold-group elements + * @brief A non-templated config holding any types of Manifold-group elements * * Detailed story: * A values structure is a map from keys to values. It is used to specify the value of a bunch * of variables in a factor graph. A Values is a values structure which can hold variables that * are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type * which is also a manifold element, and hence supports operations dim, retract, and localCoordinates. - * \nosubgrouping */ #pragma once -#include +#include +#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include #include -#include -#include +#include #include -namespace boost { template class optional; } -namespace gtsam { class VectorValues; class Ordering; } - namespace gtsam { // Forward declarations - template class KeyDoesNotExist; - template class KeyAlreadyExists; + class ValueCloneAllocator; + class ValueAutomaticCasting; - /** - * Manifold type values structure - * Takes two template types - * J: a key type to look up values in the values structure (need to be sortable) - * - * Key concept: - * The key will be assumed to be sortable, and must have a - * typedef called "Value" with the type of the value the key - * labels (example: Pose2, Point2, etc) - */ - template +/** + * A non-templated config holding any types of Manifold-group elements. A + * values structure is a map from keys to values. It is used to specify the + * value of a bunch of variables in a factor graph. A Values is a values + * structure which can hold variables that are elements on manifolds, not just + * vectors. It then, as a whole, implements a aggregate type which is also a + * manifold element, and hence supports operations dim, retract, and + * localCoordinates. + */ class Values { - public: - - /** - * Typedefs - */ - typedef J Key; - typedef typename J::Value Value; - typedef std::map, boost::fast_pool_allocator > > KeyValueMap; - // typedef FastMap KeyValueMap; - typedef typename KeyValueMap::value_type KeyValuePair; - typedef typename KeyValueMap::iterator iterator; - typedef typename KeyValueMap::const_iterator const_iterator; - private: - /** concept check */ - GTSAM_CONCEPT_TESTABLE_TYPE(Value) - GTSAM_CONCEPT_MANIFOLD_TYPE(Value) + // Internally we store a boost ptr_map, with a ValueCloneAllocator (defined + // below) to clone and deallocate the Value objects, and a boost + // fast_pool_allocator to allocate map nodes. In this way, all memory is + // allocated in a boost memory pool. + typedef boost::ptr_map< + Key, + Value, + std::less, + ValueCloneAllocator, + boost::fast_pool_allocator > > KeyValueMap; + // The member to store the values, see just above KeyValueMap values_; + // Types obtained by iterating + typedef KeyValueMap::const_iterator::value_type ConstKeyValuePtrPair; + typedef KeyValueMap::iterator::value_type KeyValuePtrPair; + public: - /// @name Standard Constructors - /// @{ + /// A shared_ptr to this class + typedef boost::shared_ptr shared_ptr; - ///TODO comment + /// A key-value pair, which you get by dereferencing iterators + typedef std::pair KeyValuePair; + + /// A key-value pair, which you get by dereferencing iterators + typedef std::pair ConstKeyValuePair; + + /// Mutable forward iterator, with value type KeyValuePair + typedef boost::transform_iterator< + boost::function1, KeyValueMap::iterator> iterator; + + /// Const forward iterator, with value type ConstKeyValuePair + typedef boost::transform_iterator< + boost::function1, KeyValueMap::const_iterator> const_iterator; + + /// Mutable reverse iterator, with value type KeyValuePair + typedef boost::transform_iterator< + boost::function1, KeyValueMap::reverse_iterator> reverse_iterator; + + /// Const reverse iterator, with value type ConstKeyValuePair + typedef boost::transform_iterator< + boost::function1, KeyValueMap::const_reverse_iterator> const_reverse_iterator; + + template + class Filtered : public boost::transformed_range< + std::pair(*)(KeyValuePair key_value), + const boost::filtered_range< + boost::function, + const boost::iterator_range > > { + private: + typedef boost::transformed_range< + std::pair(*)(KeyValuePair key_value), + const boost::filtered_range< + boost::function, + const boost::iterator_range > > Base; + + Filtered(const Base& base) : Base(base) {} + + friend class Values; + }; + + template + class ConstFiltered : public boost::transformed_range< + std::pair(*)(ConstKeyValuePair key_value), + const boost::filtered_range< + boost::function, + const boost::iterator_range > > { + private: + typedef boost::transformed_range< + std::pair(*)(ConstKeyValuePair key_value), + const boost::filtered_range< + boost::function, + const boost::iterator_range > > Base; + + ConstFiltered(const Base& base) : Base(base) {} + + friend class Values; + }; + + /** Default constructor creates an empty Values class */ Values() {} - ///TODO: comment - Values(const Values& config) : - values_(config.values_) {} + /** Copy constructor duplicates all keys and values */ + Values(const Values& other); - ///TODO: comment - template - Values(const Values& other) {} // do nothing when initializing with wrong type - virtual ~Values() {} - - /// @} /// @name Testable /// @{ - /** print */ - void print(const std::string &s="") const; + /** print method for testing and debugging */ + void print(const std::string& str = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; - /** Test whether configs are identical in keys and values */ - bool equals(const Values& expected, double tol=1e-9) const; + /** Test whether the sets of keys and values are identical */ + bool equals(const Values& other, double tol=1e-9) const; /// @} - /// @name Standard Interface - /// @{ - /** Retrieve a variable by j, throws KeyDoesNotExist if not found */ - const Value& at(const J& j) const; + /** Retrieve a variable by key \c j. The type of the value associated with + * this key is supplied as a template argument to this function. + * @param j Retrieve the value associated with this key + * @tparam Value The type of the value stored with this key, this method + * throws DynamicValuesIncorrectType if this requested type is not correct. + * @return A const reference to the stored value + */ + template + const ValueType& at(Key j) const; - /** operator[] syntax for get, throws KeyDoesNotExist if not found */ - const Value& operator[](const J& j) const { - return at(j); } +#if 0 + /** Retrieve a variable by key \c j. This non-templated version returns a + * special ValueAutomaticCasting object that may be assigned to the proper + * value. + * @param j Retrieve the value associated with this key + * @return A ValueAutomaticCasting object that may be assignmed to a Value + * of the proper type. + */ + ValueAutomaticCasting at(Key j) const; +#endif - /** Check if a variable exists */ - bool exists(const J& i) const { return values_.find(i)!=values_.end(); } +#if 0 + /** operator[] syntax for at(Key j) */ + ValueAutomaticCasting operator[](Key j) const; +#endif - /** Check if a variable exists and return it if so */ - boost::optional exists_(const J& i) const { - const_iterator it = values_.find(i); - if (it==values_.end()) return boost::none; else return it->second; - } + /** Check if a value exists with key \c j. See exists<>(Key j) + * and exists(const TypedKey& j) for versions that return the value if it + * exists. */ + bool exists(Key j) const; + + /** Check if a value with key \c j exists, returns the value with type + * \c Value if the key does exist, or boost::none if it does not exist. + * Throws DynamicValuesIncorrectType if the value type associated with the + * requested key does not match the stored value type. */ + template + boost::optional exists(Key j) const; /** The number of variables in this config */ size_t size() const { return values_.size(); } @@ -130,29 +206,27 @@ namespace gtsam { bool empty() const { return values_.empty(); } /** Get a zero VectorValues of the correct structure */ - VectorValues zero(const Ordering& ordering) const; + VectorValues zeroVectors(const Ordering& ordering) const; - const_iterator begin() const { return values_.begin(); } /// dims(const Ordering& ordering) const; + public: + const_iterator begin() const { return boost::make_transform_iterator(values_.begin(), &make_const_deref_pair); } + const_iterator end() const { return boost::make_transform_iterator(values_.end(), &make_const_deref_pair); } + iterator begin() { return boost::make_transform_iterator(values_.begin(), &make_deref_pair); } + iterator end() { return boost::make_transform_iterator(values_.end(), &make_deref_pair); } + const_reverse_iterator rbegin() const { return boost::make_transform_iterator(values_.rbegin(), &make_const_deref_pair); } + const_reverse_iterator rend() const { return boost::make_transform_iterator(values_.rend(), &make_const_deref_pair); } + reverse_iterator rbegin() { return boost::make_transform_iterator(values_.rbegin(), &make_deref_pair); } + reverse_iterator rend() { return boost::make_transform_iterator(values_.rend(), &make_deref_pair); } - /** - * Generate a default ordering, simply in key sort order. To instead - * create a fill-reducing ordering, use - * NonlinearFactorGraph::orderingCOLAMD(). Alternatively, you may permute - * this ordering yourself (as orderingCOLAMD() does internally). - */ - Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const; - - /// @} /// @name Manifold Operations /// @{ - /** The dimensionality of the tangent space */ - size_t dim() const; - /** Add a delta config to current config and returns a new config */ Values retract(const VectorValues& delta, const Ordering& ordering) const; @@ -162,73 +236,161 @@ namespace gtsam { /** Get a delta config about a linearization point c0 (*this) */ void localCoordinates(const Values& cp, const Ordering& ordering, VectorValues& delta) const; - /// @} - /// @name Advanced Interface - /// @{ - - // imperative methods: - - iterator begin() { return values_.begin(); } /// if j is already present */ - void insert(const J& j, const Value& val); + void insert(Key j, const Value& val); /** Add a set of variables, throws KeyAlreadyExists if a key is already present */ - void insert(const Values& cfg); - - /** update the current available values without adding new ones */ - void update(const Values& cfg); + void insert(const Values& values); /** single element change of existing element */ - void update(const J& j, const Value& val); + void update(Key j, const Value& val); + + /** update the current available values without adding new ones */ + void update(const Values& values); /** Remove a variable from the config, throws KeyDoesNotExist if j is not present */ - void erase(const J& j); - - /** Remove a variable from the config while returning the dimensionality of - * the removed element (normally not needed by user code), throws - * KeyDoesNotExist if j is not present. - */ - void erase(const J& j, size_t& dim); + void erase(Key j); /** * Returns a set of keys in the config * Note: by construction, the list is ordered */ - std::list keys() const; + FastList keys() const; /** Replace all keys and variables */ - Values& operator=(const Values& rhs) { - values_ = rhs.values_; - return (*this); - } + Values& operator=(const Values& rhs); /** Remove all variables from the config */ - void clear() { - values_.clear(); + void clear() { values_.clear(); } + + /** Create an array of variable dimensions using the given ordering (\f$ O(n) \f$) */ + std::vector dims(const Ordering& ordering) const; + + /** Compute the total dimensionality of all values (\f$ O(n) \f$) */ + size_t dim() const; + + /** + * Generate a default ordering, simply in key sort order. To instead + * create a fill-reducing ordering, use + * NonlinearFactorGraph::orderingCOLAMD(). Alternatively, you may permute + * this ordering yourself (as orderingCOLAMD() does internally). + */ + Ordering::shared_ptr orderingArbitrary(Index firstVar = 0) const; + + /** + * Return a filtered view of this Values class, without copying any data. + * When iterating over the filtered view, only the key-value pairs + * with a key causing \c filterFcn to return \c true are visited. Because + * the object Filtered returned from filter() is only a + * view the original Values object must not be deallocated or + * go out of scope as long as the view is needed. + * @param filterFcn The function that determines which key-value pairs are + * included in the filtered view, for which this function returns \c true + * on their keys. + * @return A filtered view of the original Values class, which references + * the original Values class. + */ + Filtered + filter(const boost::function& filterFcn) { + return filter(filterFcn); } /** - * Apply a class with an application operator() to a const_iterator over - * every pair. The operator must be able to handle such an - * iterator for every type in the Values, (i.e. through templating). + * Return a filtered view of this Values class, without copying any data. + * In this templated version, only key-value pairs whose value matches the + * template argument \c ValueType and whose key causes the function argument + * \c filterFcn to return true are visited when iterating over the filtered + * view. Because the object Filtered returned from filter() is only + * a view the original Values object must not be deallocated or + * go out of scope as long as the view is needed. + * @tparam ValueType The type that the value in a key-value pair must match + * to be included in the filtered view. Currently, base classes are not + * resolved so the type must match exactly, except if ValueType = Value, in + * which case no type filtering is done. + * @param filterFcn The function that determines which key-value pairs are + * included in the filtered view, for which this function returns \c true + * on their keys (default: always return true so that filter() only + * filters by type, matching \c ValueType). + * @return A filtered view of the original Values class, which references + * the original Values class. */ - template - void apply(A& operation) { - for(iterator it = begin(); it != end(); ++it) - operation(it); - } - template - void apply(A& operation) const { - for(const_iterator it = begin(); it != end(); ++it) - operation(it); + template + Filtered + filter(const boost::function& filterFcn = (boost::lambda::_1, true)) { + return boost::adaptors::transform( + boost::adaptors::filter( + boost::make_iterator_range(begin(), end()), + boost::function( + boost::bind(&filterHelper, filterFcn, _1))), + &castHelper); } - /// @} + /** + * Return a filtered view of this Values class, without copying any data. + * When iterating over the filtered view, only the key-value pairs + * with a key causing \c filterFcn to return \c true are visited. Because + * the object Filtered returned from filter() is only a + * view the original Values object must not be deallocated or + * go out of scope as long as the view is needed. + * @param filterFcn The function that determines which key-value pairs are + * included in the filtered view, for which this function returns \c true + * on their keys. + * @return A filtered view of the original Values class, which references + * the original Values class. + */ + ConstFiltered + filter(const boost::function& filterFcn) const { + return filter(filterFcn); + } + + /** + * Return a filtered view of this Values class, without copying any data. + * In this templated version, only key-value pairs whose value matches the + * template argument \c ValueType and whose key causes the function argument + * \c filterFcn to return true are visited when iterating over the filtered + * view. Because the object Filtered returned from filter() is only + * a view the original Values object must not be deallocated or + * go out of scope as long as the view is needed. + * @tparam ValueType The type that the value in a key-value pair must match + * to be included in the filtered view. Currently, base classes are not + * resolved so the type must match exactly, except if ValueType = Value, in + * which case no type filtering is done. + * @param filterFcn The function that determines which key-value pairs are + * included in the filtered view, for which this function returns \c true + * on their keys. + * @return A filtered view of the original Values class, which references + * the original Values class. + */ + template + ConstFiltered + filter(const boost::function& filterFcn = (boost::lambda::_1, true)) const { + return boost::adaptors::transform( + boost::adaptors::filter( + boost::make_iterator_range(begin(), end()), + boost::function( + boost::bind(&filterHelper, filterFcn, _1))), + &castHelper); + } private: - /** Serialization function */ + // Filters based on ValueType (if not Value) and also based on the user- + // supplied \c filter function. + template + static bool filterHelper(const boost::function filter, const ConstKeyValuePair& key_value) { + // Filter and check the type + return filter(key_value.first) && (typeid(ValueType) == typeid(key_value.second) || typeid(ValueType) == typeid(Value)); + } + + // Cast to the derived ValueType + template + static std::pair castHelper(KeyValuePairType key_value) { + // Static cast because we already checked the type during filtering + return std::pair(key_value.first, static_cast(key_value.second)); + } + + /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { @@ -237,83 +399,95 @@ namespace gtsam { }; - struct _ValuesDimensionCollector { - const Ordering& ordering; - std::vector dimensions; - _ValuesDimensionCollector(const Ordering& _ordering) : ordering(_ordering), dimensions(_ordering.nVars()) {} - template void operator()(const I& key_value) { - Index var; - if(ordering.tryAt(key_value->first, var)) { - assert(var < dimensions.size()); - dimensions[var] = key_value->second.dim(); - } - } + /* ************************************************************************* */ + class ValuesKeyAlreadyExists : public std::exception { + protected: + const Key key_; ///< The key that already existed + + private: + mutable std::string message_; + + public: + /// Construct with the key-value pair attemped to be added + ValuesKeyAlreadyExists(Key key) throw() : + key_(key) {} + + virtual ~ValuesKeyAlreadyExists() throw() {} + + /// The duplicate key that was attemped to be added + Key key() const throw() { return key_; } + + /// The message to be displayed to the user + virtual const char* what() const throw(); }; /* ************************************************************************* */ - struct _ValuesKeyOrderer { - Index var; - Ordering::shared_ptr ordering; - _ValuesKeyOrderer(Index firstVar) : var(firstVar), ordering(new Ordering) {} - template void operator()(const I& key_value) { - ordering->insert(key_value->first, var); - ++ var; + class ValuesKeyDoesNotExist : public std::exception { + protected: + const char* operation_; ///< The operation that attempted to access the key + const Key key_; ///< The key that does not exist + + private: + mutable std::string message_; + + public: + /// Construct with the key that does not exist in the values + ValuesKeyDoesNotExist(const char* operation, Key key) throw() : + operation_(operation), key_(key) {} + + virtual ~ValuesKeyDoesNotExist() throw() {} + + /// The key that was attempted to be accessed that does not exist + Key key() const throw() { return key_; } + + /// The message to be displayed to the user + virtual const char* what() const throw(); + }; + + /* ************************************************************************* */ + class ValuesIncorrectType : public std::exception { + protected: + const Key key_; ///< The key requested + const std::type_info& storedTypeId_; + const std::type_info& requestedTypeId_; + + private: + mutable std::string message_; + + public: + /// Construct with the key that does not exist in the values + ValuesIncorrectType(Key key, + const std::type_info& storedTypeId, const std::type_info& requestedTypeId) throw() : + key_(key), storedTypeId_(storedTypeId), requestedTypeId_(requestedTypeId) {} + + virtual ~ValuesIncorrectType() throw() {} + + /// The key that was attempted to be accessed that does not exist + Key key() const throw() { return key_; } + + /// The typeid of the value stores in the Values + const std::type_info& storedTypeId() const { return storedTypeId_; } + + /// The requested typeid + const std::type_info& requestedTypeId() const { return requestedTypeId_; } + + /// The message to be displayed to the user + virtual const char* what() const throw(); + }; + + /* ************************************************************************* */ + class DynamicValuesMismatched : public std::exception { + + public: + DynamicValuesMismatched() throw() {} + + virtual ~DynamicValuesMismatched() throw() {} + + virtual const char* what() const throw() { + return "The Values 'this' and the argument passed to Values::localCoordinates have mismatched keys and values"; } }; - -/* ************************************************************************* */ -template -class KeyAlreadyExists : public std::exception { -protected: - const J key_; ///< The key that already existed - const typename J::Value value_; ///< The value attempted to be inserted - -private: - mutable std::string message_; - -public: - /// Construct with the key-value pair attemped to be added - KeyAlreadyExists(const J& key, const typename J::Value& value) throw() : - key_(key), value_(value) {} - - virtual ~KeyAlreadyExists() throw() {} - - /// The duplicate key that was attemped to be added - const J& key() const throw() { return key_; } - - /// The value that was attempted to be added - const typename J::Value& value() const throw() { return value_; } - - /// The message to be displayed to the user - virtual const char* what() const throw(); -}; - -/* ************************************************************************* */ -template -class KeyDoesNotExist : public std::exception { -protected: - const char* operation_; ///< The operation that attempted to access the key - const J key_; ///< The key that does not exist - -private: - mutable std::string message_; - -public: - /// Construct with the key that does not exist in the values - KeyDoesNotExist(const char* operation, const J& key) throw() : - operation_(operation), key_(key) {} - - virtual ~KeyDoesNotExist() throw() {} - - /// The key that was attempted to be accessed that does not exist - const J& key() const throw() { return key_; } - - /// The message to be displayed to the user - virtual const char* what() const throw(); -}; - -} // \namespace gtsam +} #include - diff --git a/gtsam/nonlinear/tests/testKey.cpp b/gtsam/nonlinear/tests/testKey.cpp index e05e5d8b5..78688192f 100644 --- a/gtsam/nonlinear/tests/testKey.cpp +++ b/gtsam/nonlinear/tests/testKey.cpp @@ -19,104 +19,18 @@ using namespace boost::assign; #include #include -#include +#include using namespace std; using namespace gtsam; -class Pose3; - /* ************************************************************************* */ -TEST ( TypedSymbol, basic_operations ) { - typedef TypedSymbol Key; +TEST(Key, KeySymbolConversion) { + Symbol expected('j', 4); + Key key(expected); + Symbol actual(key); - Key key1(0), - key2(0), - key3(1), - key4(2); - - CHECK(key1.index()==0); - CHECK(key1 == key2); - CHECK(assert_equal(key1, key2)); - CHECK(!(key1 == key3)); - CHECK(key1 < key3); - CHECK(key3 < key4); -} - -/* ************************************************************************* */ -TEST ( TypedLabledSymbol, basic_operations ) { - typedef TypedSymbol SimpleKey; - typedef TypedLabeledSymbol RobotKey; - - SimpleKey key7(1); - RobotKey key1(0, 1), - key2(0, 1), - key3(1, 1), - key4(2, 1), - key5(0, 2), - key6(1, 2), - key8(1, 3), - key9(key7, 3); - - - CHECK(key1.label()==1); - CHECK(key1.index()==0); - CHECK(key1 == key2); - CHECK(assert_equal(key1, key2)); - CHECK(!(key1 == key3)); - CHECK(key1 < key3); - CHECK(key3 < key4); - CHECK(!(key1 == key5)); - CHECK(key1 < key5); - CHECK(key5 < key6); - CHECK(assert_equal(key9, key8)); -} - -/* ************************************************************************* */ -TEST ( TypedLabledSymbol, encoding ) { - typedef TypedLabeledSymbol RobotKey; - - RobotKey key1(37, 'A'); - - // Note: calculations done in test due to possible differences between machines - // take the upper two bytes for the label - short label = key1.label(); - - // find the shift necessary - size_t shift = (sizeof(size_t)-sizeof(short)) * 8; - size_t modifier = label; - modifier = modifier << shift; - size_t index = key1.index() + modifier; - - // check index encoding - Symbol act1(key1), exp('x', index); - CHECK(assert_equal(exp, act1)); - - // check casting - Symbol act2 = (Symbol) key1; - CHECK(assert_equal(exp, act2)); - - // decode - CHECK(assert_equal(key1, RobotKey(act1))); -} - -/* ************************************************************************* */ -TEST ( TypedLabledSymbol, template_reconstruction ) { - typedef TypedSymbol Key; - typedef TypedLabeledSymbol NewKey; - NewKey k(1, 'A'); -} - -/* ************************************************************************* */ -TEST ( Key, keys2symbols ) -{ - typedef TypedSymbol Key; - list expected; - expected += Key(1), Key(2), Key(3); - - list > typeds; - typeds += 1, 2, 3; - CHECK(expected == keys2symbols(typeds)); + EXPECT(assert_equal(expected, actual)) } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp new file mode 100644 index 000000000..120a9fa94 --- /dev/null +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -0,0 +1,69 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSerializationNonlinear.cpp + * @brief + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + +/* ************************************************************************* */ +// Export all classes derived from Value +BOOST_CLASS_EXPORT(gtsam::Cal3_S2) +BOOST_CLASS_EXPORT(gtsam::Cal3Bundler) +BOOST_CLASS_EXPORT(gtsam::Point3) +BOOST_CLASS_EXPORT(gtsam::Pose3) +BOOST_CLASS_EXPORT(gtsam::Rot3) +BOOST_CLASS_EXPORT(gtsam::PinholeCamera) +BOOST_CLASS_EXPORT(gtsam::PinholeCamera) +BOOST_CLASS_EXPORT(gtsam::PinholeCamera) + +/* ************************************************************************* */ +typedef PinholeCamera PinholeCal3S2; +typedef PinholeCamera PinholeCal3DS2; +typedef PinholeCamera PinholeCal3Bundler; + +/* ************************************************************************* */ +Point3 pt3(1.0, 2.0, 3.0); +Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); +Pose3 pose3(rt3, pt3); + +Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); +Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); +Cal3Bundler cal3(1.0, 2.0, 3.0); + +TEST (Serialization, TemplatedValues) { + Values values; + values.insert(Symbol('a',0), PinholeCal3S2(pose3, cal1)); + values.insert(Symbol('s',5), PinholeCal3DS2(pose3, cal2)); + values.insert(Symbol('d',47), PinholeCal3Bundler(pose3, cal3)); + values.insert(Symbol('a',5), PinholeCal3S2(pose3, cal1)); + EXPECT(equalsObj(values)); + EXPECT(equalsXML(values)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testValues.cpp b/gtsam/nonlinear/tests/testValues.cpp index 3bf76d681..bdf3e1191 100644 --- a/gtsam/nonlinear/tests/testValues.cpp +++ b/gtsam/nonlinear/tests/testValues.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testValues.cpp + * @file testDynamicValues.cpp * @author Richard Roberts */ @@ -21,35 +21,35 @@ using namespace boost::assign; #include +#include #include +#include +#include #include using namespace gtsam; using namespace std; static double inf = std::numeric_limits::infinity(); -typedef TypedSymbol VecKey; -typedef Values TestValues; - -VecKey key1(1), key2(2), key3(3), key4(4); +Key key1(Symbol('v',1)), key2(Symbol('v',2)), key3(Symbol('v',3)), key4(Symbol('v',4)); /* ************************************************************************* */ -TEST( TestValues, equals1 ) +TEST( Values, equals1 ) { - TestValues expected; - Vector v = Vector_(3, 5.0, 6.0, 7.0); + Values expected; + LieVector v(3, 5.0, 6.0, 7.0); expected.insert(key1,v); - TestValues actual; + Values actual; actual.insert(key1,v); CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */ -TEST( TestValues, equals2 ) +TEST( Values, equals2 ) { - TestValues cfg1, cfg2; - Vector v1 = Vector_(3, 5.0, 6.0, 7.0); - Vector v2 = Vector_(3, 5.0, 6.0, 8.0); + Values cfg1, cfg2; + LieVector v1(3, 5.0, 6.0, 7.0); + LieVector v2(3, 5.0, 6.0, 8.0); cfg1.insert(key1, v1); cfg2.insert(key1, v2); CHECK(!cfg1.equals(cfg2)); @@ -57,11 +57,11 @@ TEST( TestValues, equals2 ) } /* ************************************************************************* */ -TEST( TestValues, equals_nan ) +TEST( Values, equals_nan ) { - TestValues cfg1, cfg2; - Vector v1 = Vector_(3, 5.0, 6.0, 7.0); - Vector v2 = Vector_(3, inf, inf, inf); + Values cfg1, cfg2; + LieVector v1(3, 5.0, 6.0, 7.0); + LieVector v2(3, inf, inf, inf); cfg1.insert(key1, v1); cfg2.insert(key1, v2); CHECK(!cfg1.equals(cfg2)); @@ -69,13 +69,13 @@ TEST( TestValues, equals_nan ) } /* ************************************************************************* */ -TEST( TestValues, insert_good ) +TEST( Values, insert_good ) { - TestValues cfg1, cfg2, expected; - Vector v1 = Vector_(3, 5.0, 6.0, 7.0); - Vector v2 = Vector_(3, 8.0, 9.0, 1.0); - Vector v3 = Vector_(3, 2.0, 4.0, 3.0); - Vector v4 = Vector_(3, 8.0, 3.0, 7.0); + Values cfg1, cfg2, expected; + LieVector v1(3, 5.0, 6.0, 7.0); + LieVector v2(3, 8.0, 9.0, 1.0); + LieVector v3(3, 2.0, 4.0, 3.0); + LieVector v4(3, 8.0, 3.0, 7.0); cfg1.insert(key1, v1); cfg1.insert(key2, v2); cfg2.insert(key3, v4); @@ -86,48 +86,48 @@ TEST( TestValues, insert_good ) expected.insert(key2, v2); expected.insert(key3, v4); - CHECK(assert_equal(cfg1, expected)); + CHECK(assert_equal(expected, cfg1)); } /* ************************************************************************* */ -TEST( TestValues, insert_bad ) +TEST( Values, insert_bad ) { - TestValues cfg1, cfg2; - Vector v1 = Vector_(3, 5.0, 6.0, 7.0); - Vector v2 = Vector_(3, 8.0, 9.0, 1.0); - Vector v3 = Vector_(3, 2.0, 4.0, 3.0); - Vector v4 = Vector_(3, 8.0, 3.0, 7.0); + Values cfg1, cfg2; + LieVector v1(3, 5.0, 6.0, 7.0); + LieVector v2(3, 8.0, 9.0, 1.0); + LieVector v3(3, 2.0, 4.0, 3.0); + LieVector v4(3, 8.0, 3.0, 7.0); cfg1.insert(key1, v1); cfg1.insert(key2, v2); cfg2.insert(key2, v3); cfg2.insert(key3, v4); - CHECK_EXCEPTION(cfg1.insert(cfg2), const KeyAlreadyExists); + CHECK_EXCEPTION(cfg1.insert(cfg2), ValuesKeyAlreadyExists); } /* ************************************************************************* */ -TEST( TestValues, update_element ) +TEST( Values, update_element ) { - TestValues cfg; - Vector v1 = Vector_(3, 5.0, 6.0, 7.0); - Vector v2 = Vector_(3, 8.0, 9.0, 1.0); + Values cfg; + LieVector v1(3, 5.0, 6.0, 7.0); + LieVector v2(3, 8.0, 9.0, 1.0); cfg.insert(key1, v1); CHECK(cfg.size() == 1); - CHECK(assert_equal(v1, cfg.at(key1))); + CHECK(assert_equal(v1, cfg.at(key1))); cfg.update(key1, v2); CHECK(cfg.size() == 1); - CHECK(assert_equal(v2, cfg.at(key1))); + CHECK(assert_equal(v2, cfg.at(key1))); } ///* ************************************************************************* */ -//TEST(TestValues, dim_zero) +//TEST(Values, dim_zero) //{ -// TestValues config0; -// config0.insert(key1, Vector_(2, 2.0, 3.0)); -// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); -// LONGS_EQUAL(5,config0.dim()); +// Values config0; +// config0.insert(key1, LieVector(2, 2.0, 3.0)); +// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); +// LONGS_EQUAL(5, config0.dim()); // // VectorValues expected; // expected.insert(key1, zero(2)); @@ -136,149 +136,175 @@ TEST( TestValues, update_element ) //} /* ************************************************************************* */ -TEST(TestValues, expmap_a) +TEST(Values, expmap_a) { - TestValues config0; - config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); - config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); + Values config0; + config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); + config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); Ordering ordering(*config0.orderingArbitrary()); VectorValues increment(config0.dims(ordering)); increment[ordering[key1]] = Vector_(3, 1.0, 1.1, 1.2); increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5); - TestValues expected; - expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2)); - expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); + Values expected; + expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2)); + expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); CHECK(assert_equal(expected, config0.retract(increment, ordering))); } -///* ************************************************************************* */ -//TEST(TestValues, expmap_b) -//{ -// TestValues config0; -// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); -// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); -// -// Ordering ordering(*config0.orderingArbitrary()); -// VectorValues increment(config0.dims(ordering)); -// increment[ordering[key2]] = Vector_(3, 1.3, 1.4, 1.5); -// -// TestValues expected; -// expected.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); -// expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); -// -// CHECK(assert_equal(expected, config0.retract(increment, ordering))); -//} +/* ************************************************************************* */ +TEST(Values, expmap_b) +{ + Values config0; + config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); + config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); -///* ************************************************************************* */ -//TEST(TestValues, expmap_c) + Ordering ordering(*config0.orderingArbitrary()); + VectorValues increment(VectorValues::Zero(config0.dims(ordering))); + increment[ordering[key2]] = LieVector(3, 1.3, 1.4, 1.5); + + Values expected; + expected.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); + expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); + + CHECK(assert_equal(expected, config0.retract(increment, ordering))); +} + +/* ************************************************************************* */ +//TEST(Values, expmap_c) //{ -// TestValues config0; -// config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); -// config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); +// Values config0; +// config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); +// config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); // -// Vector increment = Vector_(6, +// Vector increment = LieVector(6, // 1.0, 1.1, 1.2, // 1.3, 1.4, 1.5); // -// TestValues expected; -// expected.insert(key1, Vector_(3, 2.0, 3.1, 4.2)); -// expected.insert(key2, Vector_(3, 6.3, 7.4, 8.5)); +// Values expected; +// expected.insert(key1, LieVector(3, 2.0, 3.1, 4.2)); +// expected.insert(key2, LieVector(3, 6.3, 7.4, 8.5)); // // CHECK(assert_equal(expected, config0.retract(increment))); //} /* ************************************************************************* */ -/*TEST(TestValues, expmap_d) +TEST(Values, expmap_d) { - TestValues config0; - config0.insert(key1, Vector_(3, 1.0, 2.0, 3.0)); - config0.insert(key2, Vector_(3, 5.0, 6.0, 7.0)); + Values config0; + config0.insert(key1, LieVector(3, 1.0, 2.0, 3.0)); + config0.insert(key2, LieVector(3, 5.0, 6.0, 7.0)); //config0.print("config0"); CHECK(equal(config0, config0)); CHECK(config0.equals(config0)); - TestValues poseconfig; - poseconfig.insert("p1", Pose2(1,2,3)); - poseconfig.insert("p2", Pose2(0.3, 0.4, 0.5)); - //poseconfig.print("poseconfig"); + Values poseconfig; + poseconfig.insert(key1, Pose2(1,2,3)); + poseconfig.insert(key2, Pose2(0.3, 0.4, 0.5)); + CHECK(equal(config0, config0)); CHECK(config0.equals(config0)); -}*/ +} /* ************************************************************************* */ -/*TEST(TestValues, extract_keys) +TEST(Values, extract_keys) { - typedef TypedSymbol PoseKey; - TestValues config; + Values config; - config.insert(PoseKey(1), Pose2()); - config.insert(PoseKey(2), Pose2()); - config.insert(PoseKey(4), Pose2()); - config.insert(PoseKey(5), Pose2()); + config.insert(key1, Pose2()); + config.insert(key2, Pose2()); + config.insert(key3, Pose2()); + config.insert(key4, Pose2()); - list expected, actual; - expected += PoseKey(1), PoseKey(2), PoseKey(4), PoseKey(5); + FastList expected, actual; + expected += key1, key2, key3, key4; actual = config.keys(); CHECK(actual.size() == expected.size()); - list::const_iterator itAct = actual.begin(), itExp = expected.begin(); + FastList::const_iterator itAct = actual.begin(), itExp = expected.begin(); for (; itAct != actual.end() && itExp != expected.end(); ++itAct, ++itExp) { - CHECK(assert_equal(*itExp, *itAct)); + LONGS_EQUAL(*itExp, *itAct); } -}*/ +} /* ************************************************************************* */ -TEST(TestValues, exists_) +TEST(Values, exists_) { - TestValues config0; - config0.insert(key1, Vector_(1, 1.)); - config0.insert(key2, Vector_(1, 2.)); + Values config0; + config0.insert(key1, LieVector(Vector_(1, 1.))); + config0.insert(key2, LieVector(Vector_(1, 2.))); - boost::optional v = config0.exists_(key1); + boost::optional v = config0.exists(key1); CHECK(assert_equal(Vector_(1, 1.),*v)); } /* ************************************************************************* */ -TEST(TestValues, update) +TEST(Values, update) { - TestValues config0; - config0.insert(key1, Vector_(1, 1.)); - config0.insert(key2, Vector_(1, 2.)); + Values config0; + config0.insert(key1, LieVector(1, 1.)); + config0.insert(key2, LieVector(1, 2.)); - TestValues superset; - superset.insert(key1, Vector_(1, -1.)); - superset.insert(key2, Vector_(1, -2.)); - superset.insert(key3, Vector_(1, -3.)); + Values superset; + superset.insert(key1, LieVector(1, -1.)); + superset.insert(key2, LieVector(1, -2.)); config0.update(superset); - TestValues expected; - expected.insert(key1, Vector_(1, -1.)); - expected.insert(key2, Vector_(1, -2.)); + Values expected; + expected.insert(key1, LieVector(1, -1.)); + expected.insert(key2, LieVector(1, -2.)); CHECK(assert_equal(expected,config0)); } /* ************************************************************************* */ -TEST(TestValues, dummy_initialization) -{ - typedef TypedSymbol Key; - typedef Values Values1; +TEST(Values, filter) { + Pose2 pose0(1.0, 2.0, 0.3); + Pose3 pose1(Pose2(0.1, 0.2, 0.3)); + Pose2 pose2(4.0, 5.0, 0.6); + Pose3 pose3(Pose2(0.3, 0.7, 0.9)); - Values1 init1; - init1.insert(Key(1), Vector_(2, 1.0, 2.0)); - init1.insert(Key(2), Vector_(2, 4.0, 3.0)); + Values values; + values.insert(0, pose0); + values.insert(1, pose1); + values.insert(2, pose2); + values.insert(3, pose3); - TestValues init2; - init2.insert(key1, Vector_(2, 1.0, 2.0)); - init2.insert(key2, Vector_(2, 4.0, 3.0)); + // Filter by key + int i = 0; + Values::Filtered filtered = values.filter(boost::bind(std::greater_equal(), _1, 2)); + BOOST_FOREACH(const Values::Filtered::value_type& key_value, filtered) { + if(i == 0) { + LONGS_EQUAL(2, key_value.first); + EXPECT(typeid(Pose2) == typeid(key_value.second)); + EXPECT(assert_equal(pose2, dynamic_cast(key_value.second))); + } else if(i == 1) { + LONGS_EQUAL(3, key_value.first); + EXPECT(typeid(Pose3) == typeid(key_value.second)); + EXPECT(assert_equal(pose3, dynamic_cast(key_value.second))); + } else { + EXPECT(false); + } + ++ i; + } + LONGS_EQUAL(2, i); - Values1 actual1(init2); - TestValues actual2(init1); - - EXPECT(actual1.empty()); - EXPECT(actual2.empty()); + // Filter by type + i = 0; + BOOST_FOREACH(const Values::Filtered::value_type& key_value, values.filter()) { + if(i == 0) { + LONGS_EQUAL(1, key_value.first); + EXPECT(assert_equal(pose1, key_value.second)); + } else if(i == 1) { + LONGS_EQUAL(3, key_value.first); + EXPECT(assert_equal(pose3, key_value.second)); + } else { + EXPECT(false); + } + ++ i; + } + LONGS_EQUAL(2, i); } /* ************************************************************************* */ diff --git a/gtsam/slam/AntiFactor.h b/gtsam/slam/AntiFactor.h index ad33a67cd..633360fed 100644 --- a/gtsam/slam/AntiFactor.h +++ b/gtsam/slam/AntiFactor.h @@ -27,40 +27,39 @@ namespace gtsam { * linearized Hessian matrices of the original factor, but with the opposite sign. This effectively * cancels out any affects of the original factor during optimization." */ - template - class AntiFactor: public NonlinearFactor { + class AntiFactor: public NonlinearFactor { private: - typedef AntiFactor This; - typedef NonlinearFactor Base; - typedef typename NonlinearFactor::shared_ptr sharedFactor; + typedef AntiFactor This; + typedef NonlinearFactor Base; + typedef NonlinearFactor::shared_ptr sharedFactor; sharedFactor factor_; public: // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + typedef boost::shared_ptr shared_ptr; /** default constructor - only use for serialization */ AntiFactor() {} /** constructor - Creates the equivalent AntiFactor from an existing factor */ - AntiFactor(typename NonlinearFactor::shared_ptr factor) : Base(factor->begin(), factor->end()), factor_(factor) {} + AntiFactor(NonlinearFactor::shared_ptr factor) : Base(factor->begin(), factor->end()), factor_(factor) {} virtual ~AntiFactor() {} /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s) const { + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "AntiFactor version of:" << std::endl; - factor_->print(s); + factor_->print(s, keyFormatter); } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && this->factor_->equals(*e->factor_, tol); } @@ -72,7 +71,7 @@ namespace gtsam { * For the AntiFactor, this will have the same magnitude of the original factor, * but the opposite sign. */ - double error(const VALUES& c) const { return -factor_->error(c); } + double error(const Values& c) const { return -factor_->error(c); } /** get the dimension of the factor (same as the original factor) */ size_t dim() const { return factor_->dim(); } @@ -81,7 +80,7 @@ namespace gtsam { * Checks whether this factor should be used based on a set of values. * The AntiFactor will have the same 'active' profile as the original factor. */ - bool active(const VALUES& c) const { return factor_->active(c); } + bool active(const Values& c) const { return factor_->active(c); } /** * Linearize to a GaussianFactor. The AntiFactor always returns a Hessian Factor @@ -90,7 +89,7 @@ namespace gtsam { * effectively cancels the effect of the original factor on the factor graph. */ boost::shared_ptr - linearize(const VALUES& c, const Ordering& ordering) const { + linearize(const Values& c, const Ordering& ordering) const { // Generate the linearized factor from the contained nonlinear factor GaussianFactor::shared_ptr gaussianFactor = factor_->linearize(c, ordering); diff --git a/gtsam/slam/BearingFactor.h b/gtsam/slam/BearingFactor.h index d17dd135e..7c208436a 100644 --- a/gtsam/slam/BearingFactor.h +++ b/gtsam/slam/BearingFactor.h @@ -25,18 +25,18 @@ namespace gtsam { /** * Binary factor for a bearing measurement */ - template - class BearingFactor: public NonlinearFactor2 { + template + class BearingFactor: public NoiseModelFactor2 { private: - typedef typename POSEKEY::Value Pose; - typedef typename POSEKEY::Value::Rotation Rot; - typedef typename POINTKEY::Value Point; + typedef POSE Pose; + typedef typename Pose::Rotation Rot; + typedef POINT Point; - typedef BearingFactor This; - typedef NonlinearFactor2 Base; + typedef BearingFactor This; + typedef NoiseModelFactor2 Base; - Rot z_; /** measurement */ + Rot measured_; /** measurement */ /** concept check by type */ GTSAM_CONCEPT_TESTABLE_TYPE(Rot) @@ -48,9 +48,9 @@ namespace gtsam { BearingFactor() {} /** primary constructor */ - BearingFactor(const POSEKEY& i, const POINTKEY& j, const Rot& z, + BearingFactor(Key poseKey, Key pointKey, const Rot& measured, const SharedNoiseModel& model) : - Base(model, i, j), z_(z) { + Base(model, poseKey, pointKey), measured_(measured) { } virtual ~BearingFactor() {} @@ -59,18 +59,18 @@ namespace gtsam { Vector evaluateError(const Pose& pose, const Point& point, boost::optional H1, boost::optional H2) const { Rot hx = pose.bearing(point, H1, H2); - return Rot::Logmap(z_.between(hx)); + return Rot::Logmap(measured_.between(hx)); } /** return the measured */ - inline const Rot measured() const { - return z_; + const Rot& measured() const { + return measured_; } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && this->z_.equals(e->z_, tol); + return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); } private: @@ -79,9 +79,9 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor2", + ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(measured_); } }; // BearingFactor diff --git a/gtsam/slam/BearingRangeFactor.h b/gtsam/slam/BearingRangeFactor.h index 833708b5d..4c170c811 100644 --- a/gtsam/slam/BearingRangeFactor.h +++ b/gtsam/slam/BearingRangeFactor.h @@ -27,20 +27,20 @@ namespace gtsam { /** * Binary factor for a bearing measurement */ - template - class BearingRangeFactor: public NonlinearFactor2 { + template + class BearingRangeFactor: public NoiseModelFactor2 { private: - typedef typename POSEKEY::Value Pose; - typedef typename POSEKEY::Value::Rotation Rot; - typedef typename POINTKEY::Value Point; + typedef POSE Pose; + typedef typename POSE::Rotation Rot; + typedef POINT Point; - typedef BearingRangeFactor This; - typedef NonlinearFactor2 Base; + typedef BearingRangeFactor This; + typedef NoiseModelFactor2 Base; // the measurement - Rot bearing_; - double range_; + Rot measuredBearing_; + double measuredRange_; /** concept check by type */ GTSAM_CONCEPT_TESTABLE_TYPE(Rot) @@ -50,29 +50,29 @@ namespace gtsam { public: BearingRangeFactor() {} /* Default constructor */ - BearingRangeFactor(const POSEKEY& i, const POINTKEY& j, const Rot& bearing, const double range, + BearingRangeFactor(Key poseKey, Key pointKey, const Rot& measuredBearing, const double measuredRange, const SharedNoiseModel& model) : - Base(model, i, j), bearing_(bearing), range_(range) { + Base(model, poseKey, pointKey), measuredBearing_(measuredBearing), measuredRange_(measuredRange) { } virtual ~BearingRangeFactor() {} /** Print */ - virtual void print(const std::string& s = "") const { + virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << ": BearingRangeFactor(" - << (std::string) this->key1_ << "," - << (std::string) this->key2_ << ")\n"; - bearing_.print(" measured bearing"); - std::cout << " measured range: " << range_ << std::endl; + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\n"; + measuredBearing_.print(" measured bearing"); + std::cout << " measured range: " << measuredRange_ << std::endl; this->noiseModel_->print(" noise model"); } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && - fabs(this->range_ - e->range_) < tol && - this->bearing_.equals(e->bearing_, tol); + fabs(this->measuredRange_ - e->measuredRange_) < tol && + this->measuredBearing_.equals(e->measuredBearing_, tol); } /** h(x)-z -> between(z,h(x)) for Rot manifold */ @@ -85,10 +85,10 @@ namespace gtsam { boost::optional H22_ = H2 ? boost::optional(H22) : boost::optional(); Rot y1 = pose.bearing(point, H11_, H12_); - Vector e1 = Rot::Logmap(bearing_.between(y1)); + Vector e1 = Rot::Logmap(measuredBearing_.between(y1)); double y2 = pose.range(point, H21_, H22_); - Vector e2 = Vector_(1, y2 - range_); + Vector e2 = Vector_(1, y2 - measuredRange_); if (H1) *H1 = gtsam::stack(2, &H11, &H21); if (H2) *H2 = gtsam::stack(2, &H12, &H22); @@ -96,8 +96,8 @@ namespace gtsam { } /** return the measured */ - inline const std::pair measured() const { - return std::make_pair(bearing_, range_); + const std::pair measured() const { + return std::make_pair(measuredBearing_, measuredRange_); } private: @@ -106,10 +106,10 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor2", + ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(bearing_); - ar & BOOST_SERIALIZATION_NVP(range_); + ar & BOOST_SERIALIZATION_NVP(measuredBearing_); + ar & BOOST_SERIALIZATION_NVP(measuredRange_); } }; // BearingRangeFactor diff --git a/gtsam/slam/BetweenFactor.h b/gtsam/slam/BetweenFactor.h index dee18ebc7..2df0b98de 100644 --- a/gtsam/slam/BetweenFactor.h +++ b/gtsam/slam/BetweenFactor.h @@ -26,18 +26,21 @@ namespace gtsam { /** * A class for a measurement predicted by "between(config[key1],config[key2])" - * KEY1::Value is the Lie Group type - * T is the measurement type, by default the same + * @tparam VALUE the Value type */ - template - class BetweenFactor: public NonlinearFactor2 { + template + class BetweenFactor: public NoiseModelFactor2 { + + public: + + typedef VALUE T; private: - typedef BetweenFactor This; - typedef NonlinearFactor2 Base; + typedef BetweenFactor This; + typedef NoiseModelFactor2 Base; - T measured_; /** The measurement */ + VALUE measured_; /** The measurement */ /** concept check by type */ GTSAM_CONCEPT_LIE_TYPE(T) @@ -52,7 +55,7 @@ namespace gtsam { BetweenFactor() {} /** Constructor */ - BetweenFactor(const KEY1& key1, const KEY1& key2, const T& measured, + BetweenFactor(Key key1, Key key2, const VALUE& measured, const SharedNoiseModel& model) : Base(model, key1, key2), measured_(measured) { } @@ -62,16 +65,16 @@ namespace gtsam { /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s) const { + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "BetweenFactor(" - << (std::string) this->key1_ << "," - << (std::string) this->key2_ << ")\n"; + << keyFormatter(this->key1()) << "," + << keyFormatter(this->key2()) << ")\n"; measured_.print(" measured"); this->noiseModel_->print(" noise model"); } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol); } @@ -79,7 +82,7 @@ namespace gtsam { /** implement functions needed to derive from Factor */ /** vector of errors */ - Vector evaluateError(const typename KEY1::Value& p1, const typename KEY1::Value& p2, + Vector evaluateError(const T& p1, const T& p2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { T hx = p1.between(p2, H1, H2); // h(x) @@ -88,12 +91,12 @@ namespace gtsam { } /** return the measured */ - inline const T measured() const { + const VALUE& measured() const { return measured_; } /** number of variables attached to this factor */ - inline std::size_t size() const { + std::size_t size() const { return 2; } @@ -103,7 +106,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor2", + ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(measured_); } @@ -114,16 +117,14 @@ namespace gtsam { * This constraint requires the underlying type to a Lie type * */ - template - class BetweenConstraint : public BetweenFactor { + template + class BetweenConstraint : public BetweenFactor { public: - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr > shared_ptr; /** Syntactic sugar for constrained version */ - BetweenConstraint(const typename KEY::Value& measured, - const KEY& key1, const KEY& key2, double mu = 1000.0) - : BetweenFactor(key1, key2, measured, - noiseModel::Constrained::All(KEY::Value::Dim(), fabs(mu))) {} + BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) : + BetweenFactor(key1, key2, measured, noiseModel::Constrained::All(VALUE::Dim(), fabs(mu))) {} private: @@ -132,7 +133,7 @@ namespace gtsam { template void serialize(ARCHIVE & ar, const unsigned int version) { ar & boost::serialization::make_nvp("BetweenFactor", - boost::serialization::base_object >(*this)); + boost::serialization::base_object >(*this)); } }; // \class BetweenConstraint diff --git a/gtsam/slam/BoundingConstraint.h b/gtsam/slam/BoundingConstraint.h index e8f59cb2d..0032d9a05 100644 --- a/gtsam/slam/BoundingConstraint.h +++ b/gtsam/slam/BoundingConstraint.h @@ -28,16 +28,16 @@ namespace gtsam { * will need to have its value function implemented to return * a scalar for comparison. */ -template -struct BoundingConstraint1: public NonlinearFactor1 { - typedef typename KEY::Value X; - typedef NonlinearFactor1 Base; - typedef boost::shared_ptr > shared_ptr; +template +struct BoundingConstraint1: public NoiseModelFactor1 { + typedef VALUE X; + typedef NoiseModelFactor1 Base; + typedef boost::shared_ptr > shared_ptr; double threshold_; bool isGreaterThan_; /// flag for greater/less than - BoundingConstraint1(const KEY& key, double threshold, + BoundingConstraint1(Key key, double threshold, bool isGreaterThan, double mu = 1000.0) : Base(noiseModel::Constrained::All(1, mu), key), threshold_(threshold), isGreaterThan_(isGreaterThan) { @@ -57,9 +57,9 @@ struct BoundingConstraint1: public NonlinearFactor1 { boost::none) const = 0; /** active when constraint *NOT* met */ - bool active(const VALUES& c) const { + bool active(const Values& c) const { // note: still active at equality to avoid zigzagging - double x = value(c[this->key_]); + double x = value(c.at(this->key())); return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; } @@ -84,7 +84,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor1", + ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(threshold_); ar & BOOST_SERIALIZATION_NVP(isGreaterThan_); @@ -95,18 +95,18 @@ private: * Binary scalar inequality constraint, with a similar value() function * to implement for specific systems */ -template -struct BoundingConstraint2: public NonlinearFactor2 { - typedef typename KEY1::Value X1; - typedef typename KEY2::Value X2; +template +struct BoundingConstraint2: public NoiseModelFactor2 { + typedef VALUE1 X1; + typedef VALUE2 X2; - typedef NonlinearFactor2 Base; - typedef boost::shared_ptr > shared_ptr; + typedef NoiseModelFactor2 Base; + typedef boost::shared_ptr > shared_ptr; double threshold_; bool isGreaterThan_; /// flag for greater/less than - BoundingConstraint2(const KEY1& key1, const KEY2& key2, double threshold, + BoundingConstraint2(Key key1, Key key2, double threshold, bool isGreaterThan, double mu = 1000.0) : Base(noiseModel::Constrained::All(1, mu), key1, key2), threshold_(threshold), isGreaterThan_(isGreaterThan) {} @@ -125,9 +125,9 @@ struct BoundingConstraint2: public NonlinearFactor2 { boost::optional H2 = boost::none) const = 0; /** active when constraint *NOT* met */ - bool active(const VALUES& c) const { + bool active(const Values& c) const { // note: still active at equality to avoid zigzagging - double x = value(c[this->key1_], c[this->key2_]); + double x = value(c.at(this->key1()), c.at(this->key2())); return (isGreaterThan_) ? x <= threshold_ : x >= threshold_; } @@ -157,7 +157,7 @@ private: friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor2", + ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(threshold_); ar & BOOST_SERIALIZATION_NVP(isGreaterThan_); diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 364d97329..674d4e3f9 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -29,21 +29,21 @@ namespace gtsam { /** * Non-linear factor for a constraint derived from a 2D measurement. The calibration is unknown here compared to GenericProjectionFactor */ - template + template class GeneralSFMFactor: - public NonlinearFactor2 { + public NoiseModelFactor2 { protected: - Point2 z_; ///< the 2D measurement + Point2 measured_; ///< the 2D measurement public: - typedef typename CamK::Value Cam; ///< typedef for camera type - typedef GeneralSFMFactor Self ; ///< typedef for this object - typedef NonlinearFactor2 Base; ///< typedef for the base class - typedef Point2 Measurement; ///< typedef for the measurement + typedef CAMERA Cam; ///< typedef for camera type + typedef GeneralSFMFactor This; ///< typedef for this object + typedef NoiseModelFactor2 Base; ///< typedef for the base class + typedef Point2 Measurement; ///< typedef for the measurement // shorthand for a smart pointer to a factor - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr shared_ptr; /** * Constructor @@ -52,11 +52,12 @@ namespace gtsam { * @param i is basically the frame number * @param j is the index of the landmark */ - GeneralSFMFactor(const Point2& z, const SharedNoiseModel& model, const CamK& i, const LmK& j) : Base(model, i, j), z_(z) {} + GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) : + Base(model, cameraKey, landmarkKey), measured_(measured) {} - GeneralSFMFactor():z_(0.0,0.0) {} ///< default constructor - GeneralSFMFactor(const Point2 & p):z_(p) {} ///< constructor that takes a Point2 - GeneralSFMFactor(double x, double y):z_(x,y) {} ///< constructor that takes doubles x,y to make a Point2 + GeneralSFMFactor():measured_(0.0,0.0) {} ///< default constructor + GeneralSFMFactor(const Point2 & p):measured_(p) {} ///< constructor that takes a Point2 + GeneralSFMFactor(double x, double y):measured_(x,y) {} ///< constructor that takes doubles x,y to make a Point2 virtual ~GeneralSFMFactor() {} ///< destructor @@ -64,16 +65,17 @@ namespace gtsam { * print * @param s optional string naming the factor */ - void print(const std::string& s = "SFMFactor") const { - Base::print(s); - z_.print(s + ".z"); + void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s, keyFormatter); + measured_.print(s + ".z"); } /** * equals */ - bool equals(const GeneralSFMFactor &p, double tol = 1e-9) const { - return Base::equals(p, tol) && this->z_.equals(p.z_, tol) ; + bool equals(const NonlinearFactor &p, double tol = 1e-9) const { + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) ; } /** h(x)-z */ @@ -95,7 +97,7 @@ namespace gtsam { /** return the measured */ inline const Point2 measured() const { - return z_; + return measured_; } private: @@ -103,7 +105,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(measured_); } }; diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am deleted file mode 100644 index 71eb94d64..000000000 --- a/gtsam/slam/Makefile.am +++ /dev/null @@ -1,85 +0,0 @@ -#---------------------------------------------------------------------------------------------------- -# SLAM and SFM sources -#---------------------------------------------------------------------------------------------------- - -# use nostdinc to turn off -I. and -I.., we do not need them because -# header files are qualified so they can be included in external projects. -AUTOMAKE_OPTIONS = nostdinc - -headers = -sources = -check_PROGRAMS = - -# simulated2D example -headers += simulated2DConstraints.h -sources += simulated2D.cpp smallExample.cpp -check_PROGRAMS += tests/testSimulated2D - -# simulated2DOriented example -sources += simulated2DOriented.cpp -check_PROGRAMS += tests/testSimulated2DOriented - -# simulated3D example -sources += simulated3D.cpp -check_PROGRAMS += tests/testSimulated3D - -# Generic SLAM headers -headers += BetweenFactor.h PriorFactor.h PartialPriorFactor.h -headers += BearingFactor.h RangeFactor.h BearingRangeFactor.h -headers += AntiFactor.h -check_PROGRAMS += tests/testAntiFactor - -# Generic constraint headers -headers += BoundingConstraint.h - -# 2D Pose SLAM -sources += pose2SLAM.cpp dataset.cpp -check_PROGRAMS += tests/testPose2SLAM - -# 2D SLAM using Bearing and Range -sources += planarSLAM.cpp -check_PROGRAMS += tests/testPlanarSLAM - -# 3D Pose constraints -sources += pose3SLAM.cpp -check_PROGRAMS += tests/testPose3SLAM - -# Visual SLAM -headers += GeneralSFMFactor.h ProjectionFactor.h -sources += visualSLAM.cpp -check_PROGRAMS += tests/testProjectionFactor tests/testVSLAM -check_PROGRAMS += tests/testGeneralSFMFactor tests/testGeneralSFMFactor_Cal3Bundler - -# StereoFactor -headers += StereoFactor.h -check_PROGRAMS += tests/testStereoFactor - -#---------------------------------------------------------------------------------------------------- -# Create a libtool library that is not installed -# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am -# The headers are installed in $(includedir)/gtsam: -#---------------------------------------------------------------------------------------------------- -headers += $(sources:.cpp=.h) -slamdir = $(pkgincludedir)/slam -slam_HEADERS = $(headers) -noinst_LTLIBRARIES = libslam.la -libslam_la_SOURCES = $(sources) -AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) -AM_LDFLAGS = $(BOOST_LDFLAGS) - -#---------------------------------------------------------------------------------------------------- -# rules to build local programs -#---------------------------------------------------------------------------------------------------- -TESTS = $(check_PROGRAMS) -AM_DEFAULT_SOURCE_EXT = .cpp -AM_LDFLAGS += $(boost_serialization) -LDADD = libslam.la ../geometry/libgeometry.la ../nonlinear/libnonlinear.la ../linear/liblinear.la ../inference/libinference.la ../base/libbase.la ../3rdparty/libccolamd.la -LDADD += ../../CppUnitLite/libCppUnitLite.a - -# rule to run an executable -%.run: % $(LDADD) - ./$^ - -# rule to run executable with valgrind -%.valgrind: % $(LDADD) - valgrind ./$^ diff --git a/gtsam/slam/PartialPriorFactor.h b/gtsam/slam/PartialPriorFactor.h index eb1c4614c..4c7069174 100644 --- a/gtsam/slam/PartialPriorFactor.h +++ b/gtsam/slam/PartialPriorFactor.h @@ -38,16 +38,16 @@ namespace gtsam { * For practical use, it would be good to subclass this factor and have the class type * construct the mask. */ - template - class PartialPriorFactor: public NonlinearFactor1 { + template + class PartialPriorFactor: public NoiseModelFactor1 { public: - typedef typename KEY::Value T; + typedef VALUE T; protected: - typedef NonlinearFactor1 Base; - typedef PartialPriorFactor This; + typedef NoiseModelFactor1 Base; + typedef PartialPriorFactor This; Vector prior_; ///< measurement on logmap parameters, in compressed form std::vector mask_; ///< indices of values to constrain in compressed prior vector @@ -60,7 +60,7 @@ namespace gtsam { * constructor with just minimum requirements for a factor - allows more * computation in the constructor. This should only be used by subclasses */ - PartialPriorFactor(const KEY& key, const SharedNoiseModel& model) + PartialPriorFactor(Key key, const SharedNoiseModel& model) : Base(model, key) {} public: @@ -71,14 +71,14 @@ namespace gtsam { virtual ~PartialPriorFactor() {} /** Single Element Constructor: acts on a single parameter specified by idx */ - PartialPriorFactor(const KEY& key, size_t idx, double prior, const SharedNoiseModel& model) : + PartialPriorFactor(Key key, size_t idx, double prior, const SharedNoiseModel& model) : Base(model, key), prior_(Vector_(1, prior)), mask_(1, idx), H_(zeros(1, T::Dim())) { assert(model->dim() == 1); this->fillH(); } /** Indices Constructor: specify the mask with a set of indices */ - PartialPriorFactor(const KEY& key, const std::vector& mask, const Vector& prior, + PartialPriorFactor(Key key, const std::vector& mask, const Vector& prior, const SharedNoiseModel& model) : Base(model, key), prior_(prior), mask_(mask), H_(zeros(mask.size(), T::Dim())) { assert((size_t)prior_.size() == mask.size()); @@ -89,13 +89,13 @@ namespace gtsam { /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s) const { - Base::print(s); + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s, keyFormatter); gtsam::print(prior_, "prior"); } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && gtsam::equal_with_abs_tol(this->prior_, e->prior_, tol) && @@ -133,7 +133,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor1", + ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); ar & BOOST_SERIALIZATION_NVP(mask_); diff --git a/gtsam/slam/PriorFactor.h b/gtsam/slam/PriorFactor.h index 2d7b631cc..2ab011bf5 100644 --- a/gtsam/slam/PriorFactor.h +++ b/gtsam/slam/PriorFactor.h @@ -21,32 +21,30 @@ namespace gtsam { /** - * A class for a soft prior on any Lie type - * It takes two template parameters: - * Key (typically TypedSymbol) is used to look up T's in a Values - * Values where the T's are stored, typically Values or a TupleValues<...> - * The Key type is not arbitrary: we need to cast to a Symbol at linearize, so - * a simple type like int will not work + * A class for a soft prior on any Value type */ - template - class PriorFactor: public NonlinearFactor1 { + template + class PriorFactor: public NoiseModelFactor1 { public: - typedef typename KEY::Value T; + typedef VALUE T; private: - typedef NonlinearFactor1 Base; + typedef NoiseModelFactor1 Base; - T prior_; /** The measurement */ + VALUE prior_; /** The measurement */ /** concept check by type */ GTSAM_CONCEPT_TESTABLE_TYPE(T) public: - // shorthand for a smart pointer to a factor - typedef typename boost::shared_ptr shared_ptr; + /// shorthand for a smart pointer to a factor + typedef typename boost::shared_ptr > shared_ptr; + + /// Typedef to this class + typedef PriorFactor This; /** default constructor - only use for serialization */ PriorFactor() {} @@ -54,24 +52,22 @@ namespace gtsam { virtual ~PriorFactor() {} /** Constructor */ - PriorFactor(const KEY& key, const T& prior, - const SharedNoiseModel& model) : + PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model) : Base(model, key), prior_(prior) { } /** implement functions needed for Testable */ /** print */ - virtual void print(const std::string& s) const { - std::cout << s << "PriorFactor(" << (std::string) this->key_ << ")\n"; + virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "PriorFactor(" << keyFormatter(this->key()) << ")\n"; prior_.print(" prior"); this->noiseModel_->print(" noise model"); } /** equals */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { - const PriorFactor *e = dynamic_cast*> (&expected); + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + const This* e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol); } @@ -90,7 +86,7 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor1", + ar & boost::serialization::make_nvp("NoiseModelFactor1", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(prior_); } diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index b09b148b0..f4251273d 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -29,21 +29,24 @@ namespace gtsam { * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here. * i.e. the main building block for visual SLAM. */ - template - class GenericProjectionFactor: public NonlinearFactor2 { + template + class GenericProjectionFactor: public NoiseModelFactor2 { protected: // Keep a copy of measurement and calibration for I/O - Point2 z_; ///< 2D measurement + Point2 measured_; ///< 2D measurement boost::shared_ptr K_; ///< shared pointer to calibration object public: /// shorthand for base class type - typedef NonlinearFactor2 Base; + typedef NoiseModelFactor2 Base; + + /// shorthand for this class + typedef GenericProjectionFactor This; /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr > shared_ptr; + typedef boost::shared_ptr shared_ptr; /// Default constructor GenericProjectionFactor() : @@ -52,31 +55,31 @@ namespace gtsam { /** * Constructor + * TODO: Mark argument order standard (keys, measurement, parameters) * @param z is the 2 dimensional location of point in image (the measurement) * @param model is the standard deviation * @param j_pose is basically the frame number * @param j_landmark is the index of the landmark * @param K shared pointer to the constant calibration */ - GenericProjectionFactor(const Point2& z, const SharedNoiseModel& model, - POSK j_pose, LMK j_landmark, const shared_ptrK& K) : - Base(model, j_pose, j_landmark), z_(z), K_(K) { + GenericProjectionFactor(const Point2& measured, const SharedNoiseModel& model, + const Symbol poseKey, Key pointKey, const shared_ptrK& K) : + Base(model, poseKey, pointKey), measured_(measured), K_(K) { } /** * print * @param s optional string naming the factor */ - void print(const std::string& s = "ProjectionFactor") const { - Base::print(s); - z_.print(s + ".z"); + void print(const std::string& s = "ProjectionFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s, keyFormatter); + measured_.print(s + ".z"); } /// equals - bool equals(const GenericProjectionFactor& p - , double tol = 1e-9) const { - return Base::equals(p, tol) && this->z_.equals(p.z_, tol) - && this->K_->equals(*p.K_, tol); + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) && this->K_->equals(*e->K_, tol); } /// Evaluate error h(x)-z and optionally derivatives @@ -84,21 +87,20 @@ namespace gtsam { boost::optional H1, boost::optional H2) const { try { SimpleCamera camera(*K_, pose); - Point2 reprojectionError(camera.project(point, H1, H2) - z_); + Point2 reprojectionError(camera.project(point, H1, H2) - measured_); return reprojectionError.vector(); - } - catch( CheiralityException& e) { + } catch( CheiralityException& e) { if (H1) *H1 = zeros(2,6); if (H2) *H2 = zeros(2,3); - cout << e.what() << ": Landmark "<< this->key2_.index() << - " moved behind camera " << this->key1_.index() << endl; + cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << + " moved behind camera " << DefaultKeyFormatter(this->key1()) << endl; return ones(2) * 2.0 * K_->fx(); } } /** return the measurement */ - inline const Point2 measured() const { - return z_; + const Point2& measured() const { + return measured_; } /** return the calibration object */ @@ -113,7 +115,7 @@ namespace gtsam { template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); } }; diff --git a/gtsam/slam/RangeFactor.h b/gtsam/slam/RangeFactor.h index 93cef86f1..489f751db 100644 --- a/gtsam/slam/RangeFactor.h +++ b/gtsam/slam/RangeFactor.h @@ -25,17 +25,17 @@ namespace gtsam { /** * Binary factor for a range measurement */ - template - class RangeFactor: public NonlinearFactor2 { + template + class RangeFactor: public NoiseModelFactor2 { private: - double z_; /** measurement */ + double measured_; /** measurement */ - typedef RangeFactor This; - typedef NonlinearFactor2 Base; + typedef RangeFactor This; + typedef NoiseModelFactor2 Base; - typedef typename POSEKEY::Value Pose; - typedef typename POINTKEY::Value Point; + typedef POSE Pose; + typedef POINT Point; // Concept requirements for this factor GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(Pose, Point) @@ -44,34 +44,33 @@ namespace gtsam { RangeFactor() {} /* Default constructor */ - RangeFactor(const POSEKEY& i, const POINTKEY& j, double z, + RangeFactor(Key poseKey, Key pointKey, double measured, const SharedNoiseModel& model) : - Base(model, i, j), z_(z) { + Base(model, poseKey, pointKey), measured_(measured) { } virtual ~RangeFactor() {} /** h(x)-z */ - Vector evaluateError(const typename POSEKEY::Value& pose, const typename POINTKEY::Value& point, - boost::optional H1, boost::optional H2) const { + Vector evaluateError(const Pose& pose, const Point& point, boost::optional H1, boost::optional H2) const { double hx = pose.range(point, H1, H2); - return Vector_(1, hx - z_); + return Vector_(1, hx - measured_); } /** return the measured */ - inline double measured() const { - return z_; + double measured() const { + return measured_; } /** equals specialized to this factor */ - virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { + virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); - return e != NULL && Base::equals(*e, tol) && fabs(this->z_ - e->z_) < tol; + return e != NULL && Base::equals(*e, tol) && fabs(this->measured_ - e->measured_) < tol; } /** print contents */ - void print(const std::string& s="") const { - Base::print(s + std::string(" range: ") + boost::lexical_cast(z_)); + void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s + std::string(" range: ") + boost::lexical_cast(measured_), keyFormatter); } private: @@ -80,9 +79,9 @@ namespace gtsam { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NonlinearFactor2", + ar & boost::serialization::make_nvp("NoiseModelFactor2", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(measured_); } }; // RangeFactor diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index 8d1e360d9..1eefacc96 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -22,20 +22,20 @@ namespace gtsam { -template -class GenericStereoFactor: public NonlinearFactor2 { +template +class GenericStereoFactor: public NoiseModelFactor2 { private: // Keep a copy of measurement and calibration for I/O - StereoPoint2 z_; ///< the measurement + StereoPoint2 measured_; ///< the measurement boost::shared_ptr K_; ///< shared pointer to calibration public: // shorthand for base class type - typedef NonlinearFactor2 Base; ///< typedef for base class + typedef NoiseModelFactor2 Base; ///< typedef for base class typedef boost::shared_ptr shared_ptr; ///< typedef for shared pointer to this object - typedef typename KEY1::Value CamPose; ///< typedef for Pose Lie Value type + typedef POSE CamPose; ///< typedef for Pose Lie Value type /** * Default constructor @@ -44,60 +44,45 @@ public: /** * Constructor - * @param z is the Stereo Point measurement (u_l, u_r, v). v will be identical for left & right for rectified stereo pair + * @param measured is the Stereo Point measurement (u_l, u_r, v). v will be identical for left & right for rectified stereo pair * @param model is the noise model in on the measurement - * @param j_pose the pose index - * @param j_landmark the landmark index + * @param poseKey the pose variable key + * @param landmarkKey the landmark variable key * @param K the constant calibration */ - GenericStereoFactor(const StereoPoint2& z, const SharedNoiseModel& model, KEY1 j_pose, - KEY2 j_landmark, const shared_ptrKStereo& K) : - Base(model, j_pose, j_landmark), z_(z), K_(K) { + GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, const shared_ptrKStereo& K) : + Base(model, poseKey, landmarkKey), measured_(measured), K_(K) { } - ~GenericStereoFactor() {} ///< desctructor + ~GenericStereoFactor() {} ///< destructor /** * print * @param s optional string naming the factor */ - void print(const std::string& s) const { - Base::print(s); - z_.print(s + ".z"); + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + Base::print(s, keyFormatter); + measured_.print(s + ".z"); } /** * equals */ - bool equals(const GenericStereoFactor& f, double tol) const { - const GenericStereoFactor* p = dynamic_cast (&f); - if (p == NULL) - goto fail; - //if (cameraFrameNumber_ != p->cameraFrameNumber_ || landmarkNumber_ != p->landmarkNumber_) goto fail; - if (!z_.equals(p->z_, tol)) - goto fail; - return true; - - fail: print("actual"); - p->print("expected"); - return false; + virtual bool equals(const NonlinearFactor& f, double tol) const { + const GenericStereoFactor* p = dynamic_cast (&f); + return p && Base::equals(f) && measured_.equals(p->measured_, tol); } /** h(x)-z */ Vector evaluateError(const Pose3& pose, const Point3& point, boost::optional H1, boost::optional H2) const { StereoCamera stereoCam(pose, K_); - return (stereoCam.project(point, H1, H2) - z_).vector(); - } - - /// get the measurement z - StereoPoint2 z() { - return z_; + return (stereoCam.project(point, H1, H2) - measured_).vector(); } /** return the measured */ - inline const StereoPoint2 measured() const { - return z_; + const StereoPoint2& measured() const { + return measured_; } /** return the calibration object */ @@ -111,7 +96,7 @@ private: friend class boost::serialization::access; template void serialize(Archive & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); } }; diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index af964c0ab..dae437b79 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -29,7 +29,6 @@ using namespace gtsam; #define LINESIZE 81920 typedef boost::shared_ptr sharedPose2Graph; -typedef boost::shared_ptr sharedPose2Values; typedef pose2SLAM::Odometry Pose2Factor; ///< Typedef for Constraint class for backwards compatibility namespace gtsam { @@ -66,13 +65,13 @@ pair > dataset(const string& dataset, c /* ************************************************************************* */ -pair load2D( +pair load2D( pair > dataset, int maxID, bool addNoise, bool smart) { return load2D(dataset.first, dataset.second, maxID, addNoise, smart); } -pair load2D(const string& filename, +pair load2D(const string& filename, boost::optional model, int maxID, bool addNoise, bool smart) { cout << "Will try to read " << filename << endl; ifstream is(filename.c_str()); @@ -81,7 +80,7 @@ pair load2D(const string& filename, exit(-1); } - sharedPose2Values poses(new pose2SLAM::Values); + Values::shared_ptr poses(new Values); sharedPose2Graph graph(new pose2SLAM::Graph); string tag; @@ -96,7 +95,7 @@ pair load2D(const string& filename, is >> id >> x >> y >> yaw; // optional filter if (maxID && id >= maxID) continue; - poses->insert(id, Pose2(x, y, yaw)); + poses->insert(pose2SLAM::PoseKey(id), Pose2(x, y, yaw)); } is.ignore(LINESIZE, '\n'); } @@ -133,10 +132,10 @@ pair load2D(const string& filename, l1Xl2 = l1Xl2.retract((*model)->sample()); // Insert vertices if pure odometry file - if (!poses->exists(id1)) poses->insert(id1, Pose2()); - if (!poses->exists(id2)) poses->insert(id2, poses->at(id1) * l1Xl2); + if (!poses->exists(pose2SLAM::PoseKey(id1))) poses->insert(pose2SLAM::PoseKey(id1), Pose2()); + if (!poses->exists(pose2SLAM::PoseKey(id2))) poses->insert(pose2SLAM::PoseKey(id2), poses->at(pose2SLAM::PoseKey(id1)) * l1Xl2); - pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(id1, id2, l1Xl2, *model)); + pose2SLAM::Graph::sharedFactor factor(new Pose2Factor(pose2SLAM::PoseKey(id1), pose2SLAM::PoseKey(id2), l1Xl2, *model)); graph->push_back(factor); } is.ignore(LINESIZE, '\n'); @@ -149,27 +148,26 @@ pair load2D(const string& filename, } /* ************************************************************************* */ -void save2D(const pose2SLAM::Graph& graph, const pose2SLAM::Values& config, const SharedDiagonal model, +void save2D(const pose2SLAM::Graph& graph, const Values& config, const SharedDiagonal model, const string& filename) { - typedef pose2SLAM::Values::Key Key; fstream stream(filename.c_str(), fstream::out); // save poses - pose2SLAM::Values::Key key; - Pose2 pose; - BOOST_FOREACH(boost::tie(key, pose), config) - stream << "VERTEX2 " << key.index() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) { + const Pose2& pose = dynamic_cast(key_value.second); + stream << "VERTEX2 " << key_value.first << " " << pose.x() << " " << pose.y() << " " << pose.theta() << endl; + } // save edges Matrix R = model->R(); Matrix RR = trans(R)*R;//prod(trans(R),R); - BOOST_FOREACH(boost::shared_ptr > factor_, graph) { + BOOST_FOREACH(boost::shared_ptr factor_, graph) { boost::shared_ptr factor = boost::dynamic_pointer_cast(factor_); if (!factor) continue; - pose = factor->measured().inverse(); - stream << "EDGE2 " << factor->key2().index() << " " << factor->key1().index() + Pose2 pose = factor->measured().inverse(); + stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << " " << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) << " " << RR(0, 2) << " " << RR(1, 2) << endl; diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 99e7f7eef..5e06e583d 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -40,16 +40,16 @@ std::pair > * @param maxID, if non-zero cut out vertices >= maxID * @param smart: try to reduce complexity of covariance to cheapest model */ -std::pair, boost::shared_ptr > load2D( +std::pair, boost::shared_ptr > load2D( std::pair > dataset, int maxID = 0, bool addNoise=false, bool smart=true); -std::pair, boost::shared_ptr > load2D( +std::pair, boost::shared_ptr > load2D( const std::string& filename, boost::optional model = boost::optional(), int maxID = 0, bool addNoise=false, bool smart=true); /** save 2d graph */ -void save2D(const pose2SLAM::Graph& graph, const pose2SLAM::Values& config, const gtsam::SharedDiagonal model, +void save2D(const pose2SLAM::Graph& graph, const Values& config, const gtsam::SharedDiagonal model, const std::string& filename); /** diff --git a/gtsam/slam/planarSLAM.cpp b/gtsam/slam/planarSLAM.cpp index 6942ab429..db3d6b146 100644 --- a/gtsam/slam/planarSLAM.cpp +++ b/gtsam/slam/planarSLAM.cpp @@ -23,41 +23,37 @@ // Use planarSLAM namespace for specific SLAM instance namespace planarSLAM { - Graph::Graph(const NonlinearFactorGraph& graph) : - NonlinearFactorGraph(graph) {} + Graph::Graph(const NonlinearFactorGraph& graph) : + NonlinearFactorGraph(graph) {} - void Graph::addPrior(const PoseKey& i, const Pose2& p, - const SharedNoiseModel& model) { - sharedFactor factor(new Prior(i, p, model)); + void Graph::addPrior(Index i, const Pose2& p, const SharedNoiseModel& model) { + sharedFactor factor(new Prior(PoseKey(i), p, model)); push_back(factor); } - void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) { - sharedFactor factor(new Constraint(i, p)); + void Graph::addPoseConstraint(Index i, const Pose2& p) { + sharedFactor factor(new Constraint(PoseKey(i), p)); push_back(factor); } - void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, - const SharedNoiseModel& model) { - sharedFactor factor(new Odometry(i, j, z, model)); + void Graph::addOdometry(Index i, Index j, const Pose2& odometry, const SharedNoiseModel& model) { + sharedFactor factor(new Odometry(PoseKey(i), PoseKey(j), odometry, model)); push_back(factor); } - void Graph::addBearing(const PoseKey& i, const PointKey& j, const Rot2& z, - const SharedNoiseModel& model) { - sharedFactor factor(new Bearing(i, j, z, model)); + void Graph::addBearing(Index i, Index j, const Rot2& z, const SharedNoiseModel& model) { + sharedFactor factor(new Bearing(PoseKey(i), PointKey(j), z, model)); push_back(factor); } - void Graph::addRange(const PoseKey& i, const PointKey& j, double z, - const SharedNoiseModel& model) { - sharedFactor factor(new Range(i, j, z, model)); + void Graph::addRange(Index i, Index j, double z, const SharedNoiseModel& model) { + sharedFactor factor(new Range(PoseKey(i), PointKey(j), z, model)); push_back(factor); } - void Graph::addBearingRange(const PoseKey& i, const PointKey& j, const Rot2& z1, + void Graph::addBearingRange(Index i, Index j, const Rot2& z1, double z2, const SharedNoiseModel& model) { - sharedFactor factor(new BearingRange(i, j, z1, z2, model)); + sharedFactor factor(new BearingRange(PoseKey(i), PointKey(j), z1, z2, model)); push_back(factor); } diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index b8e887f8a..33dc97a8c 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -22,7 +22,6 @@ #include #include #include -#include #include #include #include @@ -34,112 +33,91 @@ namespace planarSLAM { using namespace gtsam; - /// Typedef for a PoseKey with Pose2 data and 'x' symbol - typedef TypedSymbol PoseKey; + /// Convenience function for constructing a pose key + inline Symbol PoseKey(Index j) { return Symbol('x', j); } - /// Typedef for a PointKey with Point2 data and 'l' symbol - typedef TypedSymbol PointKey; + /// Convenience function for constructing a pose key + inline Symbol PointKey(Index j) { return Symbol('l', j); } - /// Typedef for Values structure with PoseKey type - typedef Values PoseValues; + /* + * List of typedefs for factors + */ + /// A hard constraint for PoseKeys to enforce particular values + typedef NonlinearEquality Constraint; + /// A prior factor to bias the value of a PoseKey + typedef PriorFactor Prior; + /// A factor between two PoseKeys set with a Pose2 + typedef BetweenFactor Odometry; + /// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2) + typedef BearingFactor Bearing; + /// A factor between a PoseKey and a PointKey to express distance between them (set with a double) + typedef RangeFactor Range; + /// A factor between a PoseKey and a PointKey to express difference in rotation and location + typedef BearingRangeFactor BearingRange; - /// Typedef for Values structure with PointKey type - typedef Values PointValues; - - /// Values class, inherited from TupleValues2, using PoseKeys and PointKeys - struct Values: public TupleValues2 { + /** Values class, using specific PoseKeys and PointKeys + * Mainly as a convenience for MATLAB wrapper, which does not allow for identically named methods + */ + struct Values: public gtsam::Values { /// Default constructor Values() {} /// Copy constructor - Values(const TupleValues2& values) : - TupleValues2(values) { + Values(const gtsam::Values& values) : + gtsam::Values(values) { } - /// Copy constructor - Values(const TupleValues2::Base& values) : - TupleValues2(values) { - } - - /// From sub-values - Values(const PoseValues& poses, const PointValues& points) : - TupleValues2(poses, points) { - } - - // Convenience for MATLAB wrapper, which does not allow for identically named methods - /// get a pose - Pose2 pose(int key) const { return (*this)[PoseKey(key)]; } + Pose2 pose(Index key) const { return at(PoseKey(key)); } /// get a point - Point2 point(int key) const { return (*this)[PointKey(key)]; } + Point2 point(Index key) const { return at(PointKey(key)); } /// insert a pose - void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); } + void insertPose(Index key, const Pose2& pose) { insert(PoseKey(key), pose); } /// insert a point - void insertPoint(int key, const Point2& point) {insert(PointKey(key), point); } + void insertPoint(Index key, const Point2& point) { insert(PointKey(key), point); } }; - /** - * List of typedefs for factors - */ - - /// A hard constraint for PoseKeys to enforce particular values - typedef NonlinearEquality Constraint; - /// A prior factor to bias the value of a PoseKey - typedef PriorFactor Prior; - /// A factor between two PoseKeys set with a Pose2 - typedef BetweenFactor Odometry; - /// A factor between a PoseKey and a PointKey to express difference in rotation (set with a Rot2) - typedef BearingFactor Bearing; - /// A factor between a PoseKey and a PointKey to express distance between them (set with a double) - typedef RangeFactor Range; - /// A factor between a PoseKey and a PointKey to express difference in rotation and location - typedef BearingRangeFactor BearingRange; - /// Creates a NonlinearFactorGraph with the Values type - struct Graph: public NonlinearFactorGraph { + struct Graph: public NonlinearFactorGraph { /// Default constructor for a NonlinearFactorGraph Graph(){} /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph); + Graph(const NonlinearFactorGraph& graph); /// Biases the value of PoseKey key with Pose2 p given a noise model - void addPrior(const PoseKey& key, const Pose2& pose, const SharedNoiseModel& noiseModel); + void addPrior(Index poseKey, const Pose2& pose, const SharedNoiseModel& noiseModel); /// Creates a hard constraint to enforce Pose2 p for PoseKey poseKey's value - void addPoseConstraint(const PoseKey& poseKey, const Pose2& pose); + void addPoseConstraint(Index poseKey, const Pose2& pose); /// Creates a factor with a Pose2 between PoseKeys poseKey and pointKey (poseKey.e. an odometry measurement) - void addOdometry(const PoseKey& poseKey, const PoseKey& pointKey, const Pose2& odometry, - const SharedNoiseModel& model); + void addOdometry(Index poseKey1, Index poseKey2, const Pose2& odometry, const SharedNoiseModel& model); /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation - void addBearing(const PoseKey& poseKey, const PointKey& pointKey, const Rot2& bearing, - const SharedNoiseModel& model); + void addBearing(Index poseKey, Index pointKey, const Rot2& bearing, const SharedNoiseModel& model); /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in location - void addRange(const PoseKey& poseKey, const PointKey& pointKey, double range, - const SharedNoiseModel& model); + void addRange(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model); /// Creates a factor with a Rot2 between a PoseKey poseKey and PointKey pointKey for difference in rotation and location - void addBearingRange(const PoseKey& poseKey, const PointKey& pointKey, - const Rot2& bearing, double range, const SharedNoiseModel& model); + void addBearingRange(Index poseKey, Index pointKey, const Rot2& bearing, double range, const SharedNoiseModel& model); /// Optimize Values optimize(const Values& initialEstimate) { - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; return *Optimizer::optimizeLM(*this, initialEstimate, - NonlinearOptimizationParameters::LAMBDA); + NonlinearOptimizationParameters::LAMBDA); } }; /// Optimizer - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; } // planarSLAM diff --git a/gtsam/slam/pose2SLAM.cpp b/gtsam/slam/pose2SLAM.cpp index e078505a6..b006c61f2 100644 --- a/gtsam/slam/pose2SLAM.cpp +++ b/gtsam/slam/pose2SLAM.cpp @@ -21,7 +21,7 @@ // Use pose2SLAM namespace for specific SLAM instance -template class gtsam::NonlinearOptimizer; + template class gtsam::NonlinearOptimizer; namespace pose2SLAM { @@ -30,25 +30,25 @@ namespace pose2SLAM { Values x; double theta = 0, dtheta = 2 * M_PI / n; for (size_t i = 0; i < n; i++, theta += dtheta) - x.insert(i, Pose2(cos(theta), sin(theta), M_PI_2 + theta)); + x.insert(PoseKey(i), Pose2(cos(theta), sin(theta), M_PI_2 + theta)); return x; } /* ************************************************************************* */ - void Graph::addPrior(const PoseKey& i, const Pose2& p, + void Graph::addPrior(Index i, const Pose2& p, const SharedNoiseModel& model) { - sharedFactor factor(new Prior(i, p, model)); + sharedFactor factor(new Prior(PoseKey(i), p, model)); push_back(factor); } - void Graph::addPoseConstraint(const PoseKey& i, const Pose2& p) { - sharedFactor factor(new HardConstraint(i, p)); + void Graph::addPoseConstraint(Index i, const Pose2& p) { + sharedFactor factor(new HardConstraint(PoseKey(i), p)); push_back(factor); } - void Graph::addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, + void Graph::addOdometry(Index i, Index j, const Pose2& z, const SharedNoiseModel& model) { - sharedFactor factor(new Odometry(i, j, z, model)); + sharedFactor factor(new Odometry(PoseKey(i), PoseKey(j), z, model)); push_back(factor); } diff --git a/gtsam/slam/pose2SLAM.h b/gtsam/slam/pose2SLAM.h index 82a5a6efd..121e0c062 100644 --- a/gtsam/slam/pose2SLAM.h +++ b/gtsam/slam/pose2SLAM.h @@ -18,10 +18,10 @@ #pragma once #include -#include #include #include -#include +#include +#include #include #include #include @@ -33,27 +33,27 @@ namespace pose2SLAM { using namespace gtsam; - /// Keys with Pose2 and symbol 'x' - typedef TypedSymbol PoseKey; + /// Convenience function for constructing a pose key + inline Symbol PoseKey(Index j) { return Symbol('x', j); } - /// Values class, inherited from Values, using PoseKeys - struct Values: public gtsam::Values { + /// Values class, inherited from Values, using PoseKeys, mainly used as a convenience for MATLAB wrapper + struct Values: public gtsam::Values { /// Default constructor Values() {} /// Copy constructor - Values(const gtsam::Values& values) : - gtsam::Values(values) { + Values(const gtsam::Values& values) : + gtsam::Values(values) { } // Convenience for MATLAB wrapper, which does not allow for identically named methods /// get a pose - Pose2 pose(int key) const { return (*this)[PoseKey(key)]; } + Pose2 pose(Index key) const { return at(PoseKey(key)); } /// insert a pose - void insertPose(int key, const Pose2& pose) { insert(PoseKey(key), pose); } + void insertPose(Index key, const Pose2& pose) { insert(PoseKey(key), pose); } }; /** @@ -70,45 +70,45 @@ namespace pose2SLAM { */ /// A hard constraint to enforce a specific value for a pose - typedef NonlinearEquality HardConstraint; + typedef NonlinearEquality HardConstraint; /// A prior factor on a pose with Pose2 data type. - typedef PriorFactor Prior; + typedef PriorFactor Prior; /// A factor to add an odometry measurement between two poses. - typedef BetweenFactor Odometry; + typedef BetweenFactor Odometry; /// Graph - struct Graph: public NonlinearFactorGraph { + struct Graph: public NonlinearFactorGraph { /// Default constructor for a NonlinearFactorGraph Graph(){} /// Creates a NonlinearFactorGraph based on another NonlinearFactorGraph - Graph(const NonlinearFactorGraph& graph); + Graph(const NonlinearFactorGraph& graph); /// Adds a Pose2 prior with a noise model to one of the keys in the nonlinear factor graph - void addPrior(const PoseKey& i, const Pose2& p, const SharedNoiseModel& model); + void addPrior(Index i, const Pose2& p, const SharedNoiseModel& model); /// Creates a hard constraint for key i with the given Pose2 p. - void addPoseConstraint(const PoseKey& i, const Pose2& p); + void addPoseConstraint(Index i, const Pose2& p); /// Creates a between factor between keys i and j with a noise model with Pose2 z in the graph - void addOdometry(const PoseKey& i, const PoseKey& j, const Pose2& z, + void addOdometry(Index i, Index j, const Pose2& z, const SharedNoiseModel& model); /// Optimize Values optimize(const Values& initialEstimate) { - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; return *Optimizer::optimizeLM(*this, initialEstimate, NonlinearOptimizationParameters::LAMBDA); } }; /// The sequential optimizer - typedef NonlinearOptimizer OptimizerSequential; /// The multifrontal optimizer - typedef NonlinearOptimizer Optimizer; } // pose2SLAM diff --git a/gtsam/slam/pose3SLAM.cpp b/gtsam/slam/pose3SLAM.cpp index 102652813..2b9a3db1d 100644 --- a/gtsam/slam/pose3SLAM.cpp +++ b/gtsam/slam/pose3SLAM.cpp @@ -39,26 +39,24 @@ namespace gtsam { Point3 gti(radius*cos(theta), radius*sin(theta), 0); Rot3 _0Ri = Rot3::yaw(-theta); // negative yaw goes counterclockwise, with Z down ! Pose3 gTi(gR0 * _0Ri, gti); - x.insert(i, gTi); + x.insert(PoseKey(i), gTi); } return x; } /* ************************************************************************* */ - void Graph::addPrior(const Key& i, const Pose3& p, - const SharedNoiseModel& model) { - sharedFactor factor(new Prior(i, p, model)); + void Graph::addPrior(Index i, const Pose3& p, const SharedNoiseModel& model) { + sharedFactor factor(new Prior(PoseKey(i), p, model)); push_back(factor); } - void Graph::addConstraint(const Key& i, const Key& j, const Pose3& z, - const SharedNoiseModel& model) { - sharedFactor factor(new Constraint(i, j, z, model)); + void Graph::addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model) { + sharedFactor factor(new Constraint(PoseKey(i), PoseKey(j), z, model)); push_back(factor); } - void Graph::addHardConstraint(const Key& i, const Pose3& p) { - sharedFactor factor(new HardConstraint(i, p)); + void Graph::addHardConstraint(Index i, const Pose3& p) { + sharedFactor factor(new HardConstraint(PoseKey(i), p)); push_back(factor); } diff --git a/gtsam/slam/pose3SLAM.h b/gtsam/slam/pose3SLAM.h index f4516e647..eb4eb9573 100644 --- a/gtsam/slam/pose3SLAM.h +++ b/gtsam/slam/pose3SLAM.h @@ -18,10 +18,9 @@ #pragma once #include -#include #include #include -#include +#include #include #include #include @@ -31,10 +30,8 @@ namespace gtsam { /// Use pose3SLAM namespace for specific SLAM instance namespace pose3SLAM { - /// Creates a Key with data Pose3 and symbol 'x' - typedef TypedSymbol Key; - /// Creates a Values structure with type 'Key' - typedef Values Values; + /// Convenience function for constructing a pose key + inline Symbol PoseKey(Index j) { return Symbol('x', j); } /** * Create a circle of n 3D poses tangent to circle of radius R, first pose at (R,0) @@ -45,36 +42,33 @@ namespace gtsam { Values circle(size_t n, double R); /// A prior factor on Key with Pose3 data type. - typedef PriorFactor Prior; + typedef PriorFactor Prior; /// A factor to put constraints between two factors. - typedef BetweenFactor Constraint; + typedef BetweenFactor Constraint; /// A hard constraint would enforce that the given key would have the input value in the results. - typedef NonlinearEquality HardConstraint; + typedef NonlinearEquality HardConstraint; /// Graph - struct Graph: public NonlinearFactorGraph { + struct Graph: public NonlinearFactorGraph { /// Adds a factor between keys of the same type - void addPrior(const Key& i, const Pose3& p, - const SharedNoiseModel& model); + void addPrior(Index i, const Pose3& p, const SharedNoiseModel& model); /// Creates a between factor between keys i and j with a noise model with Pos3 z in the graph - void addConstraint(const Key& i, const Key& j, const Pose3& z, - const SharedNoiseModel& model); + void addConstraint(Index i, Index j, const Pose3& z, const SharedNoiseModel& model); /// Creates a hard constraint for key i with the given Pose3 p. - void addHardConstraint(const Key& i, const Pose3& p); + void addHardConstraint(Index i, const Pose3& p); }; /// Optimizer - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; } // pose3SLAM /** * Backwards compatibility */ - typedef pose3SLAM::Values Pose3Values; ///< Typedef for Values class for backwards compatibility typedef pose3SLAM::Prior Pose3Prior; ///< Typedef for Prior class for backwards compatibility typedef pose3SLAM::Constraint Pose3Factor; ///< Typedef for Constraint class for backwards compatibility typedef pose3SLAM::Graph Pose3Graph; ///< Typedef for Graph class for backwards compatibility diff --git a/gtsam/slam/simulated2D.h b/gtsam/slam/simulated2D.h index ffcfce04d..ca9c0cf15 100644 --- a/gtsam/slam/simulated2D.h +++ b/gtsam/slam/simulated2D.h @@ -19,7 +19,6 @@ #pragma once #include -#include #include #include @@ -30,59 +29,67 @@ namespace simulated2D { using namespace gtsam; // Simulated2D robots have no orientation, just a position - typedef TypedSymbol PoseKey; - typedef TypedSymbol PointKey; - typedef Values PoseValues; - typedef Values PointValues; + + /// Convenience function for constructing a pose key + inline Symbol PoseKey(Index j) { return Symbol('x', j); } + + /// Convenience function for constructing a landmark key + inline Symbol PointKey(Index j) { return Symbol('l', j); } /** - * Custom Values class that holds poses and points + * Custom Values class that holds poses and points, mainly used as a convenience for MATLAB wrapper */ - class Values: public TupleValues2 { + class Values: public gtsam::Values { + private: + int nrPoses_, nrPoints_; + public: - typedef TupleValues2 Base; ///< base class + typedef gtsam::Values Base; ///< base class typedef boost::shared_ptr sharedPoint; ///< shortcut to shared Point type /// Constructor - Values() { + Values() : nrPoses_(0), nrPoints_(0) { } /// Copy constructor Values(const Base& base) : - Base(base) { + Base(base), nrPoses_(0), nrPoints_(0) { } /// Insert a pose - void insertPose(const simulated2D::PoseKey& i, const Point2& p) { - insert(i, p); + void insertPose(Index j, const Point2& p) { + insert(PoseKey(j), p); + nrPoses_++; } /// Insert a point - void insertPoint(const simulated2D::PointKey& j, const Point2& p) { - insert(j, p); + void insertPoint(Index j, const Point2& p) { + insert(PointKey(j), p); + nrPoints_++; } /// Number of poses int nrPoses() const { - return this->first_.size(); + return nrPoses_; } /// Number of points int nrPoints() const { - return this->second_.size(); + return nrPoints_; } /// Return pose i - Point2 pose(const simulated2D::PoseKey& i) const { - return (*this)[i]; + Point2 pose(Index j) const { + return at(PoseKey(j)); } /// Return point j - Point2 point(const simulated2D::PointKey& j) const { - return (*this)[j]; + Point2 point(Index j) const { + return at(PointKey(j)); } }; + /// Prior on a single pose inline Point2 prior(const Point2& x) { return x; @@ -112,24 +119,23 @@ namespace simulated2D { /** * Unary factor encoding a soft prior on a vector */ - template - class GenericPrior: public NonlinearFactor1 { + template + class GenericPrior: public NoiseModelFactor1 { public: - typedef NonlinearFactor1 Base; ///< base class - typedef boost::shared_ptr > shared_ptr; - typedef typename KEY::Value Pose; ///< shortcut to Pose type + typedef NoiseModelFactor1 Base; ///< base class + typedef boost::shared_ptr > shared_ptr; + typedef VALUE Pose; ///< shortcut to Pose type - Pose z_; ///< prior mean + Pose measured_; ///< prior mean /// Create generic prior - GenericPrior(const Pose& z, const SharedNoiseModel& model, const KEY& key) : - NonlinearFactor1(model, key), z_(z) { + GenericPrior(const Pose& z, const SharedNoiseModel& model, Key key) : + Base(model, key), measured_(z) { } /// Return error and optional derivative - Vector evaluateError(const Pose& x, boost::optional H = - boost::none) const { - return (prior(x, H) - z_).vector(); + Vector evaluateError(const Pose& x, boost::optional H = boost::none) const { + return (prior(x, H) - measured_).vector(); } private: @@ -143,33 +149,32 @@ namespace simulated2D { template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(measured_); } }; /** * Binary factor simulating "odometry" between two Vectors */ - template - class GenericOdometry: public NonlinearFactor2 { + template + class GenericOdometry: public NoiseModelFactor2 { public: - typedef NonlinearFactor2 Base; ///< base class - typedef boost::shared_ptr > shared_ptr; - typedef typename KEY::Value Pose; ///< shortcut to Pose type + typedef NoiseModelFactor2 Base; ///< base class + typedef boost::shared_ptr > shared_ptr; + typedef VALUE Pose; ///< shortcut to Pose type - Pose z_; ///< odometry measurement + Pose measured_; ///< odometry measurement /// Create odometry - GenericOdometry(const Pose& z, const SharedNoiseModel& model, - const KEY& i1, const KEY& i2) : - NonlinearFactor2(model, i1, i2), z_(z) { + GenericOdometry(const Pose& measured, const SharedNoiseModel& model, Key key1, Key key2) : + Base(model, key1, key2), measured_(measured) { } /// Evaluate error and optionally return derivatives Vector evaluateError(const Pose& x1, const Pose& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return (odo(x1, x2, H1, H2) - z_).vector(); + return (odo(x1, x2, H1, H2) - measured_).vector(); } private: @@ -183,34 +188,33 @@ namespace simulated2D { template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(measured_); } }; /** * Binary factor simulating "measurement" between two Vectors */ - template - class GenericMeasurement: public NonlinearFactor2 { + template + class GenericMeasurement: public NoiseModelFactor2 { public: - typedef NonlinearFactor2 Base; ///< base class - typedef boost::shared_ptr > shared_ptr; - typedef typename XKEY::Value Pose; ///< shortcut to Pose type - typedef typename LKEY::Value Point; ///< shortcut to Point type + typedef NoiseModelFactor2 Base; ///< base class + typedef boost::shared_ptr > shared_ptr; + typedef POSE Pose; ///< shortcut to Pose type + typedef LANDMARK Landmark; ///< shortcut to Landmark type - Point z_; ///< Measurement + Landmark measured_; ///< Measurement /// Create measurement factor - GenericMeasurement(const Point& z, const SharedNoiseModel& model, - const XKEY& i, const LKEY& j) : - NonlinearFactor2(model, i, j), z_(z) { + GenericMeasurement(const Landmark& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey) : + Base(model, poseKey, landmarkKey), measured_(measured) { } /// Evaluate error and optionally return derivatives - Vector evaluateError(const Pose& x1, const Point& x2, + Vector evaluateError(const Pose& x1, const Landmark& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return (mea(x1, x2, H1, H2) - z_).vector(); + return (mea(x1, x2, H1, H2) - measured_).vector(); } private: @@ -224,18 +228,18 @@ namespace simulated2D { template void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(z_); + ar & BOOST_SERIALIZATION_NVP(measured_); } }; /** Typedefs for regular use */ - typedef GenericPrior Prior; - typedef GenericOdometry Odometry; - typedef GenericMeasurement Measurement; + typedef GenericPrior Prior; + typedef GenericOdometry Odometry; + typedef GenericMeasurement Measurement; // Specialization of a graph for this example domain // TODO: add functions to add factor types - class Graph : public NonlinearFactorGraph { + class Graph : public NonlinearFactorGraph { public: Graph() {} }; diff --git a/gtsam/slam/simulated2DConstraints.h b/gtsam/slam/simulated2DConstraints.h index 2521e86e4..b44062933 100644 --- a/gtsam/slam/simulated2DConstraints.h +++ b/gtsam/slam/simulated2DConstraints.h @@ -33,13 +33,13 @@ namespace simulated2D { namespace equality_constraints { /** Typedefs for regular use */ - typedef NonlinearEquality1 UnaryEqualityConstraint; - typedef NonlinearEquality1 UnaryEqualityPointConstraint; - typedef BetweenConstraint OdoEqualityConstraint; + typedef NonlinearEquality1 UnaryEqualityConstraint; + typedef NonlinearEquality1 UnaryEqualityPointConstraint; + typedef BetweenConstraint OdoEqualityConstraint; /** Equality between variables */ - typedef NonlinearEquality2 PoseEqualityConstraint; - typedef NonlinearEquality2 PointEqualityConstraint; + typedef NonlinearEquality2 PoseEqualityConstraint; + typedef NonlinearEquality2 PointEqualityConstraint; } // \namespace equality_constraints @@ -47,15 +47,14 @@ namespace simulated2D { /** * Unary inequality constraint forcing a coordinate to be greater/less than a fixed value (c) - * @tparam VALUES is the values structure for the graph - * @tparam KEY is the key type for the variable constrained + * @tparam VALUE is the value type for the variable constrained, e.g. Pose2, Point3, etc. * @tparam IDX is an index in tangent space to constrain, must be less than KEY::VALUE::Dim() */ - template - struct ScalarCoordConstraint1: public BoundingConstraint1 { - typedef BoundingConstraint1 Base; ///< Base class convenience typedef - typedef boost::shared_ptr > shared_ptr; ///< boost::shared_ptr convenience typedef - typedef typename KEY::Value Point; ///< Constrained variable type + template + struct ScalarCoordConstraint1: public BoundingConstraint1 { + typedef BoundingConstraint1 Base; ///< Base class convenience typedef + typedef boost::shared_ptr > shared_ptr; ///< boost::shared_ptr convenience typedef + typedef VALUE Point; ///< Constrained variable type virtual ~ScalarCoordConstraint1() {} @@ -66,9 +65,9 @@ namespace simulated2D { * @param isGreaterThan is a flag to set inequality as greater than or less than * @param mu is the penalty function gain */ - ScalarCoordConstraint1(const KEY& key, double c, + ScalarCoordConstraint1(Key key, double c, bool isGreaterThan, double mu = 1000.0) : - Base(key, c, isGreaterThan, mu) { + Base(key, c, isGreaterThan, mu) { } /** @@ -94,8 +93,8 @@ namespace simulated2D { }; /** typedefs for use with simulated2D systems */ - typedef ScalarCoordConstraint1 PoseXInequality; ///< Simulated2D domain example factor constraining X - typedef ScalarCoordConstraint1 PoseYInequality; ///< Simulated2D domain example factor constraining Y + typedef ScalarCoordConstraint1 PoseXInequality; ///< Simulated2D domain example factor constraining X + typedef ScalarCoordConstraint1 PoseYInequality; ///< Simulated2D domain example factor constraining Y /** * Trait for distance constraints to provide distance @@ -114,10 +113,10 @@ namespace simulated2D { * @tparam VALUES is the variable set for the graph * @tparam KEY is the type of the keys for the variables constrained */ - template - struct MaxDistanceConstraint : public BoundingConstraint2 { - typedef BoundingConstraint2 Base; ///< Base class for factor - typedef typename KEY::Value Point; ///< Type of variable constrained + template + struct MaxDistanceConstraint : public BoundingConstraint2 { + typedef BoundingConstraint2 Base; ///< Base class for factor + typedef VALUE Point; ///< Type of variable constrained virtual ~MaxDistanceConstraint() {} @@ -128,8 +127,8 @@ namespace simulated2D { * @param range_bound is the maximum range allowed between the variables * @param mu is the gain for the penalty function */ - MaxDistanceConstraint(const KEY& key1, const KEY& key2, double range_bound, double mu = 1000.0) - : Base(key1, key2, range_bound, false, mu) {} + MaxDistanceConstraint(Key key1, Key key2, double range_bound, double mu = 1000.0) : + Base(key1, key2, range_bound, false, mu) {} /** * computes the range with derivatives @@ -148,20 +147,19 @@ namespace simulated2D { } }; - typedef MaxDistanceConstraint PoseMaxDistConstraint; ///< Simulated2D domain example factor + typedef MaxDistanceConstraint PoseMaxDistConstraint; ///< Simulated2D domain example factor /** * Binary inequality constraint forcing a minimum range * NOTE: this is not a convex function! Be careful with initialization. - * @tparam VALUES is the variable set for the graph - * @tparam XKEY is the type of the pose key constrained - * @tparam PKEY is the type of the point key constrained + * @tparam POSE is the type of the pose value constrained + * @tparam POINT is the type of the point value constrained */ - template - struct MinDistanceConstraint : public BoundingConstraint2 { - typedef BoundingConstraint2 Base; ///< Base class for factor - typedef typename XKEY::Value Pose; ///< Type of pose variable constrained - typedef typename PKEY::Value Point; ///< Type of point variable constrained + template + struct MinDistanceConstraint : public BoundingConstraint2 { + typedef BoundingConstraint2 Base; ///< Base class for factor + typedef POSE Pose; ///< Type of pose variable constrained + typedef POINT Point; ///< Type of point variable constrained virtual ~MinDistanceConstraint() {} @@ -172,9 +170,9 @@ namespace simulated2D { * @param range_bound is the minimum range allowed between the variables * @param mu is the gain for the penalty function */ - MinDistanceConstraint(const XKEY& key1, const PKEY& key2, + MinDistanceConstraint(Key key1, Key key2, double range_bound, double mu = 1000.0) - : Base(key1, key2, range_bound, true, mu) {} + : Base(key1, key2, range_bound, true, mu) {} /** * computes the range with derivatives @@ -193,7 +191,7 @@ namespace simulated2D { } }; - typedef MinDistanceConstraint LandmarkAvoid; ///< Simulated2D domain example factor + typedef MinDistanceConstraint LandmarkAvoid; ///< Simulated2D domain example factor } // \namespace inequality_constraints diff --git a/gtsam/slam/simulated2DOriented.h b/gtsam/slam/simulated2DOriented.h index 18c0c96c2..242aab94b 100644 --- a/gtsam/slam/simulated2DOriented.h +++ b/gtsam/slam/simulated2DOriented.h @@ -19,7 +19,6 @@ #pragma once #include -#include #include #include @@ -28,31 +27,34 @@ namespace simulated2DOriented { using namespace gtsam; - // The types that take an oriented pose2 rather than point2 - typedef TypedSymbol PointKey; - typedef TypedSymbol PoseKey; - typedef Values PoseValues; - typedef Values PointValues; + /// Convenience function for constructing a pose key + inline Symbol PoseKey(Index j) { return Symbol('x', j); } + + /// Convenience function for constructing a landmark key + inline Symbol PointKey(Index j) { return Symbol('l', j); } /// Specialized Values structure with syntactic sugar for /// compatibility with matlab - class Values: public TupleValues2 { + class Values: public gtsam::Values { + int nrPoses_, nrPoints_; public: - Values() {} + Values() : nrPoses_(0), nrPoints_(0) {} - void insertPose(const PoseKey& i, const Pose2& p) { - insert(i, p); + void insertPose(Index j, const Pose2& p) { + insert(PoseKey(j), p); + nrPoses_++; } - void insertPoint(const PointKey& j, const Point2& p) { - insert(j, p); + void insertPoint(Index j, const Point2& p) { + insert(PointKey(j), p); + nrPoints_++; } - int nrPoses() const { return this->first_.size(); } - int nrPoints() const { return this->second_.size(); } + int nrPoses() const { return nrPoses_; } + int nrPoints() const { return nrPoints_; } - Pose2 pose(const PoseKey& i) const { return (*this)[i]; } - Point2 point(const PointKey& j) const { return (*this)[j]; } + Pose2 pose(Index j) const { return at(PoseKey(j)); } + Point2 point(Index j) const { return at(PointKey(j)); } }; //TODO:: point prior is not implemented right now @@ -75,21 +77,20 @@ namespace simulated2DOriented { boost::none, boost::optional H2 = boost::none); /// Unary factor encoding a soft prior on a vector - template - struct GenericPosePrior: public NonlinearFactor1 { + template + struct GenericPosePrior: public NoiseModelFactor1 { - Pose2 z_; ///< measurement + Pose2 measured_; ///< measurement /// Create generic pose prior - GenericPosePrior(const Pose2& z, const SharedNoiseModel& model, - const Key& key) : - NonlinearFactor1(model, key), z_(z) { + GenericPosePrior(const Pose2& measured, const SharedNoiseModel& model, Key key) : + NoiseModelFactor1(model, key), measured_(measured) { } /// Evaluate error and optionally derivative Vector evaluateError(const Pose2& x, boost::optional H = boost::none) const { - return z_.localCoordinates(prior(x, H)); + return measured_.localCoordinates(prior(x, H)); } }; @@ -97,31 +98,31 @@ namespace simulated2DOriented { /** * Binary factor simulating "odometry" between two Vectors */ - template - struct GenericOdometry: public NonlinearFactor2 { - Pose2 z_; ///< Between measurement for odometry factor + template + struct GenericOdometry: public NoiseModelFactor2 { + Pose2 measured_; ///< Between measurement for odometry factor /** * Creates an odometry factor between two poses */ - GenericOdometry(const Pose2& z, const SharedNoiseModel& model, - const KEY& i1, const KEY& i2) : - NonlinearFactor2(model, i1, i2), z_(z) { + GenericOdometry(const Pose2& measured, const SharedNoiseModel& model, + Key i1, Key i2) : + NoiseModelFactor2(model, i1, i2), measured_(measured) { } /// Evaluate error and optionally derivative - Vector evaluateError(const Pose2& x1, const Pose2& x2, + Vector evaluateError(const VALUE& x1, const VALUE& x2, boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { - return z_.localCoordinates(odo(x1, x2, H1, H2)); + return measured_.localCoordinates(odo(x1, x2, H1, H2)); } }; - typedef GenericOdometry Odometry; + typedef GenericOdometry Odometry; /// Graph specialization for syntactic sugar use with matlab - class Graph : public NonlinearFactorGraph { + class Graph : public NonlinearFactorGraph { public: Graph() {} // TODO: add functions to add factors diff --git a/gtsam/slam/simulated3D.h b/gtsam/slam/simulated3D.h index 32f05e967..7b4dfce37 100644 --- a/gtsam/slam/simulated3D.h +++ b/gtsam/slam/simulated3D.h @@ -23,8 +23,7 @@ #include #include #include -#include -#include +#include // \namespace @@ -37,12 +36,11 @@ namespace simulated3D { * the simulated2D domain. */ -typedef gtsam::TypedSymbol PoseKey; -typedef gtsam::TypedSymbol PointKey; + /// Convenience function for constructing a pose key + inline Symbol PoseKey(Index j) { return Symbol('x', j); } -typedef Values PoseValues; -typedef Values PointValues; -typedef TupleValues2 Values; + /// Convenience function for constructing a pose key + inline Symbol PointKey(Index j) { return Symbol('l', j); } /** * Prior on a single pose @@ -66,19 +64,18 @@ Point3 mea(const Point3& x, const Point3& l, /** * A prior factor on a single linear robot pose */ -struct PointPrior3D: public NonlinearFactor1 { +struct PointPrior3D: public NoiseModelFactor1 { - Point3 z_; ///< The prior pose value for the variable attached to this factor + Point3 measured_; ///< The prior pose value for the variable attached to this factor /** * Constructor for a prior factor - * @param z is the measured/prior position for the pose + * @param measured is the measured/prior position for the pose * @param model is the measurement model for the factor (Dimension: 3) - * @param j is the key for the pose + * @param key is the key for the pose */ - PointPrior3D(const Point3& z, - const SharedNoiseModel& model, const PoseKey& j) : - NonlinearFactor1 (model, j), z_(z) { + PointPrior3D(const Point3& measured, const SharedNoiseModel& model, Key key) : + NoiseModelFactor1 (model, key), measured_(measured) { } /** @@ -89,31 +86,28 @@ struct PointPrior3D: public NonlinearFactor1 { * @return Vector error between prior value and x (Dimension: 3) */ Vector evaluateError(const Point3& x, boost::optional H = - boost::none) { - return (prior(x, H) - z_).vector(); + boost::none) const { + return (prior(x, H) - measured_).vector(); } }; /** * Models a linear 3D measurement between 3D points */ -struct Simulated3DMeasurement: public NonlinearFactor2 { +struct Simulated3DMeasurement: public NoiseModelFactor2 { - Point3 z_; ///< Linear displacement between a pose and landmark + Point3 measured_; ///< Linear displacement between a pose and landmark /** * Creates a measurement factor with a given measurement - * @param z is the measurement, a linear displacement between poses and landmarks + * @param measured is the measurement, a linear displacement between poses and landmarks * @param model is a measurement model for the factor (Dimension: 3) - * @param j1 is the pose key of the robot - * @param j2 is the point key for the landmark + * @param poseKey is the pose key of the robot + * @param pointKey is the point key for the landmark */ - Simulated3DMeasurement(const Point3& z, - const SharedNoiseModel& model, PoseKey& j1, PointKey j2) : - NonlinearFactor2 ( - model, j1, j2), z_(z) { - } + Simulated3DMeasurement(const Point3& measured, const SharedNoiseModel& model, + Key poseKey, Key pointKey) : + NoiseModelFactor2(model, poseKey, pointKey), measured_(measured) {} /** * Error function with optional derivatives @@ -123,9 +117,9 @@ PoseKey, PointKey> { * @param H2 is an optional Jacobian matrix in terms of x2 (Dimension: 3x3) * @return vector error between measurement and prediction (Dimension: 3) */ - Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional< - Matrix&> H1 = boost::none, boost::optional H2 = boost::none) { - return (mea(x1, x2, H1, H2) - z_).vector(); + Vector evaluateError(const Point3& x1, const Point3& x2, + boost::optional H1 = boost::none, boost::optional H2 = boost::none) const { + return (mea(x1, x2, H1, H2) - measured_).vector(); } }; diff --git a/gtsam/slam/smallExample.cpp b/gtsam/slam/smallExample.cpp index 40d3e98ff..170f3ef03 100644 --- a/gtsam/slam/smallExample.cpp +++ b/gtsam/slam/smallExample.cpp @@ -24,9 +24,6 @@ using namespace std; -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include #include #include @@ -39,7 +36,10 @@ using namespace std; namespace gtsam { namespace example { - typedef boost::shared_ptr > shared; + using simulated2D::PoseKey; + using simulated2D::PointKey; + + typedef boost::shared_ptr shared; static SharedDiagonal sigma1_0 = noiseModel::Isotropic::Sigma(2,1.0); static SharedDiagonal sigma0_1 = noiseModel::Isotropic::Sigma(2,0.1); @@ -49,6 +49,9 @@ namespace example { static const Index _l1_=0, _x1_=1, _x2_=2; static const Index _x_=0, _y_=1, _z_=2; + Key kx(size_t i) { return Symbol('x',i); } + Key kl(size_t i) { return Symbol('l',i); } + /* ************************************************************************* */ boost::shared_ptr sharedNonlinearFactorGraph() { // Create @@ -57,22 +60,22 @@ namespace example { // prior on x1 Point2 mu; - shared f1(new simulated2D::Prior(mu, sigma0_1, 1)); + shared f1(new simulated2D::Prior(mu, sigma0_1, PoseKey(1))); nlfg->push_back(f1); // odometry between x1 and x2 Point2 z2(1.5, 0); - shared f2(new simulated2D::Odometry(z2, sigma0_1, 1, 2)); + shared f2(new simulated2D::Odometry(z2, sigma0_1, PoseKey(1), PoseKey(2))); nlfg->push_back(f2); // measurement between x1 and l1 Point2 z3(0, -1); - shared f3(new simulated2D::Measurement(z3, sigma0_2, 1, 1)); + shared f3(new simulated2D::Measurement(z3, sigma0_2, PoseKey(1), PointKey(1))); nlfg->push_back(f3); // measurement between x2 and l1 Point2 z4(-1.5, -1.); - shared f4(new simulated2D::Measurement(z4, sigma0_2, 2, 1)); + shared f4(new simulated2D::Measurement(z4, sigma0_2, PoseKey(2), PointKey(1))); nlfg->push_back(f4); return nlfg; @@ -86,9 +89,9 @@ namespace example { /* ************************************************************************* */ Values createValues() { Values c; - c.insert(simulated2D::PoseKey(1), Point2(0.0, 0.0)); - c.insert(simulated2D::PoseKey(2), Point2(1.5, 0.0)); - c.insert(simulated2D::PointKey(1), Point2(0.0, -1.0)); + c.insert(PoseKey(1), Point2(0.0, 0.0)); + c.insert(PoseKey(2), Point2(1.5, 0.0)); + c.insert(PointKey(1), Point2(0.0, -1.0)); return c; } @@ -104,9 +107,9 @@ namespace example { /* ************************************************************************* */ boost::shared_ptr sharedNoisyValues() { boost::shared_ptr c(new Values); - c->insert(simulated2D::PoseKey(1), Point2(0.1, 0.1)); - c->insert(simulated2D::PoseKey(2), Point2(1.4, 0.2)); - c->insert(simulated2D::PointKey(1), Point2(0.1, -1.1)); + c->insert(PoseKey(1), Point2(0.1, 0.1)); + c->insert(PoseKey(2), Point2(1.4, 0.2)); + c->insert(PointKey(1), Point2(0.1, -1.1)); return c; } @@ -118,18 +121,18 @@ namespace example { /* ************************************************************************* */ VectorValues createCorrectDelta(const Ordering& ordering) { VectorValues c(vector(3,2)); - c[ordering["l1"]] = Vector_(2, -0.1, 0.1); - c[ordering["x1"]] = Vector_(2, -0.1, -0.1); - c[ordering["x2"]] = Vector_(2, 0.1, -0.2); + c[ordering[kl(1)]] = Vector_(2, -0.1, 0.1); + c[ordering[kx(1)]] = Vector_(2, -0.1, -0.1); + c[ordering[kx(2)]] = Vector_(2, 0.1, -0.2); return c; } /* ************************************************************************* */ VectorValues createZeroDelta(const Ordering& ordering) { VectorValues c(vector(3,2)); - c[ordering["l1"]] = zero(2); - c[ordering["x1"]] = zero(2); - c[ordering["x2"]] = zero(2); + c[ordering[kl(1)]] = zero(2); + c[ordering[kx(1)]] = zero(2); + c[ordering[kx(2)]] = zero(2); return c; } @@ -141,16 +144,16 @@ namespace example { SharedDiagonal unit2 = noiseModel::Unit::Create(2); // linearized prior on x1: c[_x1_]+x1=0 i.e. x1=-c[_x1_] - fg.add(ordering["x1"], 10*eye(2), -1.0*ones(2), unit2); + fg.add(ordering[kx(1)], 10*eye(2), -1.0*ones(2), unit2); // odometry between x1 and x2: x2-x1=[0.2;-0.1] - fg.add(ordering["x1"], -10*eye(2),ordering["x2"], 10*eye(2), Vector_(2, 2.0, -1.0), unit2); + fg.add(ordering[kx(1)], -10*eye(2),ordering[kx(2)], 10*eye(2), Vector_(2, 2.0, -1.0), unit2); // measurement between x1 and l1: l1-x1=[0.0;0.2] - fg.add(ordering["x1"], -5*eye(2), ordering["l1"], 5*eye(2), Vector_(2, 0.0, 1.0), unit2); + fg.add(ordering[kx(1)], -5*eye(2), ordering[kl(1)], 5*eye(2), Vector_(2, 0.0, 1.0), unit2); // measurement between x2 and l1: l1-x2=[-0.2;0.3] - fg.add(ordering["x2"], -5*eye(2), ordering["l1"], 5*eye(2), Vector_(2, -1.0, 1.5), unit2); + fg.add(ordering[kx(2)], -5*eye(2), ordering[kl(1)], 5*eye(2), Vector_(2, -1.0, 1.5), unit2); return *fg.dynamicCastFactors >(); } @@ -195,18 +198,15 @@ namespace example { 0.0, cos(v.y())); } - struct UnaryFactor: public gtsam::NonlinearFactor1 { + struct UnaryFactor: public gtsam::NoiseModelFactor1 { Point2 z_; - UnaryFactor(const Point2& z, const SharedNoiseModel& model, - const simulated2D::PoseKey& key) : - gtsam::NonlinearFactor1(model, key), z_(z) { + UnaryFactor(const Point2& z, const SharedNoiseModel& model, Key key) : + gtsam::NoiseModelFactor1(model, key), z_(z) { } - Vector evaluateError(const Point2& x, boost::optional A = - boost::none) const { + Vector evaluateError(const Point2& x, boost::optional A = boost::none) const { if (A) *A = H(x); return (h(x) - z_).vector(); } @@ -221,7 +221,7 @@ namespace example { Vector z = Vector_(2, 1.0, 0.0); double sigma = 0.1; boost::shared_ptr factor( - new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), 1)); + new smallOptimize::UnaryFactor(z, noiseModel::Isotropic::Sigma(2,sigma), PoseKey(1))); fg->push_back(factor); return fg; } @@ -239,23 +239,23 @@ namespace example { // prior on x1 Point2 x1(1.0, 0.0); - shared prior(new simulated2D::Prior(x1, sigma1_0, 1)); + shared prior(new simulated2D::Prior(x1, sigma1_0, PoseKey(1))); nlfg.push_back(prior); poses.insert(simulated2D::PoseKey(1), x1); for (int t = 2; t <= T; t++) { // odometry between x_t and x_{t-1} Point2 odo(1.0, 0.0); - shared odometry(new simulated2D::Odometry(odo, sigma1_0, t - 1, t)); + shared odometry(new simulated2D::Odometry(odo, sigma1_0, PoseKey(t - 1), PoseKey(t))); nlfg.push_back(odometry); // measurement on x_t is like perfect GPS Point2 xt(t, 0); - shared measurement(new simulated2D::Prior(xt, sigma1_0, t)); + shared measurement(new simulated2D::Prior(xt, sigma1_0, PoseKey(t))); nlfg.push_back(measurement); // initial estimate - poses.insert(simulated2D::PoseKey(t), xt); + poses.insert(PoseKey(t), xt); } return make_pair(nlfg, poses); @@ -415,15 +415,15 @@ namespace example { /* ************************************************************************* */ // Create key for simulated planar graph - simulated2D::PoseKey key(int x, int y) { - return simulated2D::PoseKey(1000*x+y); + Symbol key(int x, int y) { + return PoseKey(1000*x+y); } /* ************************************************************************* */ boost::tuple, Ordering, VectorValues> planarGraph(size_t N) { // create empty graph - NonlinearFactorGraph nlfg; + NonlinearFactorGraph nlfg; // Create almost hard constraint on x11, sigma=0 will work for PCG not for normal shared constraint(new simulated2D::Prior(Point2(1.0, 1.0), sharedSigma(2,1e-3), key(1,1))); diff --git a/gtsam/slam/smallExample.h b/gtsam/slam/smallExample.h index 1889ee1b5..96dcbaf09 100644 --- a/gtsam/slam/smallExample.h +++ b/gtsam/slam/smallExample.h @@ -28,8 +28,7 @@ namespace gtsam { namespace example { - typedef simulated2D::Values Values; - typedef NonlinearFactorGraph Graph; + typedef NonlinearFactorGraph Graph; /** * Create small example for non-linear factor graph diff --git a/gtsam/slam/tests/testAntiFactor.cpp b/gtsam/slam/tests/testAntiFactor.cpp index a7f91c320..183b0c600 100644 --- a/gtsam/slam/tests/testAntiFactor.cpp +++ b/gtsam/slam/tests/testAntiFactor.cpp @@ -27,6 +27,8 @@ using namespace std; using namespace gtsam; +using pose3SLAM::PoseKey; + /* ************************************************************************* */ TEST( AntiFactor, NegativeHessian) { @@ -39,18 +41,18 @@ TEST( AntiFactor, NegativeHessian) SharedNoiseModel sigma(noiseModel::Unit::Create(Pose3::Dim())); // Create a configuration corresponding to the ground truth - boost::shared_ptr values(new pose3SLAM::Values()); - values->insert(1, pose1); - values->insert(2, pose2); + boost::shared_ptr values(new Values()); + values->insert(PoseKey(1), pose1); + values->insert(PoseKey(2), pose2); // Define an elimination ordering Ordering::shared_ptr ordering(new Ordering()); - ordering->insert(pose3SLAM::Key(1), 0); - ordering->insert(pose3SLAM::Key(2), 1); + ordering->insert(PoseKey(1), 0); + ordering->insert(PoseKey(2), 1); // Create a "standard" factor - BetweenFactor::shared_ptr originalFactor(new BetweenFactor(1, 2, z, sigma)); + BetweenFactor::shared_ptr originalFactor(new BetweenFactor(PoseKey(1), PoseKey(2), z, sigma)); // Linearize it into a Jacobian Factor GaussianFactor::shared_ptr originalJacobian = originalFactor->linearize(*values, *ordering); @@ -59,7 +61,7 @@ TEST( AntiFactor, NegativeHessian) HessianFactor::shared_ptr originalHessian = HessianFactor::shared_ptr(new HessianFactor(*originalJacobian)); // Create the AntiFactor version of the original nonlinear factor - AntiFactor::shared_ptr antiFactor(new AntiFactor(originalFactor)); + AntiFactor::shared_ptr antiFactor(new AntiFactor(originalFactor)); // Linearize the AntiFactor into a Hessian Factor GaussianFactor::shared_ptr antiGaussian = antiFactor->linearize(*values, *ordering); @@ -100,9 +102,9 @@ TEST( AntiFactor, EquivalentBayesNet) graph->addConstraint(1, 2, pose1.between(pose2), sigma); // Create a configuration corresponding to the ground truth - boost::shared_ptr values(new pose3SLAM::Values()); - values->insert(1, pose1); - values->insert(2, pose2); + boost::shared_ptr values(new Values()); + values->insert(PoseKey(1), pose1); + values->insert(PoseKey(2), pose2); // Define an elimination ordering Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values); @@ -115,11 +117,11 @@ TEST( AntiFactor, EquivalentBayesNet) VectorValues expectedDeltas = optimize(*expectedBayesNet); // Add an additional factor between Pose1 and Pose2 - pose3SLAM::Constraint::shared_ptr f1(new pose3SLAM::Constraint(1, 2, z, sigma)); + pose3SLAM::Constraint::shared_ptr f1(new pose3SLAM::Constraint(PoseKey(1), PoseKey(2), z, sigma)); graph->push_back(f1); // Add the corresponding AntiFactor between Pose1 and Pose2 - AntiFactor::shared_ptr f2(new AntiFactor(f1)); + AntiFactor::shared_ptr f2(new AntiFactor(f1)); graph->push_back(f2); // Again, Eliminate into a BayesNet diff --git a/gtsam/slam/tests/testGeneralSFMFactor.cpp b/gtsam/slam/tests/testGeneralSFMFactor.cpp index 0d5c7da9f..7a8dc4825 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor.cpp @@ -12,16 +12,12 @@ #include using namespace boost; -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include #include #include #include #include #include -#include #include #include @@ -29,28 +25,23 @@ using namespace std; using namespace gtsam; typedef PinholeCamera GeneralCamera; -typedef TypedSymbol CameraKey; -typedef TypedSymbol PointKey; -typedef Values CameraConfig; -typedef Values PointConfig; -typedef TupleValues2 VisualValues; -typedef GeneralSFMFactor Projection; -typedef NonlinearEquality CameraConstraint; -typedef NonlinearEquality Point3Constraint; +typedef GeneralSFMFactor Projection; +typedef NonlinearEquality CameraConstraint; +typedef NonlinearEquality Point3Constraint; -class Graph: public NonlinearFactorGraph { +class Graph: public NonlinearFactorGraph { public: - void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) { - push_back(boost::make_shared(z, model, i, j)); + void addMeasurement(int i, int j, const Point2& z, const SharedNoiseModel& model) { + push_back(boost::make_shared(z, model, Symbol('x',i), Symbol('l',j))); } void addCameraConstraint(int j, const GeneralCamera& p) { - boost::shared_ptr factor(new CameraConstraint(j, p)); + boost::shared_ptr factor(new CameraConstraint(Symbol('x',j), p)); push_back(factor); } void addPoint3Constraint(int j, const Point3& p) { - boost::shared_ptr factor(new Point3Constraint(j, p)); + boost::shared_ptr factor(new Point3Constraint(Symbol('l',j), p)); push_back(factor); } @@ -71,7 +62,7 @@ double getGaussian() return sqrt(-2.0f * (double)log(S) / S) * V1; } -typedef NonlinearOptimizer Optimizer; +typedef NonlinearOptimizer Optimizer; const SharedNoiseModel sigma1(noiseModel::Unit::Create(1)); @@ -80,7 +71,7 @@ TEST( GeneralSFMFactor, equals ) { // Create two identical factors and make sure they're equal Vector z = Vector_(2,323.,240.); - const int cameraFrameNumber=1, landmarkNumber=1; + const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); @@ -94,17 +85,15 @@ TEST( GeneralSFMFactor, equals ) /* ************************************************************************* */ TEST( GeneralSFMFactor, error ) { Point2 z(3.,0.); - const int cameraFrameNumber=1, landmarkNumber=1; const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); - boost::shared_ptr - factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + boost::shared_ptr factor(new Projection(z, sigma, Symbol('x',1), Symbol('l',1))); // For the following configuration, the factor predicts 320,240 - VisualValues values; + Values values; Rot3 R; Point3 t1(0,0,-6); Pose3 x1(R,t1); - values.insert(1, GeneralCamera(x1)); - Point3 l1; values.insert(1, l1); + values.insert(Symbol('x',1), GeneralCamera(x1)); + Point3 l1; values.insert(Symbol('l',1), l1); EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); } @@ -145,10 +134,9 @@ vector genCameraVariableCalibration() { } shared_ptr getOrdering(const vector& X, const vector& L) { - list keys; - for ( size_t i = 0 ; i < L.size() ; ++i ) keys.push_back(Symbol('l', i)) ; - for ( size_t i = 0 ; i < X.size() ; ++i ) keys.push_back(Symbol('x', i)) ; - shared_ptr ordering(new Ordering(keys)); + shared_ptr ordering(new Ordering); + for ( size_t i = 0 ; i < L.size() ; ++i ) ordering->push_back(Symbol('l', i)) ; + for ( size_t i = 0 ; i < X.size() ; ++i ) ordering->push_back(Symbol('x', i)) ; return ordering ; } @@ -172,15 +160,15 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new VisualValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert((int)i, X[i]) ; + values->insert(Symbol('x',i), X[i]) ; for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(i, pt) ; + values->insert(Symbol('l',i), pt) ; } graph->addCameraConstraint(0, X[0]); @@ -211,9 +199,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new VisualValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert((int)i, X[i]) ; + values->insert(Symbol('x',i), X[i]) ; // add noise only to the first landmark for ( size_t i = 0 ; i < L.size() ; ++i ) { @@ -221,10 +209,10 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(i, pt) ; + values->insert(Symbol('l',i), pt) ; } else { - values->insert(i, L[i]) ; + values->insert(Symbol('l',i), L[i]) ; } } @@ -257,16 +245,16 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { const size_t nMeasurements = L.size()*X.size(); - boost::shared_ptr values(new VisualValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert((int)i, X[i]) ; + values->insert(Symbol('x',i), X[i]) ; for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); //Point3 pt(L[i].x(), L[i].y(), L[i].z()); - values->insert(i, pt) ; + values->insert(Symbol('l',i), pt) ; } for ( size_t i = 0 ; i < X.size() ; ++i ) @@ -300,7 +288,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { const size_t nMeasurements = L.size()*X.size(); - boost::shared_ptr values(new VisualValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) { const double rot_noise = 1e-5, @@ -308,7 +296,7 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { focal_noise = 1, skew_noise = 1e-5; if ( i == 0 ) { - values->insert((int)i, X[i]) ; + values->insert(Symbol('x',i), X[i]) ; } else { @@ -319,12 +307,12 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { skew_noise, // s trans_noise, trans_noise // ux, uy ) ; - values->insert((int)i, X[i].retract(delta)) ; + values->insert(Symbol('x',i), X[i].retract(delta)) ; } } for ( size_t i = 0 ; i < L.size() ; ++i ) { - values->insert(i, L[i]) ; + values->insert(Symbol('l',i), L[i]) ; } // fix X0 and all landmarks, allow only the X[1] to move @@ -362,16 +350,16 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new VisualValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert((int)i, X[i]) ; + values->insert(Symbol('x',i), X[i]) ; // add noise only to the first landmark for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(i, pt) ; + values->insert(Symbol('l',i), pt) ; } graph->addCameraConstraint(0, X[0]); diff --git a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp index b402b1d0b..f762505a3 100644 --- a/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp +++ b/gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp @@ -12,16 +12,12 @@ #include using namespace boost; -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include #include #include #include #include #include -#include #include #include @@ -29,29 +25,24 @@ using namespace std; using namespace gtsam; typedef PinholeCamera GeneralCamera; -typedef TypedSymbol CameraKey; -typedef TypedSymbol PointKey; -typedef Values CameraConfig; -typedef Values PointConfig; -typedef TupleValues2 VisualValues; -typedef GeneralSFMFactor Projection; -typedef NonlinearEquality CameraConstraint; -typedef NonlinearEquality Point3Constraint; +typedef GeneralSFMFactor Projection; +typedef NonlinearEquality CameraConstraint; +typedef NonlinearEquality Point3Constraint; /* ************************************************************************* */ -class Graph: public NonlinearFactorGraph { +class Graph: public NonlinearFactorGraph { public: - void addMeasurement(const CameraKey& i, const PointKey& j, const Point2& z, const SharedNoiseModel& model) { - push_back(boost::make_shared(z, model, i, j)); + void addMeasurement(const int& i, const int& j, const Point2& z, const SharedNoiseModel& model) { + push_back(boost::make_shared(z, model, Symbol('x',i), Symbol('l',j))); } void addCameraConstraint(int j, const GeneralCamera& p) { - boost::shared_ptr factor(new CameraConstraint(j, p)); + boost::shared_ptr factor(new CameraConstraint(Symbol('x', j), p)); push_back(factor); } void addPoint3Constraint(int j, const Point3& p) { - boost::shared_ptr factor(new Point3Constraint(j, p)); + boost::shared_ptr factor(new Point3Constraint(Symbol('l', j), p)); push_back(factor); } @@ -72,7 +63,7 @@ double getGaussian() return sqrt(-2.0f * (double)log(S) / S) * V1; } -typedef NonlinearOptimizer Optimizer; +typedef NonlinearOptimizer Optimizer; const SharedNoiseModel sigma1(noiseModel::Unit::Create(1)); @@ -81,7 +72,7 @@ TEST( GeneralSFMFactor, equals ) { // Create two identical factors and make sure they're equal Vector z = Vector_(2,323.,240.); - const int cameraFrameNumber=1, landmarkNumber=1; + const Symbol cameraFrameNumber('x',1), landmarkNumber('l',1); const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr factor1(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); @@ -95,17 +86,16 @@ TEST( GeneralSFMFactor, equals ) /* ************************************************************************* */ TEST( GeneralSFMFactor, error ) { Point2 z(3.,0.); - const int cameraFrameNumber=1, landmarkNumber=1; const SharedNoiseModel sigma(noiseModel::Unit::Create(1)); boost::shared_ptr - factor(new Projection(z, sigma, cameraFrameNumber, landmarkNumber)); + factor(new Projection(z, sigma, Symbol('x',1), Symbol('l',1))); // For the following configuration, the factor predicts 320,240 - VisualValues values; + Values values; Rot3 R; Point3 t1(0,0,-6); Pose3 x1(R,t1); - values.insert(1, GeneralCamera(x1)); - Point3 l1; values.insert(1, l1); + values.insert(Symbol('x',1), GeneralCamera(x1)); + Point3 l1; values.insert(Symbol('l',1), l1); EXPECT(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(values))); } @@ -147,10 +137,9 @@ vector genCameraVariableCalibration() { } shared_ptr getOrdering(const vector& X, const vector& L) { - list keys; - for ( size_t i = 0 ; i < L.size() ; ++i ) keys.push_back(Symbol('l', i)) ; - for ( size_t i = 0 ; i < X.size() ; ++i ) keys.push_back(Symbol('x', i)) ; - shared_ptr ordering(new Ordering(keys)); + shared_ptr ordering(new Ordering); + for ( size_t i = 0 ; i < L.size() ; ++i ) ordering->push_back(Symbol('l', i)) ; + for ( size_t i = 0 ; i < X.size() ; ++i ) ordering->push_back(Symbol('x', i)) ; return ordering ; } @@ -173,15 +162,15 @@ TEST( GeneralSFMFactor, optimize_defaultK ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new VisualValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert((int)i, X[i]) ; + values->insert(Symbol('x',i), X[i]) ; for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(i, pt) ; + values->insert(Symbol('l',i), pt) ; } graph->addCameraConstraint(0, X[0]); @@ -212,9 +201,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new VisualValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert((int)i, X[i]) ; + values->insert(Symbol('x',i), X[i]) ; // add noise only to the first landmark for ( size_t i = 0 ; i < L.size() ; ++i ) { @@ -222,10 +211,10 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(i, pt) ; + values->insert(Symbol('l',i), pt) ; } else { - values->insert(i, L[i]) ; + values->insert(Symbol('l',i), L[i]) ; } } @@ -258,16 +247,16 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) { const size_t nMeasurements = L.size()*X.size(); - boost::shared_ptr values(new VisualValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert((int)i, X[i]) ; + values->insert(Symbol('x',i), X[i]) ; for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); //Point3 pt(L[i].x(), L[i].y(), L[i].z()); - values->insert(i, pt) ; + values->insert(Symbol('l',i), pt) ; } for ( size_t i = 0 ; i < X.size() ; ++i ) @@ -301,13 +290,13 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { const size_t nMeasurements = L.size()*X.size(); - boost::shared_ptr values(new VisualValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) { const double rot_noise = 1e-5, trans_noise = 1e-3, focal_noise = 1, distort_noise = 1e-3; if ( i == 0 ) { - values->insert((int)i, X[i]) ; + values->insert(Symbol('x',i), X[i]) ; } else { @@ -316,12 +305,12 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) { trans_noise, trans_noise, trans_noise, // translation focal_noise, distort_noise, distort_noise // f, k1, k2 ) ; - values->insert((int)i, X[i].retract(delta)) ; + values->insert(Symbol('x',i), X[i].retract(delta)) ; } } for ( size_t i = 0 ; i < L.size() ; ++i ) { - values->insert(i, L[i]) ; + values->insert(Symbol('l',i), L[i]) ; } // fix X0 and all landmarks, allow only the X[1] to move @@ -358,16 +347,16 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) { // add initial const double noise = baseline*0.1; - boost::shared_ptr values(new VisualValues); + boost::shared_ptr values(new Values); for ( size_t i = 0 ; i < X.size() ; ++i ) - values->insert((int)i, X[i]) ; + values->insert(Symbol('x',i), X[i]) ; // add noise only to the first landmark for ( size_t i = 0 ; i < L.size() ; ++i ) { Point3 pt(L[i].x()+noise*getGaussian(), L[i].y()+noise*getGaussian(), L[i].z()+noise*getGaussian()); - values->insert(i, pt) ; + values->insert(Symbol('l',i), pt) ; } graph->addCameraConstraint(0, X[0]); diff --git a/gtsam/slam/tests/testPlanarSLAM.cpp b/gtsam/slam/tests/testPlanarSLAM.cpp index 2362e0ae5..3daf5f4ab 100644 --- a/gtsam/slam/tests/testPlanarSLAM.cpp +++ b/gtsam/slam/tests/testPlanarSLAM.cpp @@ -23,6 +23,7 @@ using namespace std; using namespace gtsam; +using namespace planarSLAM; // some shared test values static Pose2 x1, x2(1, 1, 0), x3(1, 1, M_PI_4); @@ -36,7 +37,7 @@ SharedNoiseModel /* ************************************************************************* */ TEST( planarSLAM, PriorFactor_equals ) { - planarSLAM::Prior factor1(2, x1, I3), factor2(2, x2, I3); + planarSLAM::Prior factor1(PoseKey(2), x1, I3), factor2(PoseKey(2), x2, I3); EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5)); @@ -47,12 +48,12 @@ TEST( planarSLAM, BearingFactor ) { // Create factor Rot2 z = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 - planarSLAM::Bearing factor(2, 3, z, sigma); + planarSLAM::Bearing factor(PoseKey(2), PointKey(3), z, sigma); // create config planarSLAM::Values c; - c.insert(2, x2); - c.insert(3, l3); + c.insert(PoseKey(2), x2); + c.insert(PointKey(3), l3); // Check error Vector actual = factor.unwhitenedError(c); @@ -63,8 +64,8 @@ TEST( planarSLAM, BearingFactor ) TEST( planarSLAM, BearingFactor_equals ) { planarSLAM::Bearing - factor1(2, 3, Rot2::fromAngle(0.1), sigma), - factor2(2, 3, Rot2::fromAngle(2.3), sigma); + factor1(PoseKey(2), PointKey(3), Rot2::fromAngle(0.1), sigma), + factor2(PoseKey(2), PointKey(3), Rot2::fromAngle(2.3), sigma); EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5)); @@ -75,12 +76,12 @@ TEST( planarSLAM, RangeFactor ) { // Create factor double z(sqrt(2) - 0.22); // h(x) - z = 0.22 - planarSLAM::Range factor(2, 3, z, sigma); + planarSLAM::Range factor(PoseKey(2), PointKey(3), z, sigma); // create config planarSLAM::Values c; - c.insert(2, x2); - c.insert(3, l3); + c.insert(PoseKey(2), x2); + c.insert(PointKey(3), l3); // Check error Vector actual = factor.unwhitenedError(c); @@ -90,7 +91,7 @@ TEST( planarSLAM, RangeFactor ) /* ************************************************************************* */ TEST( planarSLAM, RangeFactor_equals ) { - planarSLAM::Range factor1(2, 3, 1.2, sigma), factor2(2, 3, 7.2, sigma); + planarSLAM::Range factor1(PoseKey(2), PointKey(3), 1.2, sigma), factor2(PoseKey(2), PointKey(3), 7.2, sigma); EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5)); @@ -102,12 +103,12 @@ TEST( planarSLAM, BearingRangeFactor ) // Create factor Rot2 r = Rot2::fromAngle(M_PI_4 + 0.1); // h(x) - z = -0.1 double b(sqrt(2) - 0.22); // h(x) - z = 0.22 - planarSLAM::BearingRange factor(2, 3, r, b, sigma2); + planarSLAM::BearingRange factor(PoseKey(2), PointKey(3), r, b, sigma2); // create config planarSLAM::Values c; - c.insert(2, x2); - c.insert(3, l3); + c.insert(PoseKey(2), x2); + c.insert(PointKey(3), l3); // Check error Vector actual = factor.unwhitenedError(c); @@ -118,8 +119,8 @@ TEST( planarSLAM, BearingRangeFactor ) TEST( planarSLAM, BearingRangeFactor_equals ) { planarSLAM::BearingRange - factor1(2, 3, Rot2::fromAngle(0.1), 7.3, sigma2), - factor2(2, 3, Rot2::fromAngle(3), 2.0, sigma2); + factor1(PoseKey(2), PointKey(3), Rot2::fromAngle(0.1), 7.3, sigma2), + factor2(PoseKey(2), PointKey(3), Rot2::fromAngle(3), 2.0, sigma2); EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5)); @@ -128,7 +129,7 @@ TEST( planarSLAM, BearingRangeFactor_equals ) /* ************************************************************************* */ TEST( planarSLAM, PoseConstraint_equals ) { - planarSLAM::Constraint factor1(2, x2), factor2(2, x3); + planarSLAM::Constraint factor1(PoseKey(2), x2), factor2(PoseKey(2), x3); EXPECT(assert_equal(factor1, factor1, 1e-5)); EXPECT(assert_equal(factor2, factor2, 1e-5)); EXPECT(assert_inequal(factor1, factor2, 1e-5)); @@ -139,9 +140,9 @@ TEST( planarSLAM, constructor ) { // create config planarSLAM::Values c; - c.insert(2, x2); - c.insert(3, x3); - c.insert(3, l3); + c.insert(PoseKey(2), x2); + c.insert(PoseKey(3), x3); + c.insert(PointKey(3), l3); // create graph planarSLAM::Graph G; @@ -165,8 +166,8 @@ TEST( planarSLAM, constructor ) Vector expected2 = Vector_(1, -0.1); Vector expected3 = Vector_(1, 0.22); // Get NoiseModelFactors - FactorGraph > GNM = - *G.dynamicCastFactors > >(); + FactorGraph GNM = + *G.dynamicCastFactors >(); EXPECT(assert_equal(expected0, GNM[0]->unwhitenedError(c))); EXPECT(assert_equal(expected1, GNM[1]->unwhitenedError(c))); EXPECT(assert_equal(expected2, GNM[2]->unwhitenedError(c))); diff --git a/gtsam/slam/tests/testPose2SLAM.cpp b/gtsam/slam/tests/testPose2SLAM.cpp index 3cbc7e082..81ce20334 100644 --- a/gtsam/slam/tests/testPose2SLAM.cpp +++ b/gtsam/slam/tests/testPose2SLAM.cpp @@ -14,9 +14,6 @@ * @author Frank Dellaert, Viorela Ila **/ -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include #include #include @@ -34,6 +31,7 @@ using namespace boost::assign; using namespace std; typedef pose2SLAM::Odometry Pose2Factor; +using pose2SLAM::PoseKey; // common measurement covariance static double sx=0.5, sy=0.5,st=0.1; @@ -45,11 +43,13 @@ static noiseModel::Gaussian::shared_ptr covariance( ))); //static noiseModel::Gaussian::shared_ptr I3(noiseModel::Unit::Create(3)); +const Key kx0 = Symbol('x',0), kx1 = Symbol('x',1), kx2 = Symbol('x',2), kx3 = Symbol('x',3), kx4 = Symbol('x',4), kx5 = Symbol('x',5), kl1 = Symbol('l',1); + /* ************************************************************************* */ // Test constraint, small displacement Vector f1(const Pose2& pose1, const Pose2& pose2) { Pose2 z(2.1130913087, 0.468461064817, 0.436332312999); - Pose2Factor constraint(1, 2, z, covariance); + Pose2Factor constraint(PoseKey(1), PoseKey(2), z, covariance); return constraint.evaluateError(pose1, pose2); } @@ -57,7 +57,7 @@ TEST( Pose2SLAM, constraint1 ) { // create a factor between unknown poses p1 and p2 Pose2 pose1, pose2(2.1130913087, 0.468461064817, 0.436332312999); - Pose2Factor constraint(1, 2, pose2, covariance); + Pose2Factor constraint(PoseKey(1), PoseKey(2), pose2, covariance); Matrix H1, H2; Vector actual = constraint.evaluateError(pose1, pose2, H1, H2); @@ -72,7 +72,7 @@ TEST( Pose2SLAM, constraint1 ) // Test constraint, large displacement Vector f2(const Pose2& pose1, const Pose2& pose2) { Pose2 z(2,2,M_PI_2); - Pose2Factor constraint(1, 2, z, covariance); + Pose2Factor constraint(PoseKey(1), PoseKey(2), z, covariance); return constraint.evaluateError(pose1, pose2); } @@ -80,7 +80,7 @@ TEST( Pose2SLAM, constraint2 ) { // create a factor between unknown poses p1 and p2 Pose2 pose1, pose2(2,2,M_PI_2); - Pose2Factor constraint(1, 2, pose2, covariance); + Pose2Factor constraint(PoseKey(1), PoseKey(2), pose2, covariance); Matrix H1, H2; Vector actual = constraint.evaluateError(pose1, pose2, H1, H2); @@ -110,7 +110,7 @@ TEST( Pose2SLAM, linearization ) { // create a factor between unknown poses p1 and p2 Pose2 measured(2,2,M_PI_2); - Pose2Factor constraint(1,2,measured, covariance); + Pose2Factor constraint(PoseKey(1), PoseKey(2), measured, covariance); pose2SLAM::Graph graph; graph.addOdometry(1,2,measured, covariance); @@ -118,8 +118,8 @@ TEST( Pose2SLAM, linearization ) Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) Pose2 p2(-1,4.1,M_PI); // robot at (-1,4) looking at negative (ground truth is at 4.1,2) pose2SLAM::Values config; - config.insert(1,p1); - config.insert(2,p2); + config.insert(pose2SLAM::PoseKey(1),p1); + config.insert(pose2SLAM::PoseKey(2),p2); // Linearize Ordering ordering(*config.orderingArbitrary()); boost::shared_ptr > lfg_linearized = graph.linearize(config, ordering); @@ -139,7 +139,7 @@ TEST( Pose2SLAM, linearization ) Vector b = Vector_(3,-0.1/sx,0.1/sy,0.0); SharedDiagonal probModel1 = noiseModel::Unit::Create(3); - lfg_expected.push_back(JacobianFactor::shared_ptr(new JacobianFactor(ordering["x1"], A1, ordering["x2"], A2, b, probModel1))); + lfg_expected.push_back(JacobianFactor::shared_ptr(new JacobianFactor(ordering[kx1], A1, ordering[kx2], A2, b, probModel1))); CHECK(assert_equal(lfg_expected, *lfg_linearized)); } @@ -153,23 +153,23 @@ TEST(Pose2Graph, optimize) { fg->addOdometry(0, 1, Pose2(1,2,M_PI_2), covariance); // Create initial config - boost::shared_ptr initial(new pose2SLAM::Values()); - initial->insert(0, Pose2(0,0,0)); - initial->insert(1, Pose2(0,0,0)); + boost::shared_ptr initial(new Values()); + initial->insert(pose2SLAM::PoseKey(0), Pose2(0,0,0)); + initial->insert(pose2SLAM::PoseKey(1), Pose2(0,0,0)); // Choose an ordering and optimize shared_ptr ordering(new Ordering); - *ordering += "x0","x1"; - typedef NonlinearOptimizer Optimizer; + *ordering += kx0, kx1; + typedef NonlinearOptimizer Optimizer; - NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15); + NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15); Optimizer optimizer0(fg, initial, ordering, params); Optimizer optimizer = optimizer0.levenbergMarquardt(); // Check with expected config - pose2SLAM::Values expected; - expected.insert(0, Pose2(0,0,0)); - expected.insert(1, Pose2(1,2,M_PI_2)); + Values expected; + expected.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0)); + expected.insert(pose2SLAM::PoseKey(1), Pose2(1,2,M_PI_2)); CHECK(assert_equal(expected, *optimizer.values())); } @@ -179,7 +179,7 @@ TEST(Pose2Graph, optimizeThreePoses) { // Create a hexagon of poses pose2SLAM::Values hexagon = pose2SLAM::circle(3,1.0); - Pose2 p0 = hexagon[0], p1 = hexagon[1]; + Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1); // create a Pose graph with one equality constraint and one measurement shared_ptr fg(new pose2SLAM::Graph); @@ -191,23 +191,23 @@ TEST(Pose2Graph, optimizeThreePoses) { // Create initial config boost::shared_ptr initial(new pose2SLAM::Values()); - initial->insert(0, p0); - initial->insert(1, hexagon[1].retract(Vector_(3,-0.1, 0.1,-0.1))); - initial->insert(2, hexagon[2].retract(Vector_(3, 0.1,-0.1, 0.1))); + initial->insertPose(0, p0); + initial->insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1))); + initial->insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1))); // Choose an ordering shared_ptr ordering(new Ordering); - *ordering += "x0","x1","x2"; + *ordering += kx0,kx1,kx2; // optimize - NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15); + NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15); pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params); pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(); - pose2SLAM::Values actual = *optimizer.values(); + Values actual = *optimizer.values(); // Check with ground truth - CHECK(assert_equal(hexagon, actual)); + CHECK(assert_equal((const Values&)hexagon, actual)); } /* ************************************************************************* */ @@ -216,7 +216,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) { // Create a hexagon of poses pose2SLAM::Values hexagon = pose2SLAM::circle(6,1.0); - Pose2 p0 = hexagon[0], p1 = hexagon[1]; + Pose2 p0 = hexagon.pose(0), p1 = hexagon.pose(1); // create a Pose graph with one equality constraint and one measurement shared_ptr fg(new pose2SLAM::Graph); @@ -231,29 +231,29 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) { // Create initial config boost::shared_ptr initial(new pose2SLAM::Values()); - initial->insert(0, p0); - initial->insert(1, hexagon[1].retract(Vector_(3,-0.1, 0.1,-0.1))); - initial->insert(2, hexagon[2].retract(Vector_(3, 0.1,-0.1, 0.1))); - initial->insert(3, hexagon[3].retract(Vector_(3,-0.1, 0.1,-0.1))); - initial->insert(4, hexagon[4].retract(Vector_(3, 0.1,-0.1, 0.1))); - initial->insert(5, hexagon[5].retract(Vector_(3,-0.1, 0.1,-0.1))); + initial->insertPose(0, p0); + initial->insertPose(1, hexagon.pose(1).retract(Vector_(3,-0.1, 0.1,-0.1))); + initial->insertPose(2, hexagon.pose(2).retract(Vector_(3, 0.1,-0.1, 0.1))); + initial->insertPose(3, hexagon.pose(3).retract(Vector_(3,-0.1, 0.1,-0.1))); + initial->insertPose(4, hexagon.pose(4).retract(Vector_(3, 0.1,-0.1, 0.1))); + initial->insertPose(5, hexagon.pose(5).retract(Vector_(3,-0.1, 0.1,-0.1))); // Choose an ordering shared_ptr ordering(new Ordering); - *ordering += "x0","x1","x2","x3","x4","x5"; + *ordering += kx0,kx1,kx2,kx3,kx4,kx5; // optimize - NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15); + NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15); pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params); pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(); - pose2SLAM::Values actual = *optimizer.values(); + Values actual = *optimizer.values(); // Check with ground truth - CHECK(assert_equal(hexagon, actual)); + CHECK(assert_equal((const Values&)hexagon, actual)); // Check loop closure - CHECK(assert_equal(delta,actual[5].between(actual[0]))); + CHECK(assert_equal(delta, actual.at(PoseKey(5)).between(actual.at(PoseKey(0))))); // Pose2SLAMOptimizer myOptimizer("3"); @@ -281,7 +281,7 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) { // // myOptimizer.update(x); // -// pose2SLAM::Values expected; +// Values expected; // expected.insert(0, Pose2(0.,0.,0.)); // expected.insert(1, Pose2(1.,0.,0.)); // expected.insert(2, Pose2(2.,0.,0.)); @@ -315,8 +315,8 @@ TEST(Pose2Graph, optimize2) { // G.addConstraint(1, 3, Pose2(0.,0.,0.), I3); // G.addConstraint(2, 3, Pose2(0.,0.,0.), I3); // -// PredecessorMap tree = -// G.findMinimumSpanningTree(); +// PredecessorMap tree = +// G.findMinimumSpanningTree(); // CHECK(tree[1] == 1); // CHECK(tree[2] == 1); // CHECK(tree[3] == 1); @@ -329,12 +329,12 @@ TEST(Pose2Graph, optimize2) { // G.addConstraint(1, 3, Pose2(0.,0.,0.), I3); // G.addConstraint(2, 3, Pose2(0.,0.,0.), I3); // -// PredecessorMap tree; +// PredecessorMap tree; // tree.insert(1,2); // tree.insert(2,2); // tree.insert(3,2); // -// G.split(tree, T, C); +// G.split(tree, T, C); // LONGS_EQUAL(2, T.size()); // LONGS_EQUAL(1, C.size()); //} @@ -346,10 +346,10 @@ TEST(Pose2Values, pose2Circle ) { // expected is 4 poses tangent to circle with radius 1m pose2SLAM::Values expected; - expected.insert(0, Pose2( 1, 0, M_PI_2)); - expected.insert(1, Pose2( 0, 1, - M_PI )); - expected.insert(2, Pose2(-1, 0, - M_PI_2)); - expected.insert(3, Pose2( 0, -1, 0 )); + expected.insert(pose2SLAM::PoseKey(0), Pose2( 1, 0, M_PI_2)); + expected.insert(pose2SLAM::PoseKey(1), Pose2( 0, 1, - M_PI )); + expected.insert(pose2SLAM::PoseKey(2), Pose2(-1, 0, - M_PI_2)); + expected.insert(pose2SLAM::PoseKey(3), Pose2( 0, -1, 0 )); pose2SLAM::Values actual = pose2SLAM::circle(4,1.0); CHECK(assert_equal(expected,actual)); @@ -360,19 +360,19 @@ TEST(Pose2SLAM, expmap ) { // expected is circle shifted to right pose2SLAM::Values expected; - expected.insert(0, Pose2( 1.1, 0, M_PI_2)); - expected.insert(1, Pose2( 0.1, 1, - M_PI )); - expected.insert(2, Pose2(-0.9, 0, - M_PI_2)); - expected.insert(3, Pose2( 0.1, -1, 0 )); + expected.insert(pose2SLAM::PoseKey(0), Pose2( 1.1, 0, M_PI_2)); + expected.insert(pose2SLAM::PoseKey(1), Pose2( 0.1, 1, - M_PI )); + expected.insert(pose2SLAM::PoseKey(2), Pose2(-0.9, 0, - M_PI_2)); + expected.insert(pose2SLAM::PoseKey(3), Pose2( 0.1, -1, 0 )); // Note expmap coordinates are in local coordinates, so shifting to right requires thought !!! - pose2SLAM::Values circle(pose2SLAM::circle(4,1.0)); + pose2SLAM::Values circle(pose2SLAM::circle(4,1.0)); Ordering ordering(*circle.orderingArbitrary()); VectorValues delta(circle.dims(ordering)); - delta[ordering[PoseKey(0)]] = Vector_(3, 0.0,-0.1,0.0); - delta[ordering[PoseKey(1)]] = Vector_(3, -0.1,0.0,0.0); - delta[ordering[PoseKey(2)]] = Vector_(3, 0.0,0.1,0.0); - delta[ordering[PoseKey(3)]] = Vector_(3, 0.1,0.0,0.0); + delta[ordering[pose2SLAM::PoseKey(0)]] = Vector_(3, 0.0,-0.1,0.0); + delta[ordering[pose2SLAM::PoseKey(1)]] = Vector_(3, -0.1,0.0,0.0); + delta[ordering[pose2SLAM::PoseKey(2)]] = Vector_(3, 0.0,0.1,0.0); + delta[ordering[pose2SLAM::PoseKey(3)]] = Vector_(3, 0.1,0.0,0.0); pose2SLAM::Values actual = circle.retract(delta, ordering); CHECK(assert_equal(expected,actual)); } @@ -387,10 +387,10 @@ TEST( Pose2Prior, error ) // Choose a linearization point Pose2 p1(1, 0, 0); // robot at (1,0) pose2SLAM::Values x0; - x0.insert(1, p1); + x0.insert(PoseKey(1), p1); // Create factor - pose2SLAM::Prior factor(1, p1, sigmas); + pose2SLAM::Prior factor(PoseKey(1), p1, sigmas); // Actual linearization Ordering ordering(*x0.orderingArbitrary()); @@ -399,14 +399,14 @@ TEST( Pose2Prior, error ) // Check error at x0, i.e. delta = zero ! VectorValues delta(VectorValues::Zero(x0.dims(ordering))); - delta[ordering["x1"]] = zero(3); + delta[ordering[kx1]] = zero(3); Vector error_at_zero = Vector_(3,0.0,0.0,0.0); CHECK(assert_equal(error_at_zero,factor.whitenedError(x0))); CHECK(assert_equal(-error_at_zero,linear->error_vector(delta))); // Check error after increasing p2 VectorValues addition(VectorValues::Zero(x0.dims(ordering))); - addition[ordering["x1"]] = Vector_(3, 0.1, 0.0, 0.0); + addition[ordering[kx1]] = Vector_(3, 0.1, 0.0, 0.0); VectorValues plus = delta + addition; pose2SLAM::Values x1 = x0.retract(plus, ordering); Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! @@ -417,7 +417,7 @@ TEST( Pose2Prior, error ) /* ************************************************************************* */ // common Pose2Prior for tests below static gtsam::Pose2 priorVal(2,2,M_PI_2); -static pose2SLAM::Prior priorFactor(1,priorVal, sigmas); +static pose2SLAM::Prior priorFactor(PoseKey(1), priorVal, sigmas); /* ************************************************************************* */ // The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector @@ -431,7 +431,7 @@ TEST( Pose2Prior, linearize ) { // Choose a linearization point at ground truth pose2SLAM::Values x0; - x0.insert(1,priorVal); + x0.insertPose(1, priorVal); // Actual linearization Ordering ordering(*x0.orderingArbitrary()); @@ -440,7 +440,7 @@ TEST( Pose2Prior, linearize ) // Test with numerical derivative Matrix numericalH = numericalDerivative11(hprior, priorVal); - CHECK(assert_equal(numericalH,actual->getA(actual->find(ordering["x1"])))); + CHECK(assert_equal(numericalH,actual->getA(actual->find(ordering[kx1])))); } /* ************************************************************************* */ @@ -451,12 +451,12 @@ TEST( Pose2Factor, error ) Pose2 p1; // robot at origin Pose2 p2(1, 0, 0); // robot at (1,0) pose2SLAM::Values x0; - x0.insert(1, p1); - x0.insert(2, p2); + x0.insertPose(1, p1); + x0.insertPose(2, p2); // Create factor Pose2 z = p1.between(p2); - Pose2Factor factor(1, 2, z, covariance); + Pose2Factor factor(PoseKey(1), PoseKey(2), z, covariance); // Actual linearization Ordering ordering(*x0.orderingArbitrary()); @@ -465,15 +465,15 @@ TEST( Pose2Factor, error ) // Check error at x0, i.e. delta = zero ! VectorValues delta(x0.dims(ordering)); - delta[ordering["x1"]] = zero(3); - delta[ordering["x2"]] = zero(3); + delta[ordering[kx1]] = zero(3); + delta[ordering[kx2]] = zero(3); Vector error_at_zero = Vector_(3,0.0,0.0,0.0); CHECK(assert_equal(error_at_zero,factor.unwhitenedError(x0))); CHECK(assert_equal(-error_at_zero, linear->error_vector(delta))); // Check error after increasing p2 VectorValues plus = delta; - plus[ordering["x2"]] = Vector_(3, 0.1, 0.0, 0.0); + plus[ordering[kx2]] = Vector_(3, 0.1, 0.0, 0.0); pose2SLAM::Values x1 = x0.retract(plus, ordering); Vector error_at_plus = Vector_(3,0.1/sx,0.0,0.0); // h(x)-z = 0.1 ! CHECK(assert_equal(error_at_plus,factor.whitenedError(x1))); @@ -483,7 +483,7 @@ TEST( Pose2Factor, error ) /* ************************************************************************* */ // common Pose2Factor for tests below static Pose2 measured(2,2,M_PI_2); -static Pose2Factor factor(1,2,measured, covariance); +static Pose2Factor factor(PoseKey(1),PoseKey(2),measured, covariance); /* ************************************************************************* */ TEST( Pose2Factor, rhs ) @@ -492,8 +492,8 @@ TEST( Pose2Factor, rhs ) Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at -1,4) pose2SLAM::Values x0; - x0.insert(1,p1); - x0.insert(2,p2); + x0.insert(pose2SLAM::PoseKey(1),p1); + x0.insert(pose2SLAM::PoseKey(2),p2); // Actual linearization Ordering ordering(*x0.orderingArbitrary()); @@ -522,8 +522,8 @@ TEST( Pose2Factor, linearize ) Pose2 p1(1,2,M_PI_2); Pose2 p2(-1,4,M_PI); pose2SLAM::Values x0; - x0.insert(1,p1); - x0.insert(2,p2); + x0.insert(pose2SLAM::PoseKey(1),p1); + x0.insert(pose2SLAM::PoseKey(2),p2); // expected linearization Matrix expectedH1 = covariance->Whiten(Matrix_(3,3, @@ -541,7 +541,7 @@ TEST( Pose2Factor, linearize ) // expected linear factor Ordering ordering(*x0.orderingArbitrary()); SharedDiagonal probModel1 = noiseModel::Unit::Create(3); - JacobianFactor expected(ordering["x1"], expectedH1, ordering["x2"], expectedH2, expected_b, probModel1); + JacobianFactor expected(ordering[kx1], expectedH1, ordering[kx2], expectedH2, expected_b, probModel1); // Actual linearization boost::shared_ptr actual = diff --git a/gtsam/slam/tests/testPose3SLAM.cpp b/gtsam/slam/tests/testPose3SLAM.cpp index b2bd86d4d..2edf6c4f0 100644 --- a/gtsam/slam/tests/testPose3SLAM.cpp +++ b/gtsam/slam/tests/testPose3SLAM.cpp @@ -28,28 +28,28 @@ using namespace boost::assign; // TODO: DANGEROUS, create shared pointers #define GTSAM_MAGIC_GAUSSIAN 6 -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include #include using namespace std; using namespace gtsam; +using namespace pose3SLAM; // common measurement covariance static Matrix covariance = eye(6); const double tol=1e-5; +const Key kx0 = Symbol('x',0), kx1 = Symbol('x',1), kx2 = Symbol('x',2), kx3 = Symbol('x',3), kx4 = Symbol('x',4), kx5 = Symbol('x',5); + /* ************************************************************************* */ // test optimization with 6 poses arranged in a hexagon and a loop closure TEST(Pose3Graph, optimizeCircle) { // Create a hexagon of poses double radius = 10; - Pose3Values hexagon = pose3SLAM::circle(6,radius); - Pose3 gT0 = hexagon[0], gT1 = hexagon[1]; + Values hexagon = pose3SLAM::circle(6,radius); + Pose3 gT0 = hexagon.at(PoseKey(0)), gT1 = hexagon.at(PoseKey(1)); // create a Pose graph with one equality constraint and one measurement shared_ptr fg(new Pose3Graph); @@ -65,38 +65,38 @@ TEST(Pose3Graph, optimizeCircle) { fg->addConstraint(5,0, _0T1, covariance); // Create initial config - boost::shared_ptr initial(new Pose3Values()); - initial->insert(0, gT0); - initial->insert(1, hexagon[1].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - initial->insert(2, hexagon[2].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); - initial->insert(3, hexagon[3].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); - initial->insert(4, hexagon[4].retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); - initial->insert(5, hexagon[5].retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + boost::shared_ptr initial(new Values()); + initial->insert(PoseKey(0), gT0); + initial->insert(PoseKey(1), hexagon.at(PoseKey(1)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + initial->insert(PoseKey(2), hexagon.at(PoseKey(2)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); + initial->insert(PoseKey(3), hexagon.at(PoseKey(3)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); + initial->insert(PoseKey(4), hexagon.at(PoseKey(4)).retract(Vector_(6, 0.1,-0.1, 0.1, 0.1,-0.1, 0.1))); + initial->insert(PoseKey(5), hexagon.at(PoseKey(5)).retract(Vector_(6,-0.1, 0.1,-0.1,-0.1, 0.1,-0.1))); // Choose an ordering and optimize shared_ptr ordering(new Ordering); - *ordering += "x0","x1","x2","x3","x4","x5"; - NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15); + *ordering += kx0,kx1,kx2,kx3,kx4,kx5; + NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15); pose3SLAM::Optimizer optimizer0(fg, initial, ordering, params); pose3SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt(); - Pose3Values actual = *optimizer.values(); + Values actual = *optimizer.values(); // Check with ground truth CHECK(assert_equal(hexagon, actual,1e-4)); // Check loop closure - CHECK(assert_equal(_0T1,actual[5].between(actual[0]),1e-5)); + CHECK(assert_equal(_0T1, actual.at(PoseKey(5)).between(actual.at(PoseKey(0))),1e-5)); } /* ************************************************************************* */ TEST(Pose3Graph, partial_prior_height) { - typedef PartialPriorFactor Partial; + typedef PartialPriorFactor Partial; // reference: Pose3 Expmap - (0-2: Rot3) (3-5: Point3) // height prior - single element interface - pose3SLAM::Key key(1); + Symbol key = PoseKey(1); double exp_height = 5.0; SharedDiagonal model = noiseModel::Unit::Create(1); Pose3 init(Rot3(), Point3(1.0, 2.0, 3.0)), expected(Rot3(), Point3(1.0, 2.0, exp_height)); @@ -109,14 +109,14 @@ TEST(Pose3Graph, partial_prior_height) { pose3SLAM::Graph graph; graph.add(height); - pose3SLAM::Values values; + Values values; values.insert(key, init); // linearization EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol); - pose3SLAM::Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values); - EXPECT(assert_equal(expected, actual[key], tol)); + Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values); + EXPECT(assert_equal(expected, actual.at(key), tol)); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); } @@ -130,12 +130,12 @@ TEST( Pose3Factor, error ) // Create factor SharedNoiseModel I6(noiseModel::Unit::Create(6)); - Pose3Factor factor(1,2, z, I6); + Pose3Factor factor(PoseKey(1), PoseKey(2), z, I6); // Create config - Pose3Values x; - x.insert(1,t1); - x.insert(2,t2); + Values x; + x.insert(PoseKey(1),t1); + x.insert(PoseKey(2),t2); // Get error h(x)-z -> localCoordinates(z,h(x)) = localCoordinates(z,between(t1,t2)) Vector actual = factor.unwhitenedError(x); @@ -145,10 +145,10 @@ TEST( Pose3Factor, error ) /* ************************************************************************* */ TEST(Pose3Graph, partial_prior_xy) { - typedef PartialPriorFactor Partial; + typedef PartialPriorFactor Partial; // XY prior - full mask interface - pose3SLAM::Key key(1); + Symbol key = PoseKey(1); Vector exp_xy = Vector_(2, 3.0, 4.0); SharedDiagonal model = noiseModel::Unit::Create(2); Pose3 init(Rot3(), Point3(1.0,-2.0, 3.0)), expected(Rot3(), Point3(3.0, 4.0, 3.0)); @@ -164,11 +164,11 @@ TEST(Pose3Graph, partial_prior_xy) { pose3SLAM::Graph graph; graph.add(priorXY); - pose3SLAM::Values values; + Values values; values.insert(key, init); - pose3SLAM::Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values); - EXPECT(assert_equal(expected, actual[key], tol)); + Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values); + EXPECT(assert_equal(expected, actual.at(key), tol)); EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol); } @@ -180,27 +180,27 @@ Rot3 R3(Point3( 0,-1, 0), Point3(-1, 0, 0), Point3(0, 0, -1)); Rot3 R4(Point3( 1, 0, 0), Point3( 0,-1, 0), Point3(0, 0, -1)); /* ************************************************************************* */ -TEST( Pose3Values, pose3Circle ) +TEST( Values, pose3Circle ) { // expected is 4 poses tangent to circle with radius 1m - Pose3Values expected; - expected.insert(0, Pose3(R1, Point3( 1, 0, 0))); - expected.insert(1, Pose3(R2, Point3( 0, 1, 0))); - expected.insert(2, Pose3(R3, Point3(-1, 0, 0))); - expected.insert(3, Pose3(R4, Point3( 0,-1, 0))); + Values expected; + expected.insert(PoseKey(0), Pose3(R1, Point3( 1, 0, 0))); + expected.insert(PoseKey(1), Pose3(R2, Point3( 0, 1, 0))); + expected.insert(PoseKey(2), Pose3(R3, Point3(-1, 0, 0))); + expected.insert(PoseKey(3), Pose3(R4, Point3( 0,-1, 0))); - Pose3Values actual = pose3SLAM::circle(4,1.0); + Values actual = pose3SLAM::circle(4,1.0); CHECK(assert_equal(expected,actual)); } /* ************************************************************************* */ -TEST( Pose3Values, expmap ) +TEST( Values, expmap ) { - Pose3Values expected; - expected.insert(0, Pose3(R1, Point3( 1.0, 0.1, 0))); - expected.insert(1, Pose3(R2, Point3(-0.1, 1.0, 0))); - expected.insert(2, Pose3(R3, Point3(-1.0,-0.1, 0))); - expected.insert(3, Pose3(R4, Point3( 0.1,-1.0, 0))); + Values expected; + expected.insert(PoseKey(0), Pose3(R1, Point3( 1.0, 0.1, 0))); + expected.insert(PoseKey(1), Pose3(R2, Point3(-0.1, 1.0, 0))); + expected.insert(PoseKey(2), Pose3(R3, Point3(-1.0,-0.1, 0))); + expected.insert(PoseKey(3), Pose3(R4, Point3( 0.1,-1.0, 0))); Ordering ordering(*expected.orderingArbitrary()); VectorValues delta(expected.dims(ordering)); @@ -209,7 +209,7 @@ TEST( Pose3Values, expmap ) 0.0,0.0,0.0, 0.1, 0.0, 0.0, 0.0,0.0,0.0, 0.1, 0.0, 0.0, 0.0,0.0,0.0, 0.1, 0.0, 0.0); - Pose3Values actual = pose3SLAM::circle(4,1.0).retract(delta, ordering); + Values actual = pose3SLAM::circle(4,1.0).retract(delta, ordering); CHECK(assert_equal(expected,actual)); } diff --git a/gtsam/slam/tests/testProjectionFactor.cpp b/gtsam/slam/tests/testProjectionFactor.cpp index 09fd612d7..8b4ecbf21 100644 --- a/gtsam/slam/tests/testProjectionFactor.cpp +++ b/gtsam/slam/tests/testProjectionFactor.cpp @@ -18,13 +18,11 @@ #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include using namespace std; using namespace gtsam; +using namespace visualSLAM; // make cube static Point3 @@ -39,6 +37,8 @@ static Cal3_S2 K(fov,w,h); static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); static shared_ptrK sK(new Cal3_S2(K)); +const Key kx1 = Symbol('x',1), kl1 = Symbol('l',1); + // make cameras /* ************************************************************************* */ @@ -48,12 +48,12 @@ TEST( ProjectionFactor, error ) Point2 z(323.,240.); int cameraFrameNumber=1, landmarkNumber=1; boost::shared_ptr - factor(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); + factor(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK)); // For the following values structure, the factor predicts 320,240 - visualSLAM::Values config; - Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(1, x1); - Point3 l1; config.insert(1, l1); + Values config; + Rot3 R;Point3 t1(0,0,-6); Pose3 x1(R,t1); config.insert(PoseKey(1), x1); + Point3 l1; config.insert(PointKey(1), l1); // Point should project to Point2(320.,240.) CHECK(assert_equal(Vector_(2, -3.0, 0.0), factor->unwhitenedError(config))); @@ -61,12 +61,12 @@ TEST( ProjectionFactor, error ) DOUBLES_EQUAL(4.5,factor->error(config),1e-9); // Check linearize - Ordering ordering; ordering += "x1","l1"; + Ordering ordering; ordering += kx1,kl1; Matrix Ax1 = Matrix_(2, 6, 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.); Matrix Al1 = Matrix_(2, 3, 92.376, 0., 0., 0., 92.376, 0.); Vector b = Vector_(2,3.,0.); SharedDiagonal probModel1 = noiseModel::Unit::Create(2); - JacobianFactor expected(ordering["x1"], Ax1, ordering["l1"], Al1, b, probModel1); + JacobianFactor expected(ordering[kx1], Ax1, ordering[kl1], Al1, b, probModel1); JacobianFactor::shared_ptr actual = boost::dynamic_pointer_cast(factor->linearize(config, ordering)); CHECK(assert_equal(expected,*actual,1e-3)); @@ -80,13 +80,13 @@ TEST( ProjectionFactor, error ) CHECK(assert_equal(expected_lfg,*actual_lfg)); // expmap on a config - visualSLAM::Values expected_config; - Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(1, x2); - Point3 l2(1,2,3); expected_config.insert(1, l2); + Values expected_config; + Point3 t2(1,1,-5); Pose3 x2(R,t2); expected_config.insert(PoseKey(1), x2); + Point3 l2(1,2,3); expected_config.insert(PointKey(1), l2); VectorValues delta(expected_config.dims(ordering)); - delta[ordering["x1"]] = Vector_(6, 0.,0.,0., 1.,1.,1.); - delta[ordering["l1"]] = Vector_(3, 1.,2.,3.); - visualSLAM::Values actual_config = config.retract(delta, ordering); + delta[ordering[kx1]] = Vector_(6, 0.,0.,0., 1.,1.,1.); + delta[ordering[kl1]] = Vector_(3, 1.,2.,3.); + Values actual_config = config.retract(delta, ordering); CHECK(assert_equal(expected_config,actual_config,1e-9)); } @@ -97,10 +97,10 @@ TEST( ProjectionFactor, equals ) Vector z = Vector_(2,323.,240.); int cameraFrameNumber=1, landmarkNumber=1; boost::shared_ptr - factor1(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); + factor1(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK)); boost::shared_ptr - factor2(new visualSLAM::ProjectionFactor(z, sigma, cameraFrameNumber, landmarkNumber, sK)); + factor2(new visualSLAM::ProjectionFactor(z, sigma, PoseKey(cameraFrameNumber), PointKey(landmarkNumber), sK)); CHECK(assert_equal(*factor1, *factor2)); } diff --git a/gtsam/slam/tests/testSerializationSLAM.cpp b/gtsam/slam/tests/testSerializationSLAM.cpp new file mode 100644 index 000000000..9cfb8bb72 --- /dev/null +++ b/gtsam/slam/tests/testSerializationSLAM.cpp @@ -0,0 +1,207 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testSerializationSLAM.cpp + * @brief + * @author Richard Roberts + * @date Feb 7, 2012 + */ + +#include +#include +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; +using namespace gtsam::serializationTestHelpers; + +/* Create GUIDs for factors */ +/* ************************************************************************* */ +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); + +/* ************************************************************************* */ +// Export Noisemodels +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); + +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +/* ************************************************************************* */ +TEST (Serialization, smallExample_linear) { + using namespace example; + + Ordering ordering; ordering += Symbol('x',1),Symbol('x',2),Symbol('l',1); + GaussianFactorGraph fg = createGaussianFactorGraph(ordering); + EXPECT(equalsObj(ordering)); + EXPECT(equalsXML(ordering)); + + EXPECT(equalsObj(fg)); + EXPECT(equalsXML(fg)); + + GaussianBayesNet cbn = createSmallGaussianBayesNet(); + EXPECT(equalsObj(cbn)); + EXPECT(equalsXML(cbn)); +} + +/* ************************************************************************* */ +TEST (Serialization, gaussianISAM) { + using namespace example; + Ordering ordering; + GaussianFactorGraph smoother; + boost::tie(smoother, ordering) = createSmoother(7); + BayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); + GaussianISAM isam(bayesTree); + + EXPECT(equalsObj(isam)); + EXPECT(equalsXML(isam)); +} + +/* ************************************************************************* */ +/* Create GUIDs for factors in simulated2D */ +BOOST_CLASS_EXPORT_GUID(simulated2D::Prior, "gtsam::simulated2D::Prior" ) +BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry, "gtsam::simulated2D::Odometry" ) +BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement") +BOOST_CLASS_EXPORT(gtsam::Point2) + +/* ************************************************************************* */ +TEST (Serialization, smallExample) { + using namespace example; + Graph nfg = createNonlinearFactorGraph(); + Values c1 = createValues(); + EXPECT(equalsObj(nfg)); + EXPECT(equalsXML(nfg)); + + EXPECT(equalsObj(c1)); + EXPECT(equalsXML(c1)); +} + +/* ************************************************************************* */ +/* Create GUIDs for factors */ +BOOST_CLASS_EXPORT_GUID(planarSLAM::Prior, "gtsam::planarSLAM::Prior"); +BOOST_CLASS_EXPORT_GUID(planarSLAM::Bearing, "gtsam::planarSLAM::Bearing"); +BOOST_CLASS_EXPORT_GUID(planarSLAM::Range, "gtsam::planarSLAM::Range"); +BOOST_CLASS_EXPORT_GUID(planarSLAM::BearingRange,"gtsam::planarSLAM::BearingRange"); +BOOST_CLASS_EXPORT_GUID(planarSLAM::Odometry, "gtsam::planarSLAM::Odometry"); +BOOST_CLASS_EXPORT_GUID(planarSLAM::Constraint, "gtsam::planarSLAM::Constraint"); + +BOOST_CLASS_EXPORT(gtsam::Pose2) + +/* ************************************************************************* */ +TEST (Serialization, planar_system) { + using namespace planarSLAM; + planarSLAM::Values values; + values.insert(PointKey(3), Point2(1.0, 2.0)); + values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3)); + + SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(1, 0.3); + SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3); + SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3); + + Prior prior(PoseKey(3), Pose2(0.1,-0.3, 0.2), model1); + Bearing bearing(PoseKey(3), PointKey(5), Rot2::fromDegrees(0.5), model1); + Range range(PoseKey(2), PointKey(9), 7.0, model1); + BearingRange bearingRange(PoseKey(2), PointKey(3), Rot2::fromDegrees(0.6), 2.0, model2); + Odometry odometry(PoseKey(2), PoseKey(3), Pose2(1.0, 2.0, 0.3), model3); + Constraint constraint(PoseKey(9), Pose2(2.0,-1.0, 0.2)); + + Graph graph; + graph.add(prior); + graph.add(bearing); + graph.add(range); + graph.add(bearingRange); + graph.add(odometry); + graph.add(constraint); + + // text + EXPECT(equalsObj(PoseKey(2))); + EXPECT(equalsObj(PointKey(3))); + EXPECT(equalsObj(values)); + EXPECT(equalsObj(prior)); + EXPECT(equalsObj(bearing)); + EXPECT(equalsObj(bearingRange)); + EXPECT(equalsObj(range)); + EXPECT(equalsObj(odometry)); + EXPECT(equalsObj(constraint)); + EXPECT(equalsObj(graph)); + + // xml + EXPECT(equalsXML(PoseKey(2))); + EXPECT(equalsXML(PointKey(3))); + EXPECT(equalsXML(values)); + EXPECT(equalsXML(prior)); + EXPECT(equalsXML(bearing)); + EXPECT(equalsXML(bearingRange)); + EXPECT(equalsXML(range)); + EXPECT(equalsXML(odometry)); + EXPECT(equalsXML(constraint)); + EXPECT(equalsXML(graph)); +} + +/* ************************************************************************* */ +/* Create GUIDs for factors */ +BOOST_CLASS_EXPORT_GUID(visualSLAM::PoseConstraint, "gtsam::visualSLAM::PoseConstraint"); +BOOST_CLASS_EXPORT_GUID(visualSLAM::PointConstraint, "gtsam::visualSLAM::PointConstraint"); +BOOST_CLASS_EXPORT_GUID(visualSLAM::PosePrior, "gtsam::visualSLAM::PosePrior"); +BOOST_CLASS_EXPORT_GUID(visualSLAM::PointPrior, "gtsam::visualSLAM::PointPrior"); +BOOST_CLASS_EXPORT_GUID(visualSLAM::ProjectionFactor,"gtsam::visualSLAM::ProjectionFactor"); +BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor, "gtsam::visualSLAM::StereoFactor"); + +BOOST_CLASS_EXPORT(gtsam::Pose3) +BOOST_CLASS_EXPORT(gtsam::Point3) + +Point3 pt3(1.0, 2.0, 3.0); +Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); +Pose3 pose3(rt3, pt3); +Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); + +/* ************************************************************************* */ +TEST (Serialization, visual_system) { + using namespace visualSLAM; + Values values; + Symbol x1('x',1), x2('x',2); + Symbol l1('l',1), l2('l',2); + Pose3 pose1 = pose3, pose2 = pose3.inverse(); + Point3 pt1(1.0, 2.0, 3.0), pt2(4.0, 5.0, 6.0); + values.insert(x1, pose1); + values.insert(l1, pt1); + SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3); + SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3); + SharedNoiseModel model6 = noiseModel::Isotropic::Sigma(6, 0.3); + boost::shared_ptr K(new Cal3_S2(cal1)); + + Graph graph; + graph.addMeasurement(Point2(1.0, 2.0), model2, 1, 1, K); + graph.addPointConstraint(1, pt1); + graph.addPointPrior(1, pt2, model3); + graph.addPoseConstraint(1, pose1); + graph.addPosePrior(1, pose2, model6); + + EXPECT(equalsObj(values)); + EXPECT(equalsObj(graph)); + + EXPECT(equalsXML(values)); + EXPECT(equalsXML(graph)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ diff --git a/gtsam/slam/tests/testSimulated2D.cpp b/gtsam/slam/tests/testSimulated2D.cpp index 2d28f11c3..3513927a7 100644 --- a/gtsam/slam/tests/testSimulated2D.cpp +++ b/gtsam/slam/tests/testSimulated2D.cpp @@ -31,8 +31,8 @@ using namespace simulated2D; TEST( simulated2D, Simulated2DValues ) { simulated2D::Values actual; - actual.insertPose(1,Point2(1,1)); - actual.insertPoint(2,Point2(2,2)); + actual.insert(simulated2D::PoseKey(1),Point2(1,1)); + actual.insert(simulated2D::PointKey(2),Point2(2,2)); EXPECT(assert_equal(actual,actual,1e-9)); } diff --git a/gtsam/slam/tests/testSimulated3D.cpp b/gtsam/slam/tests/testSimulated3D.cpp index b3086acf1..a48e797e5 100644 --- a/gtsam/slam/tests/testSimulated3D.cpp +++ b/gtsam/slam/tests/testSimulated3D.cpp @@ -29,7 +29,7 @@ using namespace simulated3D; /* ************************************************************************* */ TEST( simulated3D, Values ) { - simulated3D::Values actual; + Values actual; actual.insert(simulated3D::PointKey(1),Point3(1,1,1)); actual.insert(simulated3D::PoseKey(2),Point3(2,2,2)); EXPECT(assert_equal(actual,actual,1e-9)); diff --git a/gtsam/slam/tests/testStereoFactor.cpp b/gtsam/slam/tests/testStereoFactor.cpp index 2955e1ef9..fc026fb35 100644 --- a/gtsam/slam/tests/testStereoFactor.cpp +++ b/gtsam/slam/tests/testStereoFactor.cpp @@ -29,6 +29,7 @@ using namespace std; using namespace gtsam; using namespace boost; +using namespace visualSLAM; Pose3 camera1(Matrix_(3,3, 1., 0., 0., @@ -51,22 +52,22 @@ TEST( StereoFactor, singlePoint) boost::shared_ptr K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5)); boost::shared_ptr graph(new visualSLAM::Graph()); - graph->add(visualSLAM::PoseConstraint(1,camera1)); + graph->add(visualSLAM::PoseConstraint(PoseKey(1),camera1)); StereoPoint2 z14(320,320.0-50, 240); // arguments: measurement, sigma, cam#, measurement #, K, baseline (m) - graph->add(visualSLAM::StereoFactor(z14,sigma, 1, 1, K)); + graph->add(visualSLAM::StereoFactor(z14,sigma, PoseKey(1), PointKey(1), K)); // Create a configuration corresponding to the ground truth - boost::shared_ptr values(new visualSLAM::Values()); - values->insert(1, camera1); // add camera at z=6.25m looking towards origin + boost::shared_ptr values(new Values()); + values->insert(PoseKey(1), camera1); // add camera at z=6.25m looking towards origin Point3 l1(0, 0, 0); - values->insert(1, l1); // add point at origin; + values->insert(PointKey(1), l1); // add point at origin; Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values); - typedef gtsam::NonlinearOptimizer Optimizer; // optimization engine for this domain + typedef gtsam::NonlinearOptimizer Optimizer; // optimization engine for this domain double absoluteThreshold = 1e-9; double relativeThreshold = 1e-5; diff --git a/gtsam/slam/tests/testVSLAM.cpp b/gtsam/slam/tests/testVSLAM.cpp index cb208bfeb..0188daa96 100644 --- a/gtsam/slam/tests/testVSLAM.cpp +++ b/gtsam/slam/tests/testVSLAM.cpp @@ -22,9 +22,6 @@ #include using namespace boost; -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include #include #include @@ -32,6 +29,7 @@ using namespace boost; using namespace std; using namespace gtsam; using namespace boost; +using namespace visualSLAM; static SharedNoiseModel sigma(noiseModel::Unit::Create(1)); @@ -90,17 +88,17 @@ TEST( Graph, optimizeLM) graph->addPointConstraint(3, landmark3); // Create an initial values structure corresponding to the ground truth - boost::shared_ptr initialEstimate(new visualSLAM::Values); - initialEstimate->insert(1, camera1); - initialEstimate->insert(2, camera2); - initialEstimate->insert(1, landmark1); - initialEstimate->insert(2, landmark2); - initialEstimate->insert(3, landmark3); - initialEstimate->insert(4, landmark4); + boost::shared_ptr initialEstimate(new Values); + initialEstimate->insert(PoseKey(1), camera1); + initialEstimate->insert(PoseKey(2), camera2); + initialEstimate->insert(PointKey(1), landmark1); + initialEstimate->insert(PointKey(2), landmark2); + initialEstimate->insert(PointKey(3), landmark3); + initialEstimate->insert(PointKey(4), landmark4); // Create an ordering of the variables shared_ptr ordering(new Ordering); - *ordering += "l1","l2","l3","l4","x1","x2"; + *ordering += PointKey(1),PointKey(2),PointKey(3),PointKey(4),PoseKey(1),PoseKey(2); // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth @@ -127,17 +125,17 @@ TEST( Graph, optimizeLM2) graph->addPoseConstraint(2, camera2); // Create an initial values structure corresponding to the ground truth - boost::shared_ptr initialEstimate(new visualSLAM::Values); - initialEstimate->insert(1, camera1); - initialEstimate->insert(2, camera2); - initialEstimate->insert(1, landmark1); - initialEstimate->insert(2, landmark2); - initialEstimate->insert(3, landmark3); - initialEstimate->insert(4, landmark4); + boost::shared_ptr initialEstimate(new Values); + initialEstimate->insert(PoseKey(1), camera1); + initialEstimate->insert(PoseKey(2), camera2); + initialEstimate->insert(PointKey(1), landmark1); + initialEstimate->insert(PointKey(2), landmark2); + initialEstimate->insert(PointKey(3), landmark3); + initialEstimate->insert(PointKey(4), landmark4); // Create an ordering of the variables shared_ptr ordering(new Ordering); - *ordering += "l1","l2","l3","l4","x1","x2"; + *ordering += PointKey(1),PointKey(2),PointKey(3),PointKey(4),PoseKey(1),PoseKey(2); // Create an optimizer and check its error // We expect the initial to be zero because config is the ground truth @@ -164,13 +162,13 @@ TEST( Graph, CHECK_ORDERING) graph->addPoseConstraint(2, camera2); // Create an initial values structure corresponding to the ground truth - boost::shared_ptr initialEstimate(new visualSLAM::Values); - initialEstimate->insert(1, camera1); - initialEstimate->insert(2, camera2); - initialEstimate->insert(1, landmark1); - initialEstimate->insert(2, landmark2); - initialEstimate->insert(3, landmark3); - initialEstimate->insert(4, landmark4); + boost::shared_ptr initialEstimate(new Values); + initialEstimate->insert(PoseKey(1), camera1); + initialEstimate->insert(PoseKey(2), camera2); + initialEstimate->insert(PointKey(1), landmark1); + initialEstimate->insert(PointKey(2), landmark2); + initialEstimate->insert(PointKey(3), landmark3); + initialEstimate->insert(PointKey(4), landmark4); Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initialEstimate); @@ -192,23 +190,23 @@ TEST( Graph, CHECK_ORDERING) TEST( Values, update_with_large_delta) { // this test ensures that if the update for delta is larger than // the size of the config, it only updates existing variables - visualSLAM::Values init; - init.insert(1, Pose3()); - init.insert(1, Point3(1.0, 2.0, 3.0)); + Values init; + init.insert(PoseKey(1), Pose3()); + init.insert(PointKey(1), Point3(1.0, 2.0, 3.0)); - visualSLAM::Values expected; - expected.insert(1, Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); - expected.insert(1, Point3(1.1, 2.1, 3.1)); + Values expected; + expected.insert(PoseKey(1), Pose3(Rot3(), Point3(0.1, 0.1, 0.1))); + expected.insert(PointKey(1), Point3(1.1, 2.1, 3.1)); Ordering largeOrdering; - visualSLAM::Values largeValues = init; - largeValues.insert(2, Pose3()); - largeOrdering += "x1","l1","x2"; + Values largeValues = init; + largeValues.insert(PoseKey(2), Pose3()); + largeOrdering += PoseKey(1),PointKey(1),PoseKey(2); VectorValues delta(largeValues.dims(largeOrdering)); - delta[largeOrdering["x1"]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1); - delta[largeOrdering["l1"]] = Vector_(3, 0.1, 0.1, 0.1); - delta[largeOrdering["x2"]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1); - visualSLAM::Values actual = init.retract(delta, largeOrdering); + delta[largeOrdering[PoseKey(1)]] = Vector_(6, 0.0, 0.0, 0.0, 0.1, 0.1, 0.1); + delta[largeOrdering[PointKey(1)]] = Vector_(3, 0.1, 0.1, 0.1); + delta[largeOrdering[PoseKey(2)]] = Vector_(6, 0.0, 0.0, 0.0, 100.1, 4.1, 9.1); + Values actual = init.retract(delta, largeOrdering); CHECK(assert_equal(expected,actual)); } diff --git a/gtsam/slam/visualSLAM.h b/gtsam/slam/visualSLAM.h index 931bcfaf8..5416e2142 100644 --- a/gtsam/slam/visualSLAM.h +++ b/gtsam/slam/visualSLAM.h @@ -23,9 +23,8 @@ #include #include #include -#include +#include #include -#include #include #include #include @@ -35,30 +34,29 @@ namespace visualSLAM { using namespace gtsam; + /// Convenience function for constructing a pose key + inline Symbol PoseKey(Index j) { return Symbol('x', j); } + + /// Convenience function for constructing a pose key + inline Symbol PointKey(Index j) { return Symbol('l', j); } + /** * Typedefs that make up the visualSLAM namespace. */ - typedef TypedSymbol PoseKey; ///< The key type used for poses - typedef TypedSymbol PointKey; ///< The key type used for points - typedef Values PoseValues; ///< Values used for poses - typedef Values PointValues; ///< Values used for points - typedef TupleValues2 Values; ///< Values data structure - typedef boost::shared_ptr shared_values; ///< shared pointer to values data structure - - typedef NonlinearEquality PoseConstraint; ///< put a hard constraint on a pose - typedef NonlinearEquality PointConstraint; ///< put a hard constraint on a point - typedef PriorFactor PosePrior; ///< put a soft prior on a Pose - typedef PriorFactor PointPrior; ///< put a soft prior on a point - typedef RangeFactor RangeFactor; ///< put a factor on the range from a pose to a point + typedef NonlinearEquality PoseConstraint; ///< put a hard constraint on a pose + typedef NonlinearEquality PointConstraint; ///< put a hard constraint on a point + typedef PriorFactor PosePrior; ///< put a soft prior on a Pose + typedef PriorFactor PointPrior; ///< put a soft prior on a point + typedef RangeFactor RangeFactor; ///< put a factor on the range from a pose to a point /// monocular and stereo camera typedefs for general use - typedef GenericProjectionFactor ProjectionFactor; - typedef GenericStereoFactor StereoFactor; + typedef GenericProjectionFactor ProjectionFactor; + typedef GenericStereoFactor StereoFactor; /** * Non-linear factor graph for vanilla visual SLAM */ - class Graph: public NonlinearFactorGraph { + class Graph: public NonlinearFactorGraph { public: /// shared pointer to this type of graph @@ -69,85 +67,85 @@ namespace visualSLAM { } /// print out graph - void print(const std::string& s = "") const { - NonlinearFactorGraph::print(s); + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + NonlinearFactorGraph::print(s, keyFormatter); } /// check if two graphs are equal bool equals(const Graph& p, double tol = 1e-9) const { - return NonlinearFactorGraph::equals(p, tol); + return NonlinearFactorGraph::equals(p, tol); } /** * Add a projection factor measurement (monocular) - * @param z the measurement + * @param measured the measurement * @param model the noise model for the measurement - * @param i index of camera - * @param j index of point + * @param poseKey variable key for the camera pose + * @param pointKey variable key for the landmark * @param K shared pointer to calibration object */ - void addMeasurement(const Point2& z, const SharedNoiseModel& model, - PoseKey i, PointKey j, const shared_ptrK& K) { - boost::shared_ptr factor(new ProjectionFactor(z, model, i, j, K)); + void addMeasurement(const Point2& measured, const SharedNoiseModel& model, + Index poseKey, Index pointKey, const shared_ptrK& K) { + boost::shared_ptr factor(new ProjectionFactor(measured, model, PoseKey(poseKey), PointKey(pointKey), K)); push_back(factor); } /** * Add a constraint on a pose (for now, *must* be satisfied in any Values) - * @param j index of camera + * @param key variable key of the camera pose * @param p to which pose to constrain it to */ - void addPoseConstraint(int j, const Pose3& p = Pose3()) { - boost::shared_ptr factor(new PoseConstraint(j, p)); + void addPoseConstraint(Index poseKey, const Pose3& p = Pose3()) { + boost::shared_ptr factor(new PoseConstraint(PoseKey(poseKey), p)); push_back(factor); } /** * Add a constraint on a point (for now, *must* be satisfied in any Values) - * @param j index of landmark + * @param key variable key of the landmark * @param p point around which soft prior is defined */ - void addPointConstraint(int j, const Point3& p = Point3()) { - boost::shared_ptr factor(new PointConstraint(j, p)); + void addPointConstraint(Index pointKey, const Point3& p = Point3()) { + boost::shared_ptr factor(new PointConstraint(PointKey(pointKey), p)); push_back(factor); } /** * Add a prior on a pose - * @param j index of camera + * @param key variable key of the camera pose * @param p around which soft prior is defined * @param model uncertainty model of this prior */ - void addPosePrior(int j, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) { - boost::shared_ptr factor(new PosePrior(j, p, model)); + void addPosePrior(Index poseKey, const Pose3& p = Pose3(), const SharedNoiseModel& model = noiseModel::Unit::Create(6)) { + boost::shared_ptr factor(new PosePrior(PoseKey(poseKey), p, model)); push_back(factor); } /** * Add a prior on a landmark - * @param j index of landmark + * @param key variable key of the landmark * @param p to which point to constrain it to * @param model uncertainty model of this prior */ - void addPointPrior(int j, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) { - boost::shared_ptr factor(new PointPrior(j, p, model)); + void addPointPrior(Index pointKey, const Point3& p = Point3(), const SharedNoiseModel& model = noiseModel::Unit::Create(3)) { + boost::shared_ptr factor(new PointPrior(PointKey(pointKey), p, model)); push_back(factor); } /** * Add a range prior to a landmark - * @param i index of pose - * @param j index of landmark + * @param poseKey variable key of the camera pose + * @param pointKey variable key of the landmark * @param range approximate range to landmark * @param model uncertainty model of this prior */ - void addRangeFactor(PoseKey i, PointKey j, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) { - push_back(boost::shared_ptr(new RangeFactor(i, j, range, model))); + void addRangeFactor(Index poseKey, Index pointKey, double range, const SharedNoiseModel& model = noiseModel::Unit::Create(1)) { + push_back(boost::shared_ptr(new RangeFactor(PoseKey(poseKey), PointKey(pointKey), range, model))); } }; // Graph /// typedef for Optimizer. The current default will use the multi-frontal solver - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; } // namespaces diff --git a/m4/ac_doxygen.m4 b/m4/ac_doxygen.m4 deleted file mode 100644 index e9c56c206..000000000 --- a/m4/ac_doxygen.m4 +++ /dev/null @@ -1,312 +0,0 @@ -# This file is part of Autoconf. -*- Autoconf -*- - -# Copyright (C) 2004 Oren Ben-Kiki -# This file is distributed under the same terms as the Autoconf macro files. - -# Generate automatic documentation using Doxygen. Works in concert with the -# aminclude.m4 file and a compatible doxygen configuration file. Defines the -# following public macros: -# -# DX_???_FEATURE(ON|OFF) - control the default setting fo a Doxygen feature. -# Supported features are 'DOXYGEN' itself, 'DOT' for generating graphics, -# 'HTML' for plain HTML, 'CHM' for compressed HTML help (for MS users), 'CHI' -# for generating a seperate .chi file by the .chm file, and 'MAN', 'RTF', -# 'XML', 'PDF' and 'PS' for the appropriate output formats. The environment -# variable DOXYGEN_PAPER_SIZE may be specified to override the default 'a4wide' -# paper size. -# -# By default, HTML, PDF and PS documentation is generated as this seems to be -# the most popular and portable combination. MAN pages created by Doxygen are -# usually problematic, though by picking an appropriate subset and doing some -# massaging they might be better than nothing. CHM and RTF are specific for MS -# (note that you can't generate both HTML and CHM at the same time). The XML is -# rather useless unless you apply specialized post-processing to it. -# -# The macro mainly controls the default state of the feature. The use can -# override the default by specifying --enable or --disable. The macros ensure -# that contradictory flags are not given (e.g., --enable-doxygen-html and -# --enable-doxygen-chm, --enable-doxygen-anything with --disable-doxygen, etc.) -# Finally, each feature will be automatically disabled (with a warning) if the -# required programs are missing. -# -# Once all the feature defaults have been specified, call DX_INIT_DOXYGEN with -# the following parameters: a one-word name for the project for use as a -# filename base etc., an optional configuration file name (the default is -# 'Doxyfile', the same as Doxygen's default), and an optional output directory -# name (the default is 'doxygen-doc'). - -## ----------## -## Defaults. ## -## ----------## - -DX_ENV="" -AC_DEFUN([DX_FEATURE_doc], ON) -AC_DEFUN([DX_FEATURE_dot], ON) -AC_DEFUN([DX_FEATURE_man], OFF) -AC_DEFUN([DX_FEATURE_html], ON) -AC_DEFUN([DX_FEATURE_chm], OFF) -AC_DEFUN([DX_FEATURE_chi], OFF) -AC_DEFUN([DX_FEATURE_rtf], OFF) -AC_DEFUN([DX_FEATURE_xml], OFF) -AC_DEFUN([DX_FEATURE_pdf], ON) -AC_DEFUN([DX_FEATURE_ps], ON) - -## --------------- ## -## Private macros. ## -## --------------- ## - -# DX_ENV_APPEND(VARIABLE, VALUE) -# ------------------------------ -# Append VARIABLE="VALUE" to DX_ENV for invoking doxygen. -AC_DEFUN([DX_ENV_APPEND], [AC_SUBST([DX_ENV], ["$DX_ENV $1='$2'"])]) - -# DX_DIRNAME_EXPR -# --------------- -# Expand into a shell expression prints the directory part of a path. -AC_DEFUN([DX_DIRNAME_EXPR], - [[expr ".$1" : '\(\.\)[^/]*$' \| "x$1" : 'x\(.*\)/[^/]*$']]) - -# DX_IF_FEATURE(FEATURE, IF-ON, IF-OFF) -# ------------------------------------- -# Expands according to the M4 (static) status of the feature. -AC_DEFUN([DX_IF_FEATURE], [ifelse(DX_FEATURE_$1, ON, [$2], [$3])]) - -# DX_REQUIRE_PROG(VARIABLE, PROGRAM) -# ---------------------------------- -# Require the specified program to be found for the DX_CURRENT_FEATURE to work. -AC_DEFUN([DX_REQUIRE_PROG], [ -AC_PATH_TOOL([$1], [$2]) -if test "$DX_FLAG_[]DX_CURRENT_FEATURE$$1" = 1; then - AC_MSG_WARN([$2 not found - will not DX_CURRENT_DESCRIPTION]) - AC_SUBST([DX_FLAG_]DX_CURRENT_FEATURE, 0) -fi -]) - -# DX_TEST_FEATURE(FEATURE) -# ------------------------ -# Expand to a shell expression testing whether the feature is active. -AC_DEFUN([DX_TEST_FEATURE], [test "$DX_FLAG_$1" = 1]) - -# DX_CHECK_DEPEND(REQUIRED_FEATURE, REQUIRED_STATE) -# ------------------------------------------------- -# Verify that a required features has the right state before trying to turn on -# the DX_CURRENT_FEATURE. -AC_DEFUN([DX_CHECK_DEPEND], [ -test "$DX_FLAG_$1" = "$2" \ -|| AC_MSG_ERROR([doxygen-DX_CURRENT_FEATURE ifelse([$2], 1, - requires, contradicts) doxygen-DX_CURRENT_FEATURE]) -]) - -# DX_CLEAR_DEPEND(FEATURE, REQUIRED_FEATURE, REQUIRED_STATE) -# ---------------------------------------------------------- -# Turn off the DX_CURRENT_FEATURE if the required feature is off. -AC_DEFUN([DX_CLEAR_DEPEND], [ -test "$DX_FLAG_$1" = "$2" || AC_SUBST([DX_FLAG_]DX_CURRENT_FEATURE, 0) -]) - -# DX_FEATURE_ARG(FEATURE, DESCRIPTION, -# CHECK_DEPEND, CLEAR_DEPEND, -# REQUIRE, DO-IF-ON, DO-IF-OFF) -# -------------------------------------------- -# Parse the command-line option controlling a feature. CHECK_DEPEND is called -# if the user explicitly turns the feature on (and invokes DX_CHECK_DEPEND), -# otherwise CLEAR_DEPEND is called to turn off the default state if a required -# feature is disabled (using DX_CLEAR_DEPEND). REQUIRE performs additional -# requirement tests (DX_REQUIRE_PROG). Finally, an automake flag is set and -# DO-IF-ON or DO-IF-OFF are called according to the final state of the feature. -AC_DEFUN([DX_ARG_ABLE], [ - AC_DEFUN([DX_CURRENT_FEATURE], [$1]) - AC_DEFUN([DX_CURRENT_DESCRIPTION], [$2]) - AC_ARG_ENABLE(doxygen-$1, - [AS_HELP_STRING(DX_IF_FEATURE([$1], [--disable-doxygen-$1], - [--enable-doxygen-$1]), - DX_IF_FEATURE([$1], [don't $2], [$2]))], - [ -case "$enableval" in -#( -y|Y|yes|Yes|YES) - AC_SUBST([DX_FLAG_$1], 1) - $3 -;; #( -n|N|no|No|NO) - AC_SUBST([DX_FLAG_$1], 0) -;; #( -*) - AC_MSG_ERROR([invalid value '$enableval' given to doxygen-$1]) -;; -esac -], [ -AC_SUBST([DX_FLAG_$1], [DX_IF_FEATURE([$1], 1, 0)]) -$4 -]) -if DX_TEST_FEATURE([$1]); then - $5 - : -fi -if DX_TEST_FEATURE([$1]); then - AM_CONDITIONAL(DX_COND_$1, :) - $6 - : -else - AM_CONDITIONAL(DX_COND_$1, false) - $7 - : -fi -]) - -## -------------- ## -## Public macros. ## -## -------------- ## - -# DX_XXX_FEATURE(DEFAULT_STATE) -# ----------------------------- -AC_DEFUN([DX_DOXYGEN_FEATURE], [AC_DEFUN([DX_FEATURE_doc], [$1])]) -AC_DEFUN([DX_MAN_FEATURE], [AC_DEFUN([DX_FEATURE_man], [$1])]) -AC_DEFUN([DX_HTML_FEATURE], [AC_DEFUN([DX_FEATURE_html], [$1])]) -AC_DEFUN([DX_CHM_FEATURE], [AC_DEFUN([DX_FEATURE_chm], [$1])]) -AC_DEFUN([DX_CHI_FEATURE], [AC_DEFUN([DX_FEATURE_chi], [$1])]) -AC_DEFUN([DX_RTF_FEATURE], [AC_DEFUN([DX_FEATURE_rtf], [$1])]) -AC_DEFUN([DX_XML_FEATURE], [AC_DEFUN([DX_FEATURE_xml], [$1])]) -AC_DEFUN([DX_XML_FEATURE], [AC_DEFUN([DX_FEATURE_xml], [$1])]) -AC_DEFUN([DX_PDF_FEATURE], [AC_DEFUN([DX_FEATURE_pdf], [$1])]) -AC_DEFUN([DX_PS_FEATURE], [AC_DEFUN([DX_FEATURE_ps], [$1])]) - -# DX_INIT_DOXYGEN(PROJECT, [CONFIG-FILE], [OUTPUT-DOC-DIR]) -# --------------------------------------------------------- -# PROJECT also serves as the base name for the documentation files. -# The default CONFIG-FILE is "Doxyfile" and OUTPUT-DOC-DIR is "doxygen-doc". -AC_DEFUN([DX_INIT_DOXYGEN], [ - -# Files: -AC_SUBST([DX_PROJECT], [$1]) -AC_SUBST([DX_CONFIG], [ifelse([$2], [], Doxyfile, [$2])]) -AC_SUBST([DX_DOCDIR], [ifelse([$3], [], doxygen-doc, [$3])]) - -# Environment variables used inside doxygen.cfg: -DX_ENV_APPEND(SRCDIR, $srcdir) -DX_ENV_APPEND(PROJECT, $DX_PROJECT) -DX_ENV_APPEND(DOCDIR, $DX_DOCDIR) -DX_ENV_APPEND(VERSION, $PACKAGE_VERSION) - -# Doxygen itself: -DX_ARG_ABLE(doc, [generate any doxygen documentation], - [], - [], - [DX_REQUIRE_PROG([DX_DOXYGEN], doxygen) - DX_REQUIRE_PROG([DX_PERL], perl)], - [DX_ENV_APPEND(PERL_PATH, $DX_PERL)]) - -# Dot for graphics: -DX_ARG_ABLE(dot, [generate graphics for doxygen documentation], - [DX_CHECK_DEPEND(doc, 1)], - [DX_CLEAR_DEPEND(doc, 1)], - [DX_REQUIRE_PROG([DX_DOT], dot)], - [DX_ENV_APPEND(HAVE_DOT, YES) - DX_ENV_APPEND(DOT_PATH, [`DX_DIRNAME_EXPR($DX_DOT)`])], - [DX_ENV_APPEND(HAVE_DOT, NO)]) - -# Man pages generation: -DX_ARG_ABLE(man, [generate doxygen manual pages], - [DX_CHECK_DEPEND(doc, 1)], - [DX_CLEAR_DEPEND(doc, 1)], - [], - [DX_ENV_APPEND(GENERATE_MAN, YES)], - [DX_ENV_APPEND(GENERATE_MAN, NO)]) - -# RTF file generation: -DX_ARG_ABLE(rtf, [generate doxygen RTF documentation], - [DX_CHECK_DEPEND(doc, 1)], - [DX_CLEAR_DEPEND(doc, 1)], - [], - [DX_ENV_APPEND(GENERATE_RTF, YES)], - [DX_ENV_APPEND(GENERATE_RTF, NO)]) - -# XML file generation: -DX_ARG_ABLE(xml, [generate doxygen XML documentation], - [DX_CHECK_DEPEND(doc, 1)], - [DX_CLEAR_DEPEND(doc, 1)], - [], - [DX_ENV_APPEND(GENERATE_XML, YES)], - [DX_ENV_APPEND(GENERATE_XML, NO)]) - -# (Compressed) HTML help generation: -DX_ARG_ABLE(chm, [generate doxygen compressed HTML help documentation], - [DX_CHECK_DEPEND(doc, 1)], - [DX_CLEAR_DEPEND(doc, 1)], - [DX_REQUIRE_PROG([DX_HHC], hhc)], - [DX_ENV_APPEND(HHC_PATH, $DX_HHC) - DX_ENV_APPEND(GENERATE_HTML, YES) - DX_ENV_APPEND(GENERATE_HTMLHELP, YES)], - [DX_ENV_APPEND(GENERATE_HTMLHELP, NO)]) - -# Seperate CHI file generation. -DX_ARG_ABLE(chi, [generate doxygen seperate compressed HTML help index file], - [DX_CHECK_DEPEND(chm, 1)], - [DX_CLEAR_DEPEND(chm, 1)], - [], - [DX_ENV_APPEND(GENERATE_CHI, YES)], - [DX_ENV_APPEND(GENERATE_CHI, NO)]) - -# Plain HTML pages generation: -DX_ARG_ABLE(html, [generate doxygen plain HTML documentation], - [DX_CHECK_DEPEND(doc, 1) DX_CHECK_DEPEND(chm, 0)], - [DX_CLEAR_DEPEND(doc, 1) DX_CLEAR_DEPEND(chm, 0)], - [], - [DX_ENV_APPEND(GENERATE_HTML, YES)], - [DX_TEST_FEATURE(chm) || DX_ENV_APPEND(GENERATE_HTML, NO)]) - -# PostScript file generation: -DX_ARG_ABLE(ps, [generate doxygen PostScript documentation], - [DX_CHECK_DEPEND(doc, 1)], - [DX_CLEAR_DEPEND(doc, 1)], - [DX_REQUIRE_PROG([DX_LATEX], latex) - DX_REQUIRE_PROG([DX_MAKEINDEX], makeindex) - DX_REQUIRE_PROG([DX_DVIPS], dvips) - DX_REQUIRE_PROG([DX_EGREP], egrep)]) - -# PDF file generation: -DX_ARG_ABLE(pdf, [generate doxygen PDF documentation], - [DX_CHECK_DEPEND(doc, 1)], - [DX_CLEAR_DEPEND(doc, 1)], - [DX_REQUIRE_PROG([DX_PDFLATEX], pdflatex) - DX_REQUIRE_PROG([DX_MAKEINDEX], makeindex) - DX_REQUIRE_PROG([DX_EGREP], egrep)]) - -# LaTeX generation for PS and/or PDF: -if DX_TEST_FEATURE(ps) || DX_TEST_FEATURE(pdf); then - AM_CONDITIONAL(DX_COND_latex, :) - DX_ENV_APPEND(GENERATE_LATEX, YES) -else - AM_CONDITIONAL(DX_COND_latex, false) - DX_ENV_APPEND(GENERATE_LATEX, NO) -fi - -# Paper size for PS and/or PDF: -AC_ARG_VAR(DOXYGEN_PAPER_SIZE, - [a4wide (default), a4, letter, legal or executive]) -case "$DOXYGEN_PAPER_SIZE" in -#( -"") - AC_SUBST(DOXYGEN_PAPER_SIZE, "") -;; #( -a4wide|a4|letter|legal|executive) - DX_ENV_APPEND(PAPER_SIZE, $DOXYGEN_PAPER_SIZE) -;; #( -*) - AC_MSG_ERROR([unknown DOXYGEN_PAPER_SIZE='$DOXYGEN_PAPER_SIZE']) -;; -esac - -#For debugging: -#echo DX_FLAG_doc=$DX_FLAG_doc -#echo DX_FLAG_dot=$DX_FLAG_dot -#echo DX_FLAG_man=$DX_FLAG_man -#echo DX_FLAG_html=$DX_FLAG_html -#echo DX_FLAG_chm=$DX_FLAG_chm -#echo DX_FLAG_chi=$DX_FLAG_chi -#echo DX_FLAG_rtf=$DX_FLAG_rtf -#echo DX_FLAG_xml=$DX_FLAG_xml -#echo DX_FLAG_pdf=$DX_FLAG_pdf -#echo DX_FLAG_ps=$DX_FLAG_ps -#echo DX_ENV=$DX_ENV -]) diff --git a/m4/ax_boost_base.m4 b/m4/ax_boost_base.m4 deleted file mode 100644 index ec23a0abb..000000000 --- a/m4/ax_boost_base.m4 +++ /dev/null @@ -1,257 +0,0 @@ -# =========================================================================== -# http://www.gnu.org/software/autoconf-archive/ax_boost_base.html -# =========================================================================== -# -# SYNOPSIS -# -# AX_BOOST_BASE([MINIMUM-VERSION], [ACTION-IF-FOUND], [ACTION-IF-NOT-FOUND]) -# -# DESCRIPTION -# -# Test for the Boost C++ libraries of a particular version (or newer) -# -# If no path to the installed boost library is given the macro searchs -# under /usr, /usr/local, /opt and /opt/local and evaluates the -# $BOOST_ROOT environment variable. Further documentation is available at -# . -# -# This macro calls: -# -# AC_SUBST(BOOST_CPPFLAGS) / AC_SUBST(BOOST_LDFLAGS) -# -# And sets: -# -# HAVE_BOOST -# -# LICENSE -# -# Copyright (c) 2008 Thomas Porschberg -# Copyright (c) 2009 Peter Adolphs -# -# Copying and distribution of this file, with or without modification, are -# permitted in any medium without royalty provided the copyright notice -# and this notice are preserved. This file is offered as-is, without any -# warranty. - -#serial 18 - -AC_DEFUN([AX_BOOST_BASE], -[ -AC_ARG_WITH([boost], - [AS_HELP_STRING([--with-boost@<:@=ARG@:>@], - [use Boost library from a standard location (ARG=yes), - from the specified location (ARG=), - or disable it (ARG=no) - @<:@ARG=yes@:>@ ])], - [ - if test "$withval" = "no"; then - want_boost="no" - elif test "$withval" = "yes"; then - want_boost="yes" - ac_boost_path="" - else - want_boost="yes" - ac_boost_path="$withval" - fi - ], - [want_boost="yes"]) - - -AC_ARG_WITH([boost-libdir], - AS_HELP_STRING([--with-boost-libdir=LIB_DIR], - [Force given directory for boost libraries. Note that this will overwrite library path detection, so use this parameter only if default library detection fails and you know exactly where your boost libraries are located.]), - [ - if test -d "$withval" - then - ac_boost_lib_path="$withval" - else - AC_MSG_ERROR(--with-boost-libdir expected directory name) - fi - ], - [ac_boost_lib_path=""] -) - -if test "x$want_boost" = "xyes"; then - boost_lib_version_req=ifelse([$1], ,1.20.0,$1) - boost_lib_version_req_shorten=`expr $boost_lib_version_req : '\([[0-9]]*\.[[0-9]]*\)'` - boost_lib_version_req_major=`expr $boost_lib_version_req : '\([[0-9]]*\)'` - boost_lib_version_req_minor=`expr $boost_lib_version_req : '[[0-9]]*\.\([[0-9]]*\)'` - boost_lib_version_req_sub_minor=`expr $boost_lib_version_req : '[[0-9]]*\.[[0-9]]*\.\([[0-9]]*\)'` - if test "x$boost_lib_version_req_sub_minor" = "x" ; then - boost_lib_version_req_sub_minor="0" - fi - WANT_BOOST_VERSION=`expr $boost_lib_version_req_major \* 100000 \+ $boost_lib_version_req_minor \* 100 \+ $boost_lib_version_req_sub_minor` - AC_MSG_CHECKING(for boostlib >= $boost_lib_version_req) - succeeded=no - - dnl On x86_64 systems check for system libraries in both lib64 and lib. - dnl The former is specified by FHS, but e.g. Debian does not adhere to - dnl this (as it rises problems for generic multi-arch support). - dnl The last entry in the list is chosen by default when no libraries - dnl are found, e.g. when only header-only libraries are installed! - libsubdirs="lib" - if test `uname -m` = x86_64; then - libsubdirs="lib64 lib lib64" - fi - - dnl first we check the system location for boost libraries - dnl this location ist chosen if boost libraries are installed with the --layout=system option - dnl or if you install boost with RPM - if test "$ac_boost_path" != ""; then - BOOST_CPPFLAGS="-I$ac_boost_path/include" - for ac_boost_path_tmp in $libsubdirs; do - if test -d "$ac_boost_path"/"$ac_boost_path_tmp" ; then - BOOST_LDFLAGS="-L$ac_boost_path/$ac_boost_path_tmp" - break - fi - done - elif test "$cross_compiling" != yes; then - for ac_boost_path_tmp in /usr /usr/local /opt /opt/local ; do - if test -d "$ac_boost_path_tmp/include/boost" && test -r "$ac_boost_path_tmp/include/boost"; then - for libsubdir in $libsubdirs ; do - if ls "$ac_boost_path_tmp/$libsubdir/libboost_"* >/dev/null 2>&1 ; then break; fi - done - BOOST_LDFLAGS="-L$ac_boost_path_tmp/$libsubdir" - BOOST_CPPFLAGS="-I$ac_boost_path_tmp/include" - break; - fi - done - fi - - dnl overwrite ld flags if we have required special directory with - dnl --with-boost-libdir parameter - if test "$ac_boost_lib_path" != ""; then - BOOST_LDFLAGS="-L$ac_boost_lib_path" - fi - - CPPFLAGS_SAVED="$CPPFLAGS" - CPPFLAGS="$CPPFLAGS $BOOST_CPPFLAGS" - export CPPFLAGS - - LDFLAGS_SAVED="$LDFLAGS" - LDFLAGS="$LDFLAGS $BOOST_LDFLAGS" - export LDFLAGS - - AC_REQUIRE([AC_PROG_CXX]) - AC_LANG_PUSH(C++) - AC_COMPILE_IFELSE([AC_LANG_PROGRAM([[ - @%:@include - ]], [[ - #if BOOST_VERSION >= $WANT_BOOST_VERSION - // Everything is okay - #else - # error Boost version is too old - #endif - ]])],[ - AC_MSG_RESULT(yes) - succeeded=yes - found_system=yes - ],[ - ]) - AC_LANG_POP([C++]) - - - - dnl if we found no boost with system layout we search for boost libraries - dnl built and installed without the --layout=system option or for a staged(not installed) version - if test "x$succeeded" != "xyes"; then - _version=0 - if test "$ac_boost_path" != ""; then - if test -d "$ac_boost_path" && test -r "$ac_boost_path"; then - for i in `ls -d $ac_boost_path/include/boost-* 2>/dev/null`; do - _version_tmp=`echo $i | sed "s#$ac_boost_path##" | sed 's/\/include\/boost-//' | sed 's/_/./'` - V_CHECK=`expr $_version_tmp \> $_version` - if test "$V_CHECK" = "1" ; then - _version=$_version_tmp - fi - VERSION_UNDERSCORE=`echo $_version | sed 's/\./_/'` - BOOST_CPPFLAGS="-I$ac_boost_path/include/boost-$VERSION_UNDERSCORE" - done - fi - else - if test "$cross_compiling" != yes; then - for ac_boost_path in /usr /usr/local /opt /opt/local ; do - if test -d "$ac_boost_path" && test -r "$ac_boost_path"; then - for i in `ls -d $ac_boost_path/include/boost-* 2>/dev/null`; do - _version_tmp=`echo $i | sed "s#$ac_boost_path##" | sed 's/\/include\/boost-//' | sed 's/_/./'` - V_CHECK=`expr $_version_tmp \> $_version` - if test "$V_CHECK" = "1" ; then - _version=$_version_tmp - best_path=$ac_boost_path - fi - done - fi - done - - VERSION_UNDERSCORE=`echo $_version | sed 's/\./_/'` - BOOST_CPPFLAGS="-I$best_path/include/boost-$VERSION_UNDERSCORE" - if test "$ac_boost_lib_path" = ""; then - for libsubdir in $libsubdirs ; do - if ls "$best_path/$libsubdir/libboost_"* >/dev/null 2>&1 ; then break; fi - done - BOOST_LDFLAGS="-L$best_path/$libsubdir" - fi - fi - - if test "x$BOOST_ROOT" != "x"; then - for libsubdir in $libsubdirs ; do - if ls "$BOOST_ROOT/stage/$libsubdir/libboost_"* >/dev/null 2>&1 ; then break; fi - done - if test -d "$BOOST_ROOT" && test -r "$BOOST_ROOT" && test -d "$BOOST_ROOT/stage/$libsubdir" && test -r "$BOOST_ROOT/stage/$libsubdir"; then - version_dir=`expr //$BOOST_ROOT : '.*/\(.*\)'` - stage_version=`echo $version_dir | sed 's/boost_//' | sed 's/_/./g'` - stage_version_shorten=`expr $stage_version : '\([[0-9]]*\.[[0-9]]*\)'` - V_CHECK=`expr $stage_version_shorten \>\= $_version` - if test "$V_CHECK" = "1" -a "$ac_boost_lib_path" = "" ; then - AC_MSG_NOTICE(We will use a staged boost library from $BOOST_ROOT) - BOOST_CPPFLAGS="-I$BOOST_ROOT" - BOOST_LDFLAGS="-L$BOOST_ROOT/stage/$libsubdir" - fi - fi - fi - fi - - CPPFLAGS="$CPPFLAGS $BOOST_CPPFLAGS" - export CPPFLAGS - LDFLAGS="$LDFLAGS $BOOST_LDFLAGS" - export LDFLAGS - - AC_LANG_PUSH(C++) - AC_COMPILE_IFELSE([AC_LANG_PROGRAM([[ - @%:@include - ]], [[ - #if BOOST_VERSION >= $WANT_BOOST_VERSION - // Everything is okay - #else - # error Boost version is too old - #endif - ]])],[ - AC_MSG_RESULT(yes) - succeeded=yes - found_system=yes - ],[ - ]) - AC_LANG_POP([C++]) - fi - - if test "$succeeded" != "yes" ; then - if test "$_version" = "0" ; then - AC_MSG_NOTICE([[We could not detect the boost libraries (version $boost_lib_version_req_shorten or higher). If you have a staged boost library (still not installed) please specify \$BOOST_ROOT in your environment and do not give a PATH to --with-boost option. If you are sure you have boost installed, then check your version number looking in . See http://randspringer.de/boost for more documentation.]]) - else - AC_MSG_NOTICE([Your boost libraries seems to old (version $_version).]) - fi - # execute ACTION-IF-NOT-FOUND (if present): - ifelse([$3], , :, [$3]) - else - AC_SUBST(BOOST_CPPFLAGS) - AC_SUBST(BOOST_LDFLAGS) - AC_DEFINE(HAVE_BOOST,,[define if the Boost library is available]) - # execute ACTION-IF-FOUND (if present): - ifelse([$2], , :, [$2]) - fi - - CPPFLAGS="$CPPFLAGS_SAVED" - LDFLAGS="$LDFLAGS_SAVED" -fi - -]) diff --git a/myconfigure b/myconfigure deleted file mode 100755 index 06c23ff71..000000000 --- a/myconfigure +++ /dev/null @@ -1 +0,0 @@ -../configure --prefix=$HOME --enable-install-cppunitlite CFLAGS="-fno-inline -g -Wall" CXXFLAGS="-fno-inline -g -Wall" LDFLAGS="-fno-inline -g -Wall" --disable-static --disable-fast-install diff --git a/myconfigure.ardrone b/myconfigure.ardrone deleted file mode 100755 index 117bcdadf..000000000 --- a/myconfigure.ardrone +++ /dev/null @@ -1 +0,0 @@ -../configure --build=i686-pc-linux-gnu --host=arm-none-linux-gnueabi -prefix=/usr CFLAGS="-fno-inline -g -Wall" CXXFLAGS="-fno-inline -g -Wall" LDFLAGS="-fno-inline -g -Wall" --disable-static diff --git a/myconfigure.matlab b/myconfigure.matlab deleted file mode 100755 index d8f33f7fe..000000000 --- a/myconfigure.matlab +++ /dev/null @@ -1 +0,0 @@ -../configure --prefix=$HOME --with-toolbox=$HOME/toolbox --enable-build-toolbox --enable-install-matlab-tests --enable-install-matlab-examples --enable-install-wrap --with-wrap=$HOME/bin --enable-install-cppunitlite CFLAGS="-fno-inline -g -Wall" CXXFLAGS="-fno-inline -g -Wall" LDFLAGS="-fno-inline -g -Wall" --disable-static --disable-fast-install diff --git a/myconfigure.overo b/myconfigure.overo deleted file mode 100755 index c3889973f..000000000 --- a/myconfigure.overo +++ /dev/null @@ -1 +0,0 @@ -GCC=/home/uuv/overo-oe/tmp/cross/armv7a/bin/arm-angstrom-linux-gnueabi-gcc CXX=$OVEROTOP/tmp/cross/armv7a/bin/arm-angstrom-linux-gnueabi-g++ CC=$OVEROTOP/tmp/cross/armv7a/bin/arm-angstrom-linux-gnueabi-gcc LD=$OVEROTOP/tmp/cross/armv7a/bin/arm-angstrom-linux-gnueabi-ld ./configure --prefix=$HOME --build=i686-pc-linux-gnu --host=arm-angstrom-linux-gnueabi --with-boost=$OVERO_ROOT/usr/include CXXFLAGS="--sysroot=$OVEROTOP/tmp/staging/armv7a-angstrom-linux-gnueabi -g -O3 -DNDEBUG" CCFLAGS="--sysroot=$OVEROTOP/tmp/staging/armv7a-angstrom-linux-gnueabi -g -O3 -DNDEBUG" --disable-static diff --git a/myconfigure.profiling b/myconfigure.profiling deleted file mode 100755 index 76fd19208..000000000 --- a/myconfigure.profiling +++ /dev/null @@ -1,3 +0,0 @@ - -#../configure --disable-fast-install --prefix=$HOME/borg-simplelinear CXXFLAGS="-fno-inline -g -D_GLIBCXX_DEBUG -DNDEBUG" CFLAGS="-fno-inline -g -D_GLIBCXX_DEBUG -DNDEBUG" LDFLAGS="-fno-inline -g " --disable-static -../configure --prefix=$HOME/borg CPPFLAGS="-g -DNDEBUG -O3" LDFLAGS="-g -DNDEBUG -O3" --disable-static diff --git a/myconfigure.timing b/myconfigure.timing deleted file mode 100755 index 972b35e12..000000000 --- a/myconfigure.timing +++ /dev/null @@ -1,3 +0,0 @@ -../configure --prefix=$HOME CPP="/opt/local/bin/cpp-mp-4.5" CC="/opt/local/bin/gcc-mp-4.5" CXX="/opt/local/bin/g++-mp-4.5" CPPFLAGS="-DENABLE_TIMING -DNDEBUG -O3" LDFLAGS="-DNDEBUG -O3" --disable-static - - diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 4ada20785..8a28c8dad 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -15,14 +15,14 @@ endif (GTSAM_BUILD_CONVENIENCE_LIBRARIES) # exclude certain files # note the source dir on each set (tests_exclude - # "${CMAKE_CURRENT_SOURCE_DIR}/testPose2SLAMwSPCG.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/testPose2SLAMwSPCG.cpp" ) # Build tests if (GTSAM_BUILD_TESTS) add_custom_target(check.tests COMMAND ${CMAKE_CTEST_COMMAND}) file(GLOB tests_srcs "test*.cpp") - #list(REMOVE_ITEM tests_srcs ${tests_exclude}) #NOTE: uncomment if there are tests to exclude + list(REMOVE_ITEM tests_srcs ${tests_exclude}) foreach(test_src ${tests_srcs}) get_filename_component(test_base ${test_src} NAME_WE) set( test_bin tests.${test_base} ) @@ -40,7 +40,7 @@ endif (GTSAM_BUILD_TESTS) if (GTSAM_BUILD_TIMING) add_custom_target(timing.tests) file(GLOB timing_srcs "time*.cpp") - #list(REMOVE_ITEM timing_srcs ${tests_exclude}) #NOTE: uncomment if there are tests to exclude + list(REMOVE_ITEM timing_srcs ${tests_exclude}) foreach(time_src ${timing_srcs}) get_filename_component(time_base ${time_src} NAME_WE) set( time_bin tests.${time_base} ) diff --git a/tests/Makefile.am b/tests/Makefile.am deleted file mode 100644 index cc1b34162..000000000 --- a/tests/Makefile.am +++ /dev/null @@ -1,55 +0,0 @@ -#---------------------------------------------------------------------------------------------------- -# GTSAM tests -# More elaborate unit tests that test functionality with slam examples -#---------------------------------------------------------------------------------------------------- - -# use nostdinc to turn off -I. and -I.., we do not need them because -# header files are qualified so they can be included in external projects. -AUTOMAKE_OPTIONS = nostdinc - -check_PROGRAMS = -check_PROGRAMS += testGaussianBayesNet testGaussianFactor testGaussianFactorGraph -check_PROGRAMS += testGaussianISAM -check_PROGRAMS += testGraph -check_PROGRAMS += testInference -check_PROGRAMS += testGaussianJunctionTree -check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorGraph -check_PROGRAMS += testNonlinearOptimizer testDoglegOptimizer -check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph -check_PROGRAMS += testTupleValues -check_PROGRAMS += testNonlinearISAM -check_PROGRAMS += testBoundingConstraint -check_PROGRAMS += testPose2SLAMwSPCG -check_PROGRAMS += testGaussianISAM2 -check_PROGRAMS += testExtendedKalmanFilter -check_PROGRAMS += testRot3Optimization - -if ENABLE_SERIALIZATION -check_PROGRAMS += testSerialization -endif - -# Timing tests -noinst_PROGRAMS = timeGaussianFactorGraph timeSequentialOnDataset timeMultifrontalOnDataset - -#---------------------------------------------------------------------------------------------------- -# rules to build unit tests -#---------------------------------------------------------------------------------------------------- -TESTS = $(check_PROGRAMS) -AM_LDFLAGS = $(BOOST_LDFLAGS) -AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) - -# link to serialization library for test -if ENABLE_SERIALIZATION -AM_LDFLAGS += -lboost_serialization -endif - -LDADD = ../gtsam/libgtsam.la ../CppUnitLite/libCppUnitLite.a -AM_DEFAULT_SOURCE_EXT = .cpp - -# rule to run an executable -%.run: % $(LDADD) - ./$^ - -# rule to run executable with valgrind -%.valgrind: % $(LDADD) - valgrind ./$^ diff --git a/tests/testBoundingConstraint.cpp b/tests/testBoundingConstraint.cpp index 522d2d9a8..09baec6e1 100644 --- a/tests/testBoundingConstraint.cpp +++ b/tests/testBoundingConstraint.cpp @@ -31,13 +31,13 @@ SharedDiagonal soft_model2 = noiseModel::Unit::Create(2); SharedDiagonal soft_model2_alt = noiseModel::Isotropic::Sigma(2, 0.1); SharedDiagonal hard_model1 = noiseModel::Constrained::All(1); -typedef NonlinearFactorGraph Graph; +typedef NonlinearFactorGraph Graph; typedef boost::shared_ptr shared_graph; -typedef boost::shared_ptr shared_values; -typedef NonlinearOptimizer Optimizer; +typedef boost::shared_ptr shared_values; +typedef NonlinearOptimizer Optimizer; // some simple inequality constraints -simulated2D::PoseKey key(1); +Symbol key(simulated2D::PoseKey(1)); double mu = 10.0; // greater than iq2D::PoseXInequality constraint1(key, 1.0, true, mu); @@ -50,7 +50,7 @@ iq2D::PoseYInequality constraint4(key, 2.0, false, mu); /* ************************************************************************* */ TEST( testBoundingConstraint, unary_basics_inactive1 ) { Point2 pt1(2.0, 3.0); - simulated2D::Values config; + Values config; config.insert(key, pt1); EXPECT(!constraint1.active(config)); EXPECT(!constraint2.active(config)); @@ -69,7 +69,7 @@ TEST( testBoundingConstraint, unary_basics_inactive1 ) { /* ************************************************************************* */ TEST( testBoundingConstraint, unary_basics_inactive2 ) { Point2 pt2(-2.0, -3.0); - simulated2D::Values config; + Values config; config.insert(key, pt2); EXPECT(!constraint3.active(config)); EXPECT(!constraint4.active(config)); @@ -88,7 +88,7 @@ TEST( testBoundingConstraint, unary_basics_inactive2 ) { /* ************************************************************************* */ TEST( testBoundingConstraint, unary_basics_active1 ) { Point2 pt2(-2.0, -3.0); - simulated2D::Values config; + Values config; config.insert(key, pt2); EXPECT(constraint1.active(config)); EXPECT(constraint2.active(config)); @@ -103,7 +103,7 @@ TEST( testBoundingConstraint, unary_basics_active1 ) { /* ************************************************************************* */ TEST( testBoundingConstraint, unary_basics_active2 ) { Point2 pt1(2.0, 3.0); - simulated2D::Values config; + Values config; config.insert(key, pt1); EXPECT(constraint3.active(config)); EXPECT(constraint4.active(config)); @@ -118,7 +118,7 @@ TEST( testBoundingConstraint, unary_basics_active2 ) { /* ************************************************************************* */ TEST( testBoundingConstraint, unary_linearization_inactive) { Point2 pt1(2.0, 3.0); - simulated2D::Values config1; + Values config1; config1.insert(key, pt1); Ordering ordering; ordering += key; @@ -131,7 +131,7 @@ TEST( testBoundingConstraint, unary_linearization_inactive) { /* ************************************************************************* */ TEST( testBoundingConstraint, unary_linearization_active) { Point2 pt2(-2.0, -3.0); - simulated2D::Values config2; + Values config2; config2.insert(key, pt2); Ordering ordering; ordering += key; @@ -151,16 +151,16 @@ TEST( testBoundingConstraint, unary_simple_optimization1) { Point2 start_pt(0.0, 1.0); shared_graph graph(new Graph()); - simulated2D::PoseKey x1(1); + Symbol x1('x',1); graph->add(iq2D::PoseXInequality(x1, 1.0, true)); graph->add(iq2D::PoseYInequality(x1, 2.0, true)); graph->add(simulated2D::Prior(start_pt, soft_model2, x1)); - shared_values initValues(new simulated2D::Values()); + shared_values initValues(new Values()); initValues->insert(x1, start_pt); Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues); - simulated2D::Values expected; + Values expected; expected.insert(x1, goal_pt); CHECK(assert_equal(expected, *actual, tol)); } @@ -173,23 +173,23 @@ TEST( testBoundingConstraint, unary_simple_optimization2) { Point2 start_pt(2.0, 3.0); shared_graph graph(new Graph()); - simulated2D::PoseKey x1(1); + Symbol x1('x',1); graph->add(iq2D::PoseXInequality(x1, 1.0, false)); graph->add(iq2D::PoseYInequality(x1, 2.0, false)); graph->add(simulated2D::Prior(start_pt, soft_model2, x1)); - shared_values initValues(new simulated2D::Values()); + shared_values initValues(new Values()); initValues->insert(x1, start_pt); Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues); - simulated2D::Values expected; + Values expected; expected.insert(x1, goal_pt); CHECK(assert_equal(expected, *actual, tol)); } /* ************************************************************************* */ TEST( testBoundingConstraint, MaxDistance_basics) { - simulated2D::PoseKey key1(1), key2(2); + Symbol key1('x',1), key2('x',2); Point2 pt1, pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0); iq2D::PoseMaxDistConstraint rangeBound(key1, key2, 2.0, mu); EXPECT_DOUBLES_EQUAL(2.0, rangeBound.threshold(), tol); @@ -201,7 +201,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) { EXPECT(assert_equal(zero(1), rangeBound.evaluateError(pt1, pt3))); EXPECT(assert_equal(-1.0*ones(1), rangeBound.evaluateError(pt1, pt4))); - simulated2D::Values config1; + Values config1; config1.insert(key1, pt1); config1.insert(key2, pt1); Ordering ordering; ordering += key1, key2; @@ -231,18 +231,18 @@ TEST( testBoundingConstraint, MaxDistance_basics) { TEST( testBoundingConstraint, MaxDistance_simple_optimization) { Point2 pt1, pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0); - simulated2D::PoseKey x1(1), x2(2); + Symbol x1('x',1), x2('x',2); Graph graph; graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(pt1, x1)); graph.add(simulated2D::Prior(pt2_init, soft_model2_alt, x2)); graph.add(iq2D::PoseMaxDistConstraint(x1, x2, 2.0)); - simulated2D::Values initial_state; + Values initial_state; initial_state.insert(x1, pt1); initial_state.insert(x2, pt2_init); - simulated2D::Values expected; + Values expected; expected.insert(x1, pt1); expected.insert(x2, pt2_goal); @@ -254,8 +254,7 @@ TEST( testBoundingConstraint, MaxDistance_simple_optimization) { /* ************************************************************************* */ TEST( testBoundingConstraint, avoid_demo) { - simulated2D::PoseKey x1(1), x2(2), x3(3); - simulated2D::PointKey l1(1); + Symbol x1('x',1), x2('x',2), x3('x',3), l1('l',1); double radius = 1.0; Point2 x1_pt, x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0); Point2 odo(2.0, 0.0); @@ -268,7 +267,7 @@ TEST( testBoundingConstraint, avoid_demo) { graph.add(simulated2D::Odometry(odo, soft_model2_alt, x2, x3)); graph.add(simulated2D::equality_constraints::UnaryEqualityConstraint(x3_pt, x3)); - simulated2D::Values init, expected; + Values init, expected; init.insert(x1, x1_pt); init.insert(x3, x3_pt); init.insert(l1, l1_pt); diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index 2792d08b8..27070b801 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -385,7 +385,7 @@ TEST(DoglegOptimizer, Iterate) { // config far from minimum Point2 x0(3,0); - boost::shared_ptr config(new example::Values); + boost::shared_ptr config(new Values); config->insert(simulated2D::PoseKey(1), x0); // ordering @@ -404,7 +404,7 @@ TEST(DoglegOptimizer, Iterate) { DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, gbn, *fg, *config, *ord, fg->error(*config)); Delta = result.Delta; EXPECT(result.f_error < fg->error(*config)); // Check that error decreases - simulated2D::Values newConfig(config->retract(result.dx_d, *ord)); + Values newConfig(config->retract(result.dx_d, *ord)); (*config) = newConfig; DOUBLES_EQUAL(fg->error(*config), result.f_error, 1e-5); // Check that error is correctly filled in } diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index f747d71c8..927499050 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -20,15 +20,10 @@ #include #include #include -#include #include using namespace gtsam; -// Define Types for System Test -typedef TypedSymbol TestKey; -typedef Values TestValues; - /* ************************************************************************* */ TEST( ExtendedKalmanFilter, linear ) { @@ -37,10 +32,10 @@ TEST( ExtendedKalmanFilter, linear ) { SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); // Create an ExtendedKalmanFilter object - ExtendedKalmanFilter ekf(x_initial, P_initial); + ExtendedKalmanFilter ekf(x_initial, P_initial); // Create the TestKeys for our example - TestKey x0(0), x1(1), x2(2), x3(3); + Symbol x0('x',0), x1('x',1), x2('x',2), x3('x',3); // Create the controls and measurement properties for our example double dt = 1.0; @@ -59,30 +54,30 @@ TEST( ExtendedKalmanFilter, linear ) { // Run iteration 1 // Create motion factor - BetweenFactor factor1(x0, x1, difference, Q); + BetweenFactor factor1(x0, x1, difference, Q); Point2 predict1 = ekf.predict(factor1); EXPECT(assert_equal(expected1,predict1)); // Create the measurement factor - PriorFactor factor2(x1, z1, R); + PriorFactor factor2(x1, z1, R); Point2 update1 = ekf.update(factor2); EXPECT(assert_equal(expected1,update1)); // Run iteration 2 - BetweenFactor factor3(x1, x2, difference, Q); + BetweenFactor factor3(x1, x2, difference, Q); Point2 predict2 = ekf.predict(factor3); EXPECT(assert_equal(expected2,predict2)); - PriorFactor factor4(x2, z2, R); + PriorFactor factor4(x2, z2, R); Point2 update2 = ekf.update(factor4); EXPECT(assert_equal(expected2,update2)); // Run iteration 3 - BetweenFactor factor5(x2, x3, difference, Q); + BetweenFactor factor5(x2, x3, difference, Q); Point2 predict3 = ekf.predict(factor5); EXPECT(assert_equal(expected3,predict3)); - PriorFactor factor6(x3, z3, R); + PriorFactor factor6(x3, z3, R); Point2 update3 = ekf.update(factor6); EXPECT(assert_equal(expected3,update3)); @@ -91,12 +86,12 @@ TEST( ExtendedKalmanFilter, linear ) { // Create Motion Model Factor -class NonlinearMotionModel : public NonlinearFactor2 { +class NonlinearMotionModel : public NoiseModelFactor2 { public: - typedef TestKey::Value T; + typedef Point2 T; private: - typedef NonlinearFactor2 Base; + typedef NoiseModelFactor2 Base; typedef NonlinearMotionModel This; protected: @@ -106,7 +101,7 @@ protected: public: NonlinearMotionModel(){} - NonlinearMotionModel(const TestKey& TestKey1, const TestKey& TestKey2) : + NonlinearMotionModel(const Symbol& TestKey1, const Symbol& TestKey2) : Base(noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0)), TestKey1, TestKey2), Q_(2,2) { // Initialize motion model parameters: @@ -156,23 +151,23 @@ public: } /* print */ - virtual void print(const std::string& s = "") const { + virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << ": NonlinearMotionModel\n"; - std::cout << " TestKey1: " << (std::string) key1_ << std::endl; - std::cout << " TestKey2: " << (std::string) key2_ << std::endl; + std::cout << " TestKey1: " << keyFormatter(key1()) << std::endl; + std::cout << " TestKey2: " << keyFormatter(key2()) << std::endl; } /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ - virtual bool equals(const NonlinearFactor2& f, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { const This *e = dynamic_cast (&f); - return (e != NULL) && (key1_ == e->key1_) && (key2_ == e->key2_); + return (e != NULL) && (key1() == e->key1()) && (key2() == e->key2()); } /** * calculate the error of the factor * Override for systems with unusual noise models */ - virtual double error(const TestValues& c) const { + virtual double error(const Values& c) const { Vector w = whitenedError(c); return 0.5 * w.dot(w); } @@ -183,8 +178,8 @@ public: } /** Vector of errors, whitened ! */ - Vector whitenedError(const TestValues& c) const { - return QInvSqrt(c[key1_])*unwhitenedError(c); + Vector whitenedError(const Values& c) const { + return QInvSqrt(c.at(key1()))*unwhitenedError(c); } /** @@ -192,12 +187,12 @@ public: * Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z * Hence b = z - h(x1,x2) = - error_vector(x) */ - boost::shared_ptr linearize(const TestValues& c, const Ordering& ordering) const { - const X1& x1 = c[key1_]; - const X2& x2 = c[key2_]; + boost::shared_ptr linearize(const Values& c, const Ordering& ordering) const { + const X1& x1 = c.at(key1()); + const X2& x2 = c.at(key2()); Matrix A1, A2; Vector b = -evaluateError(x1, x2, A1, A2); - const Index var1 = ordering[key1_], var2 = ordering[key2_]; + const Index var1 = ordering[key1()], var2 = ordering[key2()]; SharedDiagonal constrained = boost::shared_dynamic_cast(this->noiseModel_); if (constrained.get() != NULL) { @@ -238,13 +233,13 @@ public: }; // Create Measurement Model Factor -class NonlinearMeasurementModel : public NonlinearFactor1 { +class NonlinearMeasurementModel : public NoiseModelFactor1 { public: - typedef TestKey::Value T; + typedef Point2 T; private: - typedef NonlinearFactor1 Base; + typedef NoiseModelFactor1 Base; typedef NonlinearMeasurementModel This; protected: @@ -255,7 +250,7 @@ protected: public: NonlinearMeasurementModel(){} - NonlinearMeasurementModel(const TestKey& TestKey, Vector z) : + NonlinearMeasurementModel(const Symbol& TestKey, Vector z) : Base(noiseModel::Diagonal::Sigmas(Vector_(2, 1.0, 1.0)), TestKey), z_(z), R_(1,1) { // Initialize nonlinear measurement model parameters: @@ -294,22 +289,22 @@ public: } /* print */ - virtual void print(const std::string& s = "") const { + virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << ": NonlinearMeasurementModel\n"; - std::cout << " TestKey: " << (std::string) key_ << std::endl; + std::cout << " TestKey: " << keyFormatter(key()) << std::endl; } /** Check if two factors are equal. Note type is IndexFactor and needs cast. */ - virtual bool equals(const NonlinearFactor1& f, double tol = 1e-9) const { + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { const This *e = dynamic_cast (&f); - return (e != NULL) && (key_ == e->key_); + return (e != NULL) && Base::equals(f); } /** * calculate the error of the factor * Override for systems with unusual noise models */ - virtual double error(const TestValues& c) const { + virtual double error(const Values& c) const { Vector w = whitenedError(c); return 0.5 * w.dot(w); } @@ -320,8 +315,8 @@ public: } /** Vector of errors, whitened ! */ - Vector whitenedError(const TestValues& c) const { - return RInvSqrt(c[key_])*unwhitenedError(c); + Vector whitenedError(const Values& c) const { + return RInvSqrt(c.at(key()))*unwhitenedError(c); } /** @@ -329,11 +324,11 @@ public: * Ax-b \approx h(x1+dx1)-z = h(x1) + A1*dx1 - z * Hence b = z - h(x1) = - error_vector(x) */ - boost::shared_ptr linearize(const TestValues& c, const Ordering& ordering) const { - const X& x1 = c[key_]; + boost::shared_ptr linearize(const Values& c, const Ordering& ordering) const { + const X& x1 = c.at(key()); Matrix A1; Vector b = -evaluateError(x1, A1); - const Index var1 = ordering[key_]; + const Index var1 = ordering[key()]; SharedDiagonal constrained = boost::shared_dynamic_cast(this->noiseModel_); if (constrained.get() != NULL) { @@ -347,7 +342,7 @@ public: } /** vector of errors */ - Vector evaluateError(const TestKey::Value& p, boost::optional H1 = boost::none) const { + Vector evaluateError(const T& p, boost::optional H1 = boost::none) const { // error = z - h(p) // H = d error / d p = -d h/ d p = -H Vector z_hat = h(p); @@ -367,8 +362,8 @@ public: TEST( ExtendedKalmanFilter, nonlinear ) { // Create the set of expected output TestValues (generated using Matlab Kalman Filter) - Point2 expected_predict[10]; - Point2 expected_update[10]; + Point2 expected_predict[11]; + Point2 expected_update[11]; expected_predict[0] = Point2(0.81,0.99); expected_update[0] = Point2(0.824926197027,0.29509808); expected_predict[1] = Point2(0.680503230541,0.24343413); @@ -408,17 +403,17 @@ TEST( ExtendedKalmanFilter, nonlinear ) { SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1)); // Create an ExtendedKalmanFilter object - ExtendedKalmanFilter ekf(x_initial, P_initial); + ExtendedKalmanFilter ekf(x_initial, P_initial); // Enter Predict-Update Loop Point2 x_predict, x_update; for(unsigned int i = 0; i < 10; ++i){ // Create motion factor - NonlinearMotionModel motionFactor(TestKey(i-1), TestKey(i)); + NonlinearMotionModel motionFactor(Symbol('x',i-1), Symbol('x',i)); x_predict = ekf.predict(motionFactor); // Create a measurement factor - NonlinearMeasurementModel measurementFactor(TestKey(i), Vector_(1, z[i])); + NonlinearMeasurementModel measurementFactor(Symbol('x',i), Vector_(1, z[i])); x_update = ekf.update(measurementFactor); EXPECT(assert_equal(expected_predict[i],x_predict, 1e-6)); diff --git a/tests/testGaussianBayesNet.cpp b/tests/testGaussianBayesNet.cpp index 7f746397c..f0cec7530 100644 --- a/tests/testGaussianBayesNet.cpp +++ b/tests/testGaussianBayesNet.cpp @@ -25,9 +25,6 @@ #include // for operator += using namespace boost::assign; -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include #include #include diff --git a/tests/testGaussianFactor.cpp b/tests/testGaussianFactor.cpp index 1260c51a2..2cfb7a076 100644 --- a/tests/testGaussianFactor.cpp +++ b/tests/testGaussianFactor.cpp @@ -26,9 +26,6 @@ using namespace boost::assign; #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include #include #include @@ -44,19 +41,21 @@ static SharedDiagonal sigma0_1 = sharedSigma(2,0.1), sigma_02 = sharedSigma(2,0.2), constraintModel = noiseModel::Constrained::All(2); +const Key kx1 = Symbol('x',1), kx2 = Symbol('x',2), kl1 = Symbol('l',1); + /* ************************************************************************* */ TEST( GaussianFactor, linearFactor ) { - Ordering ordering; ordering += "x1","x2","l1"; + Ordering ordering; ordering += kx1,kx2,kl1; Matrix I = eye(2); Vector b = Vector_(2, 2.0, -1.0); - JacobianFactor expected(ordering["x1"], -10*I,ordering["x2"], 10*I, b, noiseModel::Unit::Create(2)); + JacobianFactor expected(ordering[kx1], -10*I,ordering[kx2], 10*I, b, noiseModel::Unit::Create(2)); // create a small linear factor graph FactorGraph fg = createGaussianFactorGraph(ordering); - // get the factor "f2" from the factor graph + // get the factor kf2 from the factor graph JacobianFactor::shared_ptr lf = fg[1]; // check if the two factors are the same @@ -66,26 +65,26 @@ TEST( GaussianFactor, linearFactor ) ///* ************************************************************************* */ // SL-FIX TEST( GaussianFactor, keys ) //{ -// // get the factor "f2" from the small linear factor graph -// Ordering ordering; ordering += "x1","x2","l1"; +// // get the factor kf2 from the small linear factor graph +// Ordering ordering; ordering += kx1,kx2,kl1; // GaussianFactorGraph fg = createGaussianFactorGraph(ordering); // GaussianFactor::shared_ptr lf = fg[1]; // list expected; -// expected.push_back("x1"); -// expected.push_back("x2"); +// expected.push_back(kx1); +// expected.push_back(kx2); // EXPECT(lf->keys() == expected); //} ///* ************************************************************************* */ // SL-FIX TEST( GaussianFactor, dimensions ) //{ -// // get the factor "f2" from the small linear factor graph -// Ordering ordering; ordering += "x1","x2","l1"; +// // get the factor kf2 from the small linear factor graph +// Ordering ordering; ordering += kx1,kx2,kl1; // GaussianFactorGraph fg = createGaussianFactorGraph(ordering); // // // Check a single factor // Dimensions expected; -// insert(expected)("x1", 2)("x2", 2); +// insert(expected)(kx1, 2)(kx2, 2); // Dimensions actual = fg[1]->dimensions(); // EXPECT(expected==actual); //} @@ -94,12 +93,12 @@ TEST( GaussianFactor, linearFactor ) TEST( GaussianFactor, getDim ) { // get a factor - Ordering ordering; ordering += "x1","x2","l1"; + Ordering ordering; ordering += kx1,kx2,kl1; GaussianFactorGraph fg = createGaussianFactorGraph(ordering); GaussianFactor::shared_ptr factor = fg[0]; // get the size of a variable - size_t actual = factor->getDim(factor->find(ordering["x1"])); + size_t actual = factor->getDim(factor->find(ordering[kx1])); // verify size_t expected = 2; @@ -110,7 +109,7 @@ TEST( GaussianFactor, getDim ) // SL-FIX TEST( GaussianFactor, combine ) //{ // // create a small linear factor graph -// Ordering ordering; ordering += "x1","x2","l1"; +// Ordering ordering; ordering += kx1,kx2,kl1; // GaussianFactorGraph fg = createGaussianFactorGraph(ordering); // // // get two factors from it and insert the factors into a vector @@ -155,9 +154,9 @@ TEST( GaussianFactor, getDim ) // // // use general constructor for making arbitrary factors // vector > meas; -// meas.push_back(make_pair("x2", Ax2)); -// meas.push_back(make_pair("l1", Al1)); -// meas.push_back(make_pair("x1", Ax1)); +// meas.push_back(make_pair(kx2, Ax2)); +// meas.push_back(make_pair(kl1, Al1)); +// meas.push_back(make_pair(kx1, Ax1)); // GaussianFactor expected(meas, b2, noiseModel::Diagonal::Sigmas(ones(4))); // EXPECT(assert_equal(expected,combined)); //} @@ -166,7 +165,7 @@ TEST( GaussianFactor, getDim ) TEST( GaussianFactor, error ) { // create a small linear factor graph - Ordering ordering; ordering += "x1","x2","l1"; + Ordering ordering; ordering += kx1,kx2,kl1; GaussianFactorGraph fg = createGaussianFactorGraph(ordering); // get the first factor from the factor graph @@ -175,7 +174,7 @@ TEST( GaussianFactor, error ) // check the error of the first factor with noisy config VectorValues cfg = createZeroDelta(ordering); - // calculate the error from the factor "f1" + // calculate the error from the factor kf1 // note the error is the same as in testNonlinearFactor double actual = lf->error(cfg); DOUBLES_EQUAL( 1.0, actual, 0.00000001 ); @@ -185,7 +184,7 @@ TEST( GaussianFactor, error ) // SL-FIX TEST( GaussianFactor, eliminate ) //{ // // create a small linear factor graph -// Ordering ordering; ordering += "x1","x2","l1"; +// Ordering ordering; ordering += kx1,kx2,kl1; // GaussianFactorGraph fg = createGaussianFactorGraph(ordering); // // // get two factors from it and insert the factors into a vector @@ -199,7 +198,7 @@ TEST( GaussianFactor, error ) // // eliminate the combined factor // GaussianConditional::shared_ptr actualCG; // GaussianFactor::shared_ptr actualLF; -// boost::tie(actualCG,actualLF) = combined.eliminate("x2"); +// boost::tie(actualCG,actualLF) = combined.eliminate(kx2); // // // create expected Conditional Gaussian // Matrix I = eye(2)*sqrt(125.0); @@ -208,14 +207,14 @@ TEST( GaussianFactor, error ) // // // Check the conditional Gaussian // GaussianConditional -// expectedCG("x2", d, R11, "l1", S12, "x1", S13, repeat(2, 1.0)); +// expectedCG(kx2, d, R11, kl1, S12, kx1, S13, repeat(2, 1.0)); // // // the expected linear factor // I = eye(2)/0.2236; // Matrix Bl1 = I, Bx1 = -I; // Vector b1 = I*Vector_(2,0.0,0.2); // -// GaussianFactor expectedLF("l1", Bl1, "x1", Bx1, b1, repeat(2,1.0)); +// GaussianFactor expectedLF(kl1, Bl1, kx1, Bx1, b1, repeat(2,1.0)); // // // check if the result matches // EXPECT(assert_equal(expectedCG,*actualCG,1e-3)); @@ -226,17 +225,17 @@ TEST( GaussianFactor, error ) TEST( GaussianFactor, matrix ) { // create a small linear factor graph - Ordering ordering; ordering += "x1","x2","l1"; + Ordering ordering; ordering += kx1,kx2,kl1; FactorGraph fg = createGaussianFactorGraph(ordering); - // get the factor "f2" from the factor graph + // get the factor kf2 from the factor graph //GaussianFactor::shared_ptr lf = fg[1]; // NOTE: using the older version Vector b2 = Vector_(2, 0.2, -0.1); Matrix I = eye(2); // render with a given ordering Ordering ord; - ord += "x1","x2"; - JacobianFactor::shared_ptr lf(new JacobianFactor(ord["x1"], -I, ord["x2"], I, b2, sigma0_1)); + ord += kx1,kx2; + JacobianFactor::shared_ptr lf(new JacobianFactor(ord[kx1], -I, ord[kx2], I, b2, sigma0_1)); // Test whitened version Matrix A_act1; Vector b_act1; @@ -274,17 +273,17 @@ TEST( GaussianFactor, matrix ) TEST( GaussianFactor, matrix_aug ) { // create a small linear factor graph - Ordering ordering; ordering += "x1","x2","l1"; + Ordering ordering; ordering += kx1,kx2,kl1; FactorGraph fg = createGaussianFactorGraph(ordering); - // get the factor "f2" from the factor graph + // get the factor kf2 from the factor graph //GaussianFactor::shared_ptr lf = fg[1]; Vector b2 = Vector_(2, 0.2, -0.1); Matrix I = eye(2); // render with a given ordering Ordering ord; - ord += "x1","x2"; - JacobianFactor::shared_ptr lf(new JacobianFactor(ord["x1"], -I, ord["x2"], I, b2, sigma0_1)); + ord += kx1,kx2; + JacobianFactor::shared_ptr lf(new JacobianFactor(ord[kx1], -I, ord[kx2], I, b2, sigma0_1)); // Test unwhitened version @@ -325,15 +324,15 @@ void print(const list& i) { // SL-FIX TEST( GaussianFactor, sparse ) //{ // // create a small linear factor graph -// Ordering ordering; ordering += "x1","x2","l1"; +// Ordering ordering; ordering += kx1,kx2,kl1; // GaussianFactorGraph fg = createGaussianFactorGraph(ordering); // -// // get the factor "f2" from the factor graph +// // get the factor kf2 from the factor graph // GaussianFactor::shared_ptr lf = fg[1]; // // // render with a given ordering // Ordering ord; -// ord += "x1","x2"; +// ord += kx1,kx2; // // list i,j; // list s; @@ -355,15 +354,15 @@ void print(const list& i) { // SL-FIX TEST( GaussianFactor, sparse2 ) //{ // // create a small linear factor graph -// Ordering ordering; ordering += "x1","x2","l1"; +// Ordering ordering; ordering += kx1,kx2,kl1; // GaussianFactorGraph fg = createGaussianFactorGraph(ordering); // -// // get the factor "f2" from the factor graph +// // get the factor kf2 from the factor graph // GaussianFactor::shared_ptr lf = fg[1]; // // // render with a given ordering // Ordering ord; -// ord += "x2","l1","x1"; +// ord += kx2,kl1,kx1; // // list i,j; // list s; @@ -385,7 +384,7 @@ void print(const list& i) { TEST( GaussianFactor, size ) { // create a linear factor graph - Ordering ordering; ordering += "x1","x2","l1"; + Ordering ordering; ordering += kx1,kx2,kl1; GaussianFactorGraph fg = createGaussianFactorGraph(ordering); // get some factors from the graph diff --git a/tests/testGaussianFactorGraph.cpp b/tests/testGaussianFactorGraph.cpp index d95a05830..671d09cc7 100644 --- a/tests/testGaussianFactorGraph.cpp +++ b/tests/testGaussianFactorGraph.cpp @@ -28,9 +28,6 @@ using namespace boost::assign; #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include #include #include @@ -44,10 +41,13 @@ using namespace example; double tol=1e-5; +Key kx(size_t i) { return Symbol('x',i); } +Key kl(size_t i) { return Symbol('l',i); } + /* ************************************************************************* */ TEST( GaussianFactorGraph, equals ) { - Ordering ordering; ordering += "x1","x2","l1"; + Ordering ordering; ordering += kx(1),kx(2),kl(1); GaussianFactorGraph fg = createGaussianFactorGraph(ordering); GaussianFactorGraph fg2 = createGaussianFactorGraph(ordering); EXPECT(fg.equals(fg2)); @@ -55,7 +55,7 @@ TEST( GaussianFactorGraph, equals ) { /* ************************************************************************* */ //TEST( GaussianFactorGraph, error ) { -// Ordering ordering; ordering += "x1","x2","l1"; +// Ordering ordering; ordering += kx(1),kx(2),kl(1); // FactorGraph fg = createGaussianFactorGraph(ordering); // VectorValues cfg = createZeroDelta(ordering); // @@ -73,10 +73,10 @@ TEST( GaussianFactorGraph, equals ) { //{ // GaussianFactorGraph fg = createGaussianFactorGraph(); // -// set separator = fg.find_separator("x2"); +// set separator = fg.find_separator(kx(2)); // set expected; -// expected.insert("x1"); -// expected.insert("l1"); +// expected.insert(kx(1)); +// expected.insert(kl(1)); // // EXPECT(separator.size()==expected.size()); // set::iterator it1 = separator.begin(), it2 = expected.begin(); @@ -91,7 +91,7 @@ TEST( GaussianFactorGraph, equals ) { // GaussianFactorGraph fg = createGaussianFactorGraph(); // // // combine all factors -// GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,"x1"); +// GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,kx(1)); // // // the expected linear factor // Matrix Al1 = Matrix_(6,2, @@ -131,11 +131,11 @@ TEST( GaussianFactorGraph, equals ) { // b(5) = 1; // // vector > meas; -// meas.push_back(make_pair("l1", Al1)); -// meas.push_back(make_pair("x1", Ax1)); -// meas.push_back(make_pair("x2", Ax2)); +// meas.push_back(make_pair(kl(1), Al1)); +// meas.push_back(make_pair(kx(1), Ax1)); +// meas.push_back(make_pair(kx(2), Ax2)); // GaussianFactor expected(meas, b, ones(6)); -// //GaussianFactor expected("l1", Al1, "x1", Ax1, "x2", Ax2, b); +// //GaussianFactor expected(kl(1), Al1, kx(1), Ax1, kx(2), Ax2, b); // // // check if the two factors are the same // EXPECT(assert_equal(expected,*actual)); @@ -148,7 +148,7 @@ TEST( GaussianFactorGraph, equals ) { // GaussianFactorGraph fg = createGaussianFactorGraph(); // // // combine all factors -// GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,"x2"); +// GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,kx(2)); // // // the expected linear factor // Matrix Al1 = Matrix_(4,2, @@ -183,9 +183,9 @@ TEST( GaussianFactorGraph, equals ) { // b(3) = 1.5; // // vector > meas; -// meas.push_back(make_pair("l1", Al1)); -// meas.push_back(make_pair("x1", Ax1)); -// meas.push_back(make_pair("x2", Ax2)); +// meas.push_back(make_pair(kl(1), Al1)); +// meas.push_back(make_pair(kx(1), Ax1)); +// meas.push_back(make_pair(kx(2), Ax2)); // GaussianFactor expected(meas, b, ones(4)); // // // check if the two factors are the same @@ -195,14 +195,14 @@ TEST( GaussianFactorGraph, equals ) { ///* ************************************************************************* */ //TEST( GaussianFactorGraph, eliminateOne_x1 ) //{ -// Ordering ordering; ordering += "x1","l1","x2"; +// Ordering ordering; ordering += kx(1),kl(1),kx(2); // GaussianFactorGraph fg = createGaussianFactorGraph(ordering); // GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1); // // // create expected Conditional Gaussian // Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; // Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2); -// GaussianConditional expected(ordering["x1"],15*d,R11,ordering["l1"],S12,ordering["x2"],S13,sigma); +// GaussianConditional expected(ordering[kx(1)],15*d,R11,ordering[kl(1)],S12,ordering[kx(2)],S13,sigma); // // EXPECT(assert_equal(expected,*actual,tol)); //} @@ -210,7 +210,7 @@ TEST( GaussianFactorGraph, equals ) { ///* ************************************************************************* */ //TEST( GaussianFactorGraph, eliminateOne_x2 ) //{ -// Ordering ordering; ordering += "x2","l1","x1"; +// Ordering ordering; ordering += kx(2),kl(1),kx(1); // GaussianFactorGraph fg = createGaussianFactorGraph(ordering); // GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1); // @@ -218,7 +218,7 @@ TEST( GaussianFactorGraph, equals ) { // double sig = 0.0894427; // Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; // Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2); -// GaussianConditional expected(ordering["x2"],d,R11,ordering["l1"],S12,ordering["x1"],S13,sigma); +// GaussianConditional expected(ordering[kx(2)],d,R11,ordering[kl(1)],S12,ordering[kx(1)],S13,sigma); // // EXPECT(assert_equal(expected,*actual,tol)); //} @@ -226,7 +226,7 @@ TEST( GaussianFactorGraph, equals ) { ///* ************************************************************************* */ //TEST( GaussianFactorGraph, eliminateOne_l1 ) //{ -// Ordering ordering; ordering += "l1","x1","x2"; +// Ordering ordering; ordering += kl(1),kx(1),kx(2); // GaussianFactorGraph fg = createGaussianFactorGraph(ordering); // GaussianConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, 1); // @@ -234,7 +234,7 @@ TEST( GaussianFactorGraph, equals ) { // double sig = sqrt(2)/10.; // Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; // Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); -// GaussianConditional expected(ordering["l1"],d,R11,ordering["x1"],S12,ordering["x2"],S13,sigma); +// GaussianConditional expected(ordering[kl(1)],d,R11,ordering[kx(1)],S12,ordering[kx(2)],S13,sigma); // // EXPECT(assert_equal(expected,*actual,tol)); //} @@ -243,12 +243,12 @@ TEST( GaussianFactorGraph, equals ) { //TEST( GaussianFactorGraph, eliminateOne_x1_fast ) //{ // GaussianFactorGraph fg = createGaussianFactorGraph(); -// GaussianConditional::shared_ptr actual = fg.eliminateOne("x1", false); +// GaussianConditional::shared_ptr actual = fg.eliminateOne(kx(1), false); // // // create expected Conditional Gaussian // Matrix I = 15*eye(2), R11 = I, S12 = -0.111111*I, S13 = -0.444444*I; // Vector d = Vector_(2, -0.133333, -0.0222222), sigma = ones(2); -// GaussianConditional expected("x1",15*d,R11,"l1",S12,"x2",S13,sigma); +// GaussianConditional expected(kx(1),15*d,R11,kl(1),S12,kx(2),S13,sigma); // // EXPECT(assert_equal(expected,*actual,tol)); //} @@ -257,13 +257,13 @@ TEST( GaussianFactorGraph, equals ) { //TEST( GaussianFactorGraph, eliminateOne_x2_fast ) //{ // GaussianFactorGraph fg = createGaussianFactorGraph(); -// GaussianConditional::shared_ptr actual = fg.eliminateOne("x2", false); +// GaussianConditional::shared_ptr actual = fg.eliminateOne(kx(2), false); // // // create expected Conditional Gaussian // double sig = 0.0894427; // Matrix I = eye(2)/sig, R11 = I, S12 = -0.2*I, S13 = -0.8*I; // Vector d = Vector_(2, 0.2, -0.14)/sig, sigma = ones(2); -// GaussianConditional expected("x2",d,R11,"l1",S12,"x1",S13,sigma); +// GaussianConditional expected(kx(2),d,R11,kl(1),S12,kx(1),S13,sigma); // // EXPECT(assert_equal(expected,*actual,tol)); //} @@ -272,13 +272,13 @@ TEST( GaussianFactorGraph, equals ) { //TEST( GaussianFactorGraph, eliminateOne_l1_fast ) //{ // GaussianFactorGraph fg = createGaussianFactorGraph(); -// GaussianConditional::shared_ptr actual = fg.eliminateOne("l1", false); +// GaussianConditional::shared_ptr actual = fg.eliminateOne(kl(1), false); // // // create expected Conditional Gaussian // double sig = sqrt(2)/10.; // Matrix I = eye(2)/sig, R11 = I, S12 = -0.5*I, S13 = -0.5*I; // Vector d = Vector_(2, -0.1, 0.25)/sig, sigma = ones(2); -// GaussianConditional expected("l1",d,R11,"x1",S12,"x2",S13,sigma); +// GaussianConditional expected(kl(1),d,R11,kx(1),S12,kx(2),S13,sigma); // // EXPECT(assert_equal(expected,*actual,tol)); //} @@ -290,18 +290,18 @@ TEST( GaussianFactorGraph, eliminateAll ) Matrix I = eye(2); Ordering ordering; - ordering += "x2","l1","x1"; + ordering += kx(2),kl(1),kx(1); Vector d1 = Vector_(2, -0.1,-0.1); - GaussianBayesNet expected = simpleGaussian(ordering["x1"],d1,0.1); + GaussianBayesNet expected = simpleGaussian(ordering[kx(1)],d1,0.1); double sig1 = 0.149071; Vector d2 = Vector_(2, 0.0, 0.2)/sig1, sigma2 = ones(2); - push_front(expected,ordering["l1"],d2, I/sig1,ordering["x1"], (-1)*I/sig1,sigma2); + push_front(expected,ordering[kl(1)],d2, I/sig1,ordering[kx(1)], (-1)*I/sig1,sigma2); double sig2 = 0.0894427; Vector d3 = Vector_(2, 0.2, -0.14)/sig2, sigma3 = ones(2); - push_front(expected,ordering["x2"],d3, I/sig2,ordering["l1"], (-0.2)*I/sig2, ordering["x1"], (-0.8)*I/sig2, sigma3); + push_front(expected,ordering[kx(2)],d3, I/sig2,ordering[kl(1)], (-0.2)*I/sig2, ordering[kx(1)], (-0.8)*I/sig2, sigma3); // Check one ordering GaussianFactorGraph fg1 = createGaussianFactorGraph(ordering); @@ -319,20 +319,20 @@ TEST( GaussianFactorGraph, eliminateAll ) // Matrix I = eye(2); // // Vector d1 = Vector_(2, -0.1,-0.1); -// GaussianBayesNet expected = simpleGaussian("x1",d1,0.1); +// GaussianBayesNet expected = simpleGaussian(kx(1),d1,0.1); // // double sig1 = 0.149071; // Vector d2 = Vector_(2, 0.0, 0.2)/sig1, sigma2 = ones(2); -// push_front(expected,"l1",d2, I/sig1,"x1", (-1)*I/sig1,sigma2); +// push_front(expected,kl(1),d2, I/sig1,kx(1), (-1)*I/sig1,sigma2); // // double sig2 = 0.0894427; // Vector d3 = Vector_(2, 0.2, -0.14)/sig2, sigma3 = ones(2); -// push_front(expected,"x2",d3, I/sig2,"l1", (-0.2)*I/sig2, "x1", (-0.8)*I/sig2, sigma3); +// push_front(expected,kx(2),d3, I/sig2,kl(1), (-0.2)*I/sig2, kx(1), (-0.8)*I/sig2, sigma3); // // // Check one ordering // GaussianFactorGraph fg1 = createGaussianFactorGraph(); // Ordering ordering; -// ordering += "x2","l1","x1"; +// ordering += kx(2),kl(1),kx(1); // GaussianBayesNet actual = fg1.eliminate(ordering, false); // EXPECT(assert_equal(expected,actual,tol)); //} @@ -340,16 +340,16 @@ TEST( GaussianFactorGraph, eliminateAll ) ///* ************************************************************************* */ //TEST( GaussianFactorGraph, add_priors ) //{ -// Ordering ordering; ordering += "l1","x1","x2"; +// Ordering ordering; ordering += kl(1),kx(1),kx(2); // GaussianFactorGraph fg = createGaussianFactorGraph(ordering); // GaussianFactorGraph actual = fg.add_priors(3, vector(3,2)); // GaussianFactorGraph expected = createGaussianFactorGraph(ordering); // Matrix A = eye(2); // Vector b = zero(2); // SharedDiagonal sigma = sharedSigma(2,3.0); -// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering["l1"],A,b,sigma))); -// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering["x1"],A,b,sigma))); -// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering["x2"],A,b,sigma))); +// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[kl(1)],A,b,sigma))); +// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[kx(1)],A,b,sigma))); +// expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[kx(2)],A,b,sigma))); // EXPECT(assert_equal(expected,actual)); //} @@ -357,7 +357,7 @@ TEST( GaussianFactorGraph, eliminateAll ) TEST( GaussianFactorGraph, copying ) { // Create a graph - Ordering ordering; ordering += "x2","l1","x1"; + Ordering ordering; ordering += kx(2),kl(1),kx(1); GaussianFactorGraph actual = createGaussianFactorGraph(ordering); // Copy the graph ! @@ -378,7 +378,7 @@ TEST( GaussianFactorGraph, copying ) //{ // // render with a given ordering // Ordering ord; -// ord += "x2","l1","x1"; +// ord += kx(2),kl(1),kx(1); // // // Create a graph // GaussianFactorGraph fg = createGaussianFactorGraph(ordering); @@ -417,7 +417,7 @@ TEST( GaussianFactorGraph, copying ) TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet ) { Ordering ord; - ord += "x2","l1","x1"; + ord += kx(2),kl(1),kx(1); GaussianFactorGraph fg = createGaussianFactorGraph(ord); // render with a given ordering @@ -437,20 +437,20 @@ TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet ) /* ************************************************************************* */ TEST( GaussianFactorGraph, getOrdering) { - Ordering original; original += "l1","x1","x2"; + Ordering original; original += kl(1),kx(1),kx(2); FactorGraph symbolic(createGaussianFactorGraph(original)); Permutation perm(*Inference::PermutationCOLAMD(VariableIndex(symbolic))); Ordering actual = original; actual.permuteWithInverse((*perm.inverse())); - Ordering expected; expected += "l1","x2","x1"; + Ordering expected; expected += kl(1),kx(2),kx(1); EXPECT(assert_equal(expected,actual)); } // SL-FIX TEST( GaussianFactorGraph, getOrdering2) //{ // Ordering expected; -// expected += "l1","x1"; +// expected += kl(1),kx(1); // GaussianFactorGraph fg = createGaussianFactorGraph(); -// set interested; interested += "l1","x1"; +// set interested; interested += kl(1),kx(1); // Ordering actual = fg.getOrdering(interested); // EXPECT(assert_equal(expected,actual)); //} @@ -459,7 +459,7 @@ TEST( GaussianFactorGraph, getOrdering) TEST( GaussianFactorGraph, optimize_LDL ) { // create an ordering - Ordering ord; ord += "x2","l1","x1"; + Ordering ord; ord += kx(2),kl(1),kx(1); // create a graph GaussianFactorGraph fg = createGaussianFactorGraph(ord); @@ -477,7 +477,7 @@ TEST( GaussianFactorGraph, optimize_LDL ) TEST( GaussianFactorGraph, optimize_QR ) { // create an ordering - Ordering ord; ord += "x2","l1","x1"; + Ordering ord; ord += kx(2),kl(1),kx(1); // create a graph GaussianFactorGraph fg = createGaussianFactorGraph(ord); @@ -495,7 +495,7 @@ TEST( GaussianFactorGraph, optimize_QR ) // SL-FIX TEST( GaussianFactorGraph, optimizeMultiFrontlas ) //{ // // create an ordering -// Ordering ord; ord += "x2","l1","x1"; +// Ordering ord; ord += kx(2),kl(1),kx(1); // // // create a graph // GaussianFactorGraph fg = createGaussianFactorGraph(ord); @@ -513,7 +513,7 @@ TEST( GaussianFactorGraph, optimize_QR ) TEST( GaussianFactorGraph, combine) { // create an ordering - Ordering ord; ord += "x2","l1","x1"; + Ordering ord; ord += kx(2),kl(1),kx(1); // create a test graph GaussianFactorGraph fg1 = createGaussianFactorGraph(ord); @@ -535,7 +535,7 @@ TEST( GaussianFactorGraph, combine) TEST( GaussianFactorGraph, combine2) { // create an ordering - Ordering ord; ord += "x2","l1","x1"; + Ordering ord; ord += kx(2),kl(1),kx(1); // create a test graph GaussianFactorGraph fg1 = createGaussianFactorGraph(ord); @@ -568,13 +568,13 @@ void print(vector v) { // GaussianFactorGraph fg = createGaussianFactorGraph(); // // // ask for all factor indices connected to x1 -// list x1_factors = fg.factors("x1"); +// list x1_factors = fg.factors(kx(1)); // size_t x1_indices[] = { 0, 1, 2 }; // list x1_expected(x1_indices, x1_indices + 3); // EXPECT(x1_factors==x1_expected); // // // ask for all factor indices connected to x2 -// list x2_factors = fg.factors("x2"); +// list x2_factors = fg.factors(kx(2)); // size_t x2_indices[] = { 1, 3 }; // list x2_expected(x2_indices, x2_indices + 2); // EXPECT(x2_factors==x2_expected); @@ -592,7 +592,7 @@ void print(vector v) { // GaussianFactor::shared_ptr f2 = fg[2]; // // // call the function -// vector factors = fg.findAndRemoveFactors("x1"); +// vector factors = fg.findAndRemoveFactors(kx(1)); // // // Check the factors // EXPECT(f0==factors[0]); @@ -617,7 +617,7 @@ TEST(GaussianFactorGraph, createSmoother) //{ // GaussianFactorGraph fg = createGaussianFactorGraph(); // Dimensions expected; -// insert(expected)("l1", 2)("x1", 2)("x2", 2); +// insert(expected)(kl(1), 2)(kx(1), 2)(kx(2), 2); // Dimensions actual = fg.dimensions(); // EXPECT(expected==actual); //} @@ -627,7 +627,7 @@ TEST(GaussianFactorGraph, createSmoother) //{ // GaussianFactorGraph fg = createGaussianFactorGraph(); // Ordering expected; -// expected += "l1","x1","x2"; +// expected += kl(1),kx(1),kx(2); // EXPECT(assert_equal(expected,fg.keys())); //} @@ -635,16 +635,16 @@ TEST(GaussianFactorGraph, createSmoother) // SL-NEEDED? TEST( GaussianFactorGraph, involves ) //{ // GaussianFactorGraph fg = createGaussianFactorGraph(); -// EXPECT(fg.involves("l1")); -// EXPECT(fg.involves("x1")); -// EXPECT(fg.involves("x2")); -// EXPECT(!fg.involves("x3")); +// EXPECT(fg.involves(kl(1))); +// EXPECT(fg.involves(kx(1))); +// EXPECT(fg.involves(kx(2))); +// EXPECT(!fg.involves(kx(3))); //} /* ************************************************************************* */ double error(const VectorValues& x) { // create an ordering - Ordering ord; ord += "x2","l1","x1"; + Ordering ord; ord += kx(2),kl(1),kx(1); GaussianFactorGraph fg = createGaussianFactorGraph(ord); return fg.error(x); @@ -658,11 +658,11 @@ double error(const VectorValues& x) { // // Construct expected gradient // VectorValues expected; // -// // 2*f(x) = 100*(x1+c["x1"])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2 +// // 2*f(x) = 100*(x1+c[kx(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2 // // worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5] -// expected.insert("l1",Vector_(2, 5.0,-12.5)); -// expected.insert("x1",Vector_(2, 30.0, 5.0)); -// expected.insert("x2",Vector_(2,-25.0, 17.5)); +// expected.insert(kl(1),Vector_(2, 5.0,-12.5)); +// expected.insert(kx(1),Vector_(2, 30.0, 5.0)); +// expected.insert(kx(2),Vector_(2,-25.0, 17.5)); // // // Check the gradient at delta=0 // VectorValues zero = createZeroDelta(); @@ -675,7 +675,7 @@ double error(const VectorValues& x) { // // // Check the gradient at the solution (should be zero) // Ordering ord; -// ord += "x2","l1","x1"; +// ord += kx(2),kl(1),kx(1); // GaussianFactorGraph fg2 = createGaussianFactorGraph(); // VectorValues solution = fg2.optimize(ord); // destructive // VectorValues actual2 = fg.gradient(solution); @@ -686,7 +686,7 @@ double error(const VectorValues& x) { TEST( GaussianFactorGraph, multiplication ) { // create an ordering - Ordering ord; ord += "x2","l1","x1"; + Ordering ord; ord += kx(2),kl(1),kx(1); FactorGraph A = createGaussianFactorGraph(ord); VectorValues x = createCorrectDelta(ord); @@ -703,7 +703,7 @@ TEST( GaussianFactorGraph, multiplication ) // SL-NEEDED? TEST( GaussianFactorGraph, transposeMultiplication ) //{ // // create an ordering -// Ordering ord; ord += "x2","l1","x1"; +// Ordering ord; ord += kx(2),kl(1),kx(1); // // GaussianFactorGraph A = createGaussianFactorGraph(ord); // Errors e; @@ -713,9 +713,9 @@ TEST( GaussianFactorGraph, multiplication ) // e += Vector_(2,-7.5,-5.0); // // VectorValues expected = createZeroDelta(ord), actual = A ^ e; -// expected[ord["l1"]] = Vector_(2, -37.5,-50.0); -// expected[ord["x1"]] = Vector_(2,-150.0, 25.0); -// expected[ord["x2"]] = Vector_(2, 187.5, 25.0); +// expected[ord[kl(1)]] = Vector_(2, -37.5,-50.0); +// expected[ord[kx(1)]] = Vector_(2,-150.0, 25.0); +// expected[ord[kx(2)]] = Vector_(2, 187.5, 25.0); // EXPECT(assert_equal(expected,actual)); //} @@ -723,7 +723,7 @@ TEST( GaussianFactorGraph, multiplication ) // SL-NEEDED? TEST( GaussianFactorGraph, rhs ) //{ // // create an ordering -// Ordering ord; ord += "x2","l1","x1"; +// Ordering ord; ord += kx(2),kl(1),kx(1); // // GaussianFactorGraph Ab = createGaussianFactorGraph(ord); // Errors expected = createZeroDelta(ord), actual = Ab.rhs(); @@ -739,21 +739,21 @@ TEST( GaussianFactorGraph, multiplication ) TEST( GaussianFactorGraph, elimination ) { Ordering ord; - ord += "x1", "x2"; + ord += kx(1), kx(2); // Create Gaussian Factor Graph GaussianFactorGraph fg; Matrix Ap = eye(1), An = eye(1) * -1; Vector b = Vector_(1, 0.0); SharedDiagonal sigma = sharedSigma(1,2.0); - fg.add(ord["x1"], An, ord["x2"], Ap, b, sigma); - fg.add(ord["x1"], Ap, b, sigma); - fg.add(ord["x2"], Ap, b, sigma); + fg.add(ord[kx(1)], An, ord[kx(2)], Ap, b, sigma); + fg.add(ord[kx(1)], Ap, b, sigma); + fg.add(ord[kx(2)], Ap, b, sigma); // Eliminate GaussianBayesNet bayesNet = *GaussianSequentialSolver(fg).eliminate(); // Check sigma - EXPECT_DOUBLES_EQUAL(1.0,bayesNet[ord["x2"]]->get_sigmas()(0),1e-5); + EXPECT_DOUBLES_EQUAL(1.0,bayesNet[ord[kx(2)]]->get_sigmas()(0),1e-5); // Check matrix Matrix R;Vector d; @@ -808,7 +808,7 @@ TEST( GaussianFactorGraph, constrained_single ) // // // eliminate and solve // Ordering ord; -// ord += "y", "x"; +// ord += "yk, x"; // VectorValues actual = fg.optimize(ord); // // // verify @@ -839,7 +839,7 @@ TEST( GaussianFactorGraph, constrained_multi1 ) // // // eliminate and solve // Ordering ord; -// ord += "z", "x", "y"; +// ord += "zk, xk, y"; // VectorValues actual = fg.optimize(ord); // // // verify @@ -856,18 +856,18 @@ SharedDiagonal model = sharedSigma(2,1); // GaussianFactorGraph g; // Matrix I = eye(2); // Vector b = Vector_(0, 0, 0); -// g.add("x1", I, "x2", I, b, model); -// g.add("x1", I, "x3", I, b, model); -// g.add("x1", I, "x4", I, b, model); -// g.add("x2", I, "x3", I, b, model); -// g.add("x2", I, "x4", I, b, model); -// g.add("x3", I, "x4", I, b, model); +// g.add(kx(1), I, kx(2), I, b, model); +// g.add(kx(1), I, kx(3), I, b, model); +// g.add(kx(1), I, kx(4), I, b, model); +// g.add(kx(2), I, kx(3), I, b, model); +// g.add(kx(2), I, kx(4), I, b, model); +// g.add(kx(3), I, kx(4), I, b, model); // // map tree = g.findMinimumSpanningTree(); -// EXPECT(tree["x1"].compare("x1")==0); -// EXPECT(tree["x2"].compare("x1")==0); -// EXPECT(tree["x3"].compare("x1")==0); -// EXPECT(tree["x4"].compare("x1")==0); +// EXPECT(tree[kx(1)].compare(kx(1))==0); +// EXPECT(tree[kx(2)].compare(kx(1))==0); +// EXPECT(tree[kx(3)].compare(kx(1))==0); +// EXPECT(tree[kx(4)].compare(kx(1))==0); //} ///* ************************************************************************* */ @@ -876,17 +876,17 @@ SharedDiagonal model = sharedSigma(2,1); // GaussianFactorGraph g; // Matrix I = eye(2); // Vector b = Vector_(0, 0, 0); -// g.add("x1", I, "x2", I, b, model); -// g.add("x1", I, "x3", I, b, model); -// g.add("x1", I, "x4", I, b, model); -// g.add("x2", I, "x3", I, b, model); -// g.add("x2", I, "x4", I, b, model); +// g.add(kx(1), I, kx(2), I, b, model); +// g.add(kx(1), I, kx(3), I, b, model); +// g.add(kx(1), I, kx(4), I, b, model); +// g.add(kx(2), I, kx(3), I, b, model); +// g.add(kx(2), I, kx(4), I, b, model); // // PredecessorMap tree; -// tree["x1"] = "x1"; -// tree["x2"] = "x1"; -// tree["x3"] = "x1"; -// tree["x4"] = "x1"; +// tree[kx(1)] = kx(1); +// tree[kx(2)] = kx(1); +// tree[kx(3)] = kx(1); +// tree[kx(4)] = kx(1); // // GaussianFactorGraph Ab1, Ab2; // g.split(tree, Ab1, Ab2); @@ -897,17 +897,17 @@ SharedDiagonal model = sharedSigma(2,1); /* ************************************************************************* */ TEST(GaussianFactorGraph, replace) { - Ordering ord; ord += "x1","x2","x3","x4","x5","x6"; + Ordering ord; ord += kx(1),kx(2),kx(3),kx(4),kx(5),kx(6); SharedDiagonal noise(sharedSigma(3, 1.0)); GaussianFactorGraph::sharedFactor f1(new JacobianFactor( - ord["x1"], eye(3,3), ord["x2"], eye(3,3), zero(3), noise)); + ord[kx(1)], eye(3,3), ord[kx(2)], eye(3,3), zero(3), noise)); GaussianFactorGraph::sharedFactor f2(new JacobianFactor( - ord["x2"], eye(3,3), ord["x3"], eye(3,3), zero(3), noise)); + ord[kx(2)], eye(3,3), ord[kx(3)], eye(3,3), zero(3), noise)); GaussianFactorGraph::sharedFactor f3(new JacobianFactor( - ord["x3"], eye(3,3), ord["x4"], eye(3,3), zero(3), noise)); + ord[kx(3)], eye(3,3), ord[kx(4)], eye(3,3), zero(3), noise)); GaussianFactorGraph::sharedFactor f4(new JacobianFactor( - ord["x5"], eye(3,3), ord["x6"], eye(3,3), zero(3), noise)); + ord[kx(5)], eye(3,3), ord[kx(6)], eye(3,3), zero(3), noise)); GaussianFactorGraph actual; actual.push_back(f1); @@ -943,7 +943,7 @@ TEST(GaussianFactorGraph, replace) // // // combine in a factor // Matrix Ab; SharedDiagonal noise; -// Ordering order; order += "x2", "l1", "x1"; +// Ordering order; order += kx(2), kl(1), kx(1); // boost::tie(Ab, noise) = combineFactorsAndCreateMatrix(lfg, order, dimensions); // // // the expected augmented matrix @@ -971,26 +971,26 @@ TEST(GaussianFactorGraph, replace) // typedef GaussianFactorGraph::sharedFactor Factor; // SharedDiagonal model(Vector_(1, 0.5)); // GaussianFactorGraph fg; -// Factor factor1(new JacobianFactor("x1", Matrix_(1,1,1.), "x2", Matrix_(1,1,1.), Vector_(1,1.), model)); -// Factor factor2(new JacobianFactor("x2", Matrix_(1,1,1.), "x3", Matrix_(1,1,1.), Vector_(1,1.), model)); -// Factor factor3(new JacobianFactor("x3", Matrix_(1,1,1.), "x3", Matrix_(1,1,1.), Vector_(1,1.), model)); +// Factor factor1(new JacobianFactor(kx(1), Matrix_(1,1,1.), kx(2), Matrix_(1,1,1.), Vector_(1,1.), model)); +// Factor factor2(new JacobianFactor(kx(2), Matrix_(1,1,1.), kx(3), Matrix_(1,1,1.), Vector_(1,1.), model)); +// Factor factor3(new JacobianFactor(kx(3), Matrix_(1,1,1.), kx(3), Matrix_(1,1,1.), Vector_(1,1.), model)); // fg.push_back(factor1); // fg.push_back(factor2); // fg.push_back(factor3); // -// Ordering frontals; frontals += "x2", "x1"; +// Ordering frontals; frontals += kx(2), kx(1); // GaussianBayesNet bn = fg.eliminateFrontals(frontals); // // GaussianBayesNet bn_expected; -// GaussianBayesNet::sharedConditional conditional1(new GaussianConditional("x2", Vector_(1, 2.), Matrix_(1, 1, 2.), -// "x1", Matrix_(1, 1, 1.), "x3", Matrix_(1, 1, 1.), Vector_(1, 1.))); -// GaussianBayesNet::sharedConditional conditional2(new GaussianConditional("x1", Vector_(1, 0.), Matrix_(1, 1, -1.), -// "x3", Matrix_(1, 1, 1.), Vector_(1, 1.))); +// GaussianBayesNet::sharedConditional conditional1(new GaussianConditional(kx(2), Vector_(1, 2.), Matrix_(1, 1, 2.), +// kx(1), Matrix_(1, 1, 1.), kx(3), Matrix_(1, 1, 1.), Vector_(1, 1.))); +// GaussianBayesNet::sharedConditional conditional2(new GaussianConditional(kx(1), Vector_(1, 0.), Matrix_(1, 1, -1.), +// kx(3), Matrix_(1, 1, 1.), Vector_(1, 1.))); // bn_expected.push_back(conditional1); // bn_expected.push_back(conditional2); // EXPECT(assert_equal(bn_expected, bn)); // -// GaussianFactorGraph::sharedFactor factor_expected(new JacobianFactor("x3", Matrix_(1, 1, 2.), Vector_(1, 2.), SharedDiagonal(Vector_(1, 1.)))); +// GaussianFactorGraph::sharedFactor factor_expected(new JacobianFactor(kx(3), Matrix_(1, 1, 2.), Vector_(1, 2.), SharedDiagonal(Vector_(1, 1.)))); // GaussianFactorGraph fg_expected; // fg_expected.push_back(factor_expected); // EXPECT(assert_equal(fg_expected, fg)); @@ -1006,8 +1006,8 @@ TEST(GaussianFactorGraph, createSmoother2) LONGS_EQUAL(5,fg2.size()); // eliminate - vector x3var; x3var.push_back(ordering["x3"]); - vector x1var; x1var.push_back(ordering["x1"]); + vector x3var; x3var.push_back(ordering[kx(3)]); + vector x1var; x1var.push_back(ordering[kx(1)]); GaussianBayesNet p_x3 = *GaussianSequentialSolver( *GaussianSequentialSolver(fg2).jointFactorGraph(x3var)).eliminate(); GaussianBayesNet p_x1 = *GaussianSequentialSolver( @@ -1024,7 +1024,7 @@ TEST(GaussianFactorGraph, hasConstraints) FactorGraph fgc2 = createSimpleConstraintGraph() ; EXPECT(hasConstraints(fgc2)); - Ordering ordering; ordering += "x1", "x2", "l1"; + Ordering ordering; ordering += kx(1), kx(2), kl(1); FactorGraph fg = createGaussianFactorGraph(ordering); EXPECT(!hasConstraints(fg)); } diff --git a/tests/testGaussianISAM.cpp b/tests/testGaussianISAM.cpp index 3b432393d..b02d68583 100644 --- a/tests/testGaussianISAM.cpp +++ b/tests/testGaussianISAM.cpp @@ -21,9 +21,6 @@ using namespace boost::assign; #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include #include #include @@ -37,6 +34,9 @@ using namespace std; using namespace gtsam; using namespace example; +Key kx(size_t i) { return Symbol('x',i); } +Key kl(size_t i) { return Symbol('l',i); } + /* ************************************************************************* */ // Some numbers that should be consistent among all smoother tests @@ -49,7 +49,7 @@ const double tol = 1e-4; TEST_UNSAFE( ISAM, iSAM_smoother ) { Ordering ordering; - for (int t = 1; t <= 7; t++) ordering += Symbol('x', t); + for (int t = 1; t <= 7; t++) ordering += kx(t); // Create smoother with 7 nodes GaussianFactorGraph smoother = createSmoother(7, ordering).first; @@ -82,7 +82,7 @@ TEST_UNSAFE( ISAM, iSAM_smoother ) // GaussianFactorGraph smoother = createSmoother(7); // // // Create initial tree from first 4 timestamps in reverse order ! -// Ordering ord; ord += "x4","x3","x2","x1"; +// Ordering ord; ord += kx(4),kx(3),kx(2),kx(1); // GaussianFactorGraph factors1; // for (int i=0;i<7;i++) factors1.push_back(smoother[i]); // GaussianISAM actual(*Inference::Eliminate(factors1)); @@ -129,7 +129,7 @@ TEST_UNSAFE( BayesTree, linear_smoother_shortcuts ) EXPECT(assert_equal(empty,actual1,tol)); // Check the conditional P(C2|Root) - GaussianISAM::sharedClique C2 = isamTree[ordering["x5"]]; + GaussianISAM::sharedClique C2 = isamTree[ordering[kx(5)]]; GaussianBayesNet actual2 = GaussianISAM::shortcut(C2,R); EXPECT(assert_equal(empty,actual2,tol)); @@ -137,8 +137,8 @@ TEST_UNSAFE( BayesTree, linear_smoother_shortcuts ) double sigma3 = 0.61808; Matrix A56 = Matrix_(2,2,-0.382022,0.,0.,-0.382022); GaussianBayesNet expected3; - push_front(expected3,ordering["x5"], zero(2), eye(2)/sigma3, ordering["x6"], A56/sigma3, ones(2)); - GaussianISAM::sharedClique C3 = isamTree[ordering["x4"]]; + push_front(expected3,ordering[kx(5)], zero(2), eye(2)/sigma3, ordering[kx(6)], A56/sigma3, ones(2)); + GaussianISAM::sharedClique C3 = isamTree[ordering[kx(4)]]; GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R); EXPECT(assert_equal(expected3,actual3,tol)); @@ -146,8 +146,8 @@ TEST_UNSAFE( BayesTree, linear_smoother_shortcuts ) double sigma4 = 0.661968; Matrix A46 = Matrix_(2,2,-0.146067,0.,0.,-0.146067); GaussianBayesNet expected4; - push_front(expected4, ordering["x4"], zero(2), eye(2)/sigma4, ordering["x6"], A46/sigma4, ones(2)); - GaussianISAM::sharedClique C4 = isamTree[ordering["x3"]]; + push_front(expected4, ordering[kx(4)], zero(2), eye(2)/sigma4, ordering[kx(6)], A46/sigma4, ones(2)); + GaussianISAM::sharedClique C4 = isamTree[ordering[kx(3)]]; GaussianBayesNet actual4 = GaussianISAM::shortcut(C4,R); EXPECT(assert_equal(expected4,actual4,tol)); } @@ -175,7 +175,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_marginals ) { // Create smoother with 7 nodes Ordering ordering; - ordering += "x1","x3","x5","x7","x2","x6","x4"; + ordering += kx(1),kx(3),kx(5),kx(7),kx(2),kx(6),kx(4); GaussianFactorGraph smoother = createSmoother(7, ordering).first; // Create the Bayes tree @@ -192,48 +192,48 @@ TEST_UNSAFE( BayesTree, balanced_smoother_marginals ) double tol=1e-5; // Check marginal on x1 - GaussianBayesNet expected1 = simpleGaussian(ordering["x1"], zero(2), sigmax1); - GaussianBayesNet actual1 = *bayesTree.marginalBayesNet(ordering["x1"]); + GaussianBayesNet expected1 = simpleGaussian(ordering[kx(1)], zero(2), sigmax1); + GaussianBayesNet actual1 = *bayesTree.marginalBayesNet(ordering[kx(1)]); Matrix expectedCovarianceX1 = eye(2,2) * (sigmax1 * sigmax1); Matrix actualCovarianceX1; - actualCovarianceX1 = bayesTree.marginalCovariance(ordering["x1"]); + actualCovarianceX1 = bayesTree.marginalCovariance(ordering[kx(1)]); EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol)); EXPECT(assert_equal(expected1,actual1,tol)); // Check marginal on x2 double sigx2 = 0.68712938; // FIXME: this should be corrected analytically - GaussianBayesNet expected2 = simpleGaussian(ordering["x2"], zero(2), sigx2); - GaussianBayesNet actual2 = *bayesTree.marginalBayesNet(ordering["x2"]); + GaussianBayesNet expected2 = simpleGaussian(ordering[kx(2)], zero(2), sigx2); + GaussianBayesNet actual2 = *bayesTree.marginalBayesNet(ordering[kx(2)]); Matrix expectedCovarianceX2 = eye(2,2) * (sigx2 * sigx2); Matrix actualCovarianceX2; - actualCovarianceX2 = bayesTree.marginalCovariance(ordering["x2"]); + actualCovarianceX2 = bayesTree.marginalCovariance(ordering[kx(2)]); EXPECT(assert_equal(expectedCovarianceX2, actualCovarianceX2, tol)); EXPECT(assert_equal(expected2,actual2,tol)); // Check marginal on x3 - GaussianBayesNet expected3 = simpleGaussian(ordering["x3"], zero(2), sigmax3); - GaussianBayesNet actual3 = *bayesTree.marginalBayesNet(ordering["x3"]); + GaussianBayesNet expected3 = simpleGaussian(ordering[kx(3)], zero(2), sigmax3); + GaussianBayesNet actual3 = *bayesTree.marginalBayesNet(ordering[kx(3)]); Matrix expectedCovarianceX3 = eye(2,2) * (sigmax3 * sigmax3); Matrix actualCovarianceX3; - actualCovarianceX3 = bayesTree.marginalCovariance(ordering["x3"]); + actualCovarianceX3 = bayesTree.marginalCovariance(ordering[kx(3)]); EXPECT(assert_equal(expectedCovarianceX3, actualCovarianceX3, tol)); EXPECT(assert_equal(expected3,actual3,tol)); // Check marginal on x4 - GaussianBayesNet expected4 = simpleGaussian(ordering["x4"], zero(2), sigmax4); - GaussianBayesNet actual4 = *bayesTree.marginalBayesNet(ordering["x4"]); + GaussianBayesNet expected4 = simpleGaussian(ordering[kx(4)], zero(2), sigmax4); + GaussianBayesNet actual4 = *bayesTree.marginalBayesNet(ordering[kx(4)]); Matrix expectedCovarianceX4 = eye(2,2) * (sigmax4 * sigmax4); Matrix actualCovarianceX4; - actualCovarianceX4 = bayesTree.marginalCovariance(ordering["x4"]); + actualCovarianceX4 = bayesTree.marginalCovariance(ordering[kx(4)]); EXPECT(assert_equal(expectedCovarianceX4, actualCovarianceX4, tol)); EXPECT(assert_equal(expected4,actual4,tol)); // Check marginal on x7 (should be equal to x1) - GaussianBayesNet expected7 = simpleGaussian(ordering["x7"], zero(2), sigmax7); - GaussianBayesNet actual7 = *bayesTree.marginalBayesNet(ordering["x7"]); + GaussianBayesNet expected7 = simpleGaussian(ordering[kx(7)], zero(2), sigmax7); + GaussianBayesNet actual7 = *bayesTree.marginalBayesNet(ordering[kx(7)]); Matrix expectedCovarianceX7 = eye(2,2) * (sigmax7 * sigmax7); Matrix actualCovarianceX7; - actualCovarianceX7 = bayesTree.marginalCovariance(ordering["x7"]); + actualCovarianceX7 = bayesTree.marginalCovariance(ordering[kx(7)]); EXPECT(assert_equal(expectedCovarianceX7, actualCovarianceX7, tol)); EXPECT(assert_equal(expected7,actual7,tol)); } @@ -243,7 +243,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_shortcuts ) { // Create smoother with 7 nodes Ordering ordering; - ordering += "x1","x3","x5","x7","x2","x6","x4"; + ordering += kx(1),kx(3),kx(5),kx(7),kx(2),kx(6),kx(4); GaussianFactorGraph smoother = createSmoother(7, ordering).first; // Create the Bayes tree @@ -257,19 +257,19 @@ TEST_UNSAFE( BayesTree, balanced_smoother_shortcuts ) EXPECT(assert_equal(empty,actual1,tol)); // Check the conditional P(C2|Root) - GaussianISAM::sharedClique C2 = isamTree[ordering["x3"]]; + GaussianISAM::sharedClique C2 = isamTree[ordering[kx(3)]]; GaussianBayesNet actual2 = GaussianISAM::shortcut(C2,R); EXPECT(assert_equal(empty,actual2,tol)); // Check the conditional P(C3|Root), which should be equal to P(x2|x4) /** TODO: Note for multifrontal conditional: - * p_x2_x4 is now an element conditional of the multifrontal conditional bayesTree[ordering["x2"]]->conditional() + * p_x2_x4 is now an element conditional of the multifrontal conditional bayesTree[ordering[kx(2)]]->conditional() * We don't know yet how to take it out. */ -// GaussianConditional::shared_ptr p_x2_x4 = bayesTree[ordering["x2"]]->conditional(); +// GaussianConditional::shared_ptr p_x2_x4 = bayesTree[ordering[kx(2)]]->conditional(); // p_x2_x4->print("Conditional p_x2_x4: "); // GaussianBayesNet expected3(p_x2_x4); -// GaussianISAM::sharedClique C3 = isamTree[ordering["x1"]]; +// GaussianISAM::sharedClique C3 = isamTree[ordering[kx(1)]]; // GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R); // EXPECT(assert_equal(expected3,actual3,tol)); } @@ -279,7 +279,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_shortcuts ) //{ // // Create smoother with 7 nodes // Ordering ordering; -// ordering += "x1","x3","x5","x7","x2","x6","x4"; +// ordering += kx(1),kx(3),kx(5),kx(7),kx(2),kx(6),kx(4); // GaussianFactorGraph smoother = createSmoother(7, ordering).first; // // // Create the Bayes tree @@ -288,9 +288,9 @@ TEST_UNSAFE( BayesTree, balanced_smoother_shortcuts ) // // // Check the clique marginal P(C3) // double sigmax2_alt = 1/1.45533; // THIS NEEDS TO BE CHECKED! -// GaussianBayesNet expected = simpleGaussian(ordering["x2"],zero(2),sigmax2_alt); -// push_front(expected,ordering["x1"], zero(2), eye(2)*sqrt(2), ordering["x2"], -eye(2)*sqrt(2)/2, ones(2)); -// GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering["x1"]]; +// GaussianBayesNet expected = simpleGaussian(ordering[kx(2)],zero(2),sigmax2_alt); +// push_front(expected,ordering[kx(1)], zero(2), eye(2)*sqrt(2), ordering[kx(2)], -eye(2)*sqrt(2)/2, ones(2)); +// GaussianISAM::sharedClique R = bayesTree.root(), C3 = bayesTree[ordering[kx(1)]]; // GaussianFactorGraph marginal = C3->marginal(R); // GaussianVariableIndex varIndex(marginal); // Permutation toFront(Permutation::PullToFront(C3->keys(), varIndex.size())); @@ -308,7 +308,7 @@ TEST_UNSAFE( BayesTree, balanced_smoother_joint ) { // Create smoother with 7 nodes Ordering ordering; - ordering += "x1","x3","x5","x7","x2","x6","x4"; + ordering += kx(1),kx(3),kx(5),kx(7),kx(2),kx(6),kx(4); GaussianFactorGraph smoother = createSmoother(7, ordering).first; // Create the Bayes tree, expected to look like: @@ -327,41 +327,41 @@ TEST_UNSAFE( BayesTree, balanced_smoother_joint ) GaussianBayesNet expected1; // Why does the sign get flipped on the prior? GaussianConditional::shared_ptr - parent1(new GaussianConditional(ordering["x7"], zero(2), -1*I/sigmax7, ones(2))); + parent1(new GaussianConditional(ordering[kx(7)], zero(2), -1*I/sigmax7, ones(2))); expected1.push_front(parent1); - push_front(expected1,ordering["x1"], zero(2), I/sigmax7, ordering["x7"], A/sigmax7, sigma); - GaussianBayesNet actual1 = *bayesTree.jointBayesNet(ordering["x1"],ordering["x7"]); + push_front(expected1,ordering[kx(1)], zero(2), I/sigmax7, ordering[kx(7)], A/sigmax7, sigma); + GaussianBayesNet actual1 = *bayesTree.jointBayesNet(ordering[kx(1)],ordering[kx(7)]); EXPECT(assert_equal(expected1,actual1,tol)); // // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1) // GaussianBayesNet expected2; // GaussianConditional::shared_ptr -// parent2(new GaussianConditional(ordering["x1"], zero(2), -1*I/sigmax1, ones(2))); +// parent2(new GaussianConditional(ordering[kx(1)], zero(2), -1*I/sigmax1, ones(2))); // expected2.push_front(parent2); -// push_front(expected2,ordering["x7"], zero(2), I/sigmax1, ordering["x1"], A/sigmax1, sigma); -// GaussianBayesNet actual2 = *bayesTree.jointBayesNet(ordering["x7"],ordering["x1"]); +// push_front(expected2,ordering[kx(7)], zero(2), I/sigmax1, ordering[kx(1)], A/sigmax1, sigma); +// GaussianBayesNet actual2 = *bayesTree.jointBayesNet(ordering[kx(7)],ordering[kx(1)]); // EXPECT(assert_equal(expected2,actual2,tol)); // Check the joint density P(x1,x4), i.e. with a root variable GaussianBayesNet expected3; GaussianConditional::shared_ptr - parent3(new GaussianConditional(ordering["x4"], zero(2), I/sigmax4, ones(2))); + parent3(new GaussianConditional(ordering[kx(4)], zero(2), I/sigmax4, ones(2))); expected3.push_front(parent3); double sig14 = 0.784465; Matrix A14 = -0.0769231*I; - push_front(expected3,ordering["x1"], zero(2), I/sig14, ordering["x4"], A14/sig14, sigma); - GaussianBayesNet actual3 = *bayesTree.jointBayesNet(ordering["x1"],ordering["x4"]); + push_front(expected3,ordering[kx(1)], zero(2), I/sig14, ordering[kx(4)], A14/sig14, sigma); + GaussianBayesNet actual3 = *bayesTree.jointBayesNet(ordering[kx(1)],ordering[kx(4)]); EXPECT(assert_equal(expected3,actual3,tol)); // // Check the joint density P(x4,x1), i.e. with a root variable, factored the other way // GaussianBayesNet expected4; // GaussianConditional::shared_ptr -// parent4(new GaussianConditional(ordering["x1"], zero(2), -1.0*I/sigmax1, ones(2))); +// parent4(new GaussianConditional(ordering[kx(1)], zero(2), -1.0*I/sigmax1, ones(2))); // expected4.push_front(parent4); // double sig41 = 0.668096; // Matrix A41 = -0.055794*I; -// push_front(expected4,ordering["x4"], zero(2), I/sig41, ordering["x1"], A41/sig41, sigma); -// GaussianBayesNet actual4 = *bayesTree.jointBayesNet(ordering["x4"],ordering["x1"]); +// push_front(expected4,ordering[kx(4)], zero(2), I/sig41, ordering[kx(1)], A41/sig41, sigma); +// GaussianBayesNet actual4 = *bayesTree.jointBayesNet(ordering[kx(4)],ordering[kx(1)]); // EXPECT(assert_equal(expected4,actual4,tol)); } diff --git a/tests/testGaussianISAM2.cpp b/tests/testGaussianISAM2.cpp index d28844cd9..f41cff097 100644 --- a/tests/testGaussianISAM2.cpp +++ b/tests/testGaussianISAM2.cpp @@ -11,8 +11,6 @@ using namespace boost::assign; #include -#define GTSAM_MAGIC_KEY - #include #include #include @@ -33,10 +31,10 @@ const double tol = 1e-4; TEST(ISAM2, AddVariables) { // Create initial state - planarSLAM::Values theta; + Values theta; theta.insert(planarSLAM::PoseKey(0), Pose2(.1, .2, .3)); theta.insert(planarSLAM::PointKey(0), Point2(.4, .5)); - planarSLAM::Values newTheta; + Values newTheta; newTheta.insert(planarSLAM::PoseKey(1), Pose2(.6, .7, .8)); VectorValues deltaUnpermuted; @@ -51,7 +49,7 @@ TEST(ISAM2, AddVariables) { Ordering ordering; ordering += planarSLAM::PointKey(0), planarSLAM::PoseKey(0); - GaussianISAM2::Nodes nodes(2); + GaussianISAM2<>::Nodes nodes(2); // Verify initial state LONGS_EQUAL(0, ordering[planarSLAM::PointKey(0)]); @@ -60,7 +58,7 @@ TEST(ISAM2, AddVariables) { EXPECT(assert_equal(deltaUnpermuted[0], delta[ordering[planarSLAM::PoseKey(0)]])); // Create expected state - planarSLAM::Values thetaExpected; + Values thetaExpected; thetaExpected.insert(planarSLAM::PoseKey(0), Pose2(.1, .2, .3)); thetaExpected.insert(planarSLAM::PointKey(0), Point2(.4, .5)); thetaExpected.insert(planarSLAM::PoseKey(1), Pose2(.6, .7, .8)); @@ -79,11 +77,11 @@ TEST(ISAM2, AddVariables) { Ordering orderingExpected; orderingExpected += planarSLAM::PointKey(0), planarSLAM::PoseKey(0), planarSLAM::PoseKey(1); - GaussianISAM2::Nodes nodesExpected( - 3, GaussianISAM2::sharedClique()); + GaussianISAM2<>::Nodes nodesExpected( + 3, GaussianISAM2<>::sharedClique()); // Expand initial state - GaussianISAM2::Impl::AddVariables(newTheta, theta, delta, ordering, nodes); + GaussianISAM2<>::Impl::AddVariables(newTheta, theta, delta, ordering, nodes); EXPECT(assert_equal(thetaExpected, theta)); EXPECT(assert_equal(deltaUnpermutedExpected, deltaUnpermuted)); @@ -95,7 +93,7 @@ TEST(ISAM2, AddVariables) { //TEST(ISAM2, IndicesFromFactors) { // // using namespace gtsam::planarSLAM; -// typedef GaussianISAM2::Impl Impl; +// typedef GaussianISAM2::Impl Impl; // // Ordering ordering; ordering += PointKey(0), PoseKey(0), PoseKey(1); // planarSLAM::Graph graph; @@ -114,7 +112,7 @@ TEST(ISAM2, AddVariables) { /* ************************************************************************* */ //TEST(ISAM2, CheckRelinearization) { // -// typedef GaussianISAM2::Impl Impl; +// typedef GaussianISAM2::Impl Impl; // // // Create values where indices 1 and 3 are above the threshold of 0.1 // VectorValues values; @@ -148,7 +146,7 @@ TEST(ISAM2, AddVariables) { TEST(ISAM2, optimize2) { // Create initialization - planarSLAM::Values theta; + Values theta; theta.insert(planarSLAM::PoseKey(0), Pose2(0.01, 0.01, 0.01)); // Create conditional @@ -168,8 +166,8 @@ TEST(ISAM2, optimize2) { conditional->solveInPlace(expected); // Clique - GaussianISAM2::sharedClique clique( - GaussianISAM2::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr()))); + GaussianISAM2<>::sharedClique clique( + GaussianISAM2<>::Clique::Create(make_pair(conditional,GaussianFactor::shared_ptr()))); VectorValues actual(theta.dims(ordering)); conditional->rhs(actual); optimize2(clique, actual); @@ -180,15 +178,15 @@ TEST(ISAM2, optimize2) { } /* ************************************************************************* */ -bool isam_check(const planarSLAM::Graph& fullgraph, const planarSLAM::Values& fullinit, const GaussianISAM2& isam) { - planarSLAM::Values actual = isam.calculateEstimate(); +bool isam_check(const planarSLAM::Graph& fullgraph, const Values& fullinit, const GaussianISAM2<>& isam) { + Values actual = isam.calculateEstimate(); Ordering ordering = isam.getOrdering(); // *fullgraph.orderingCOLAMD(fullinit).first; GaussianFactorGraph linearized = *fullgraph.linearize(fullinit, ordering); // linearized.print("Expected linearized: "); GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); // gbn.print("Expected bayesnet: "); VectorValues delta = optimize(gbn); - planarSLAM::Values expected = fullinit.retract(delta, ordering); + Values expected = fullinit.retract(delta, ordering); return assert_equal(expected, actual); } @@ -202,16 +200,16 @@ TEST(ISAM2, slamlike_solution_gaussnewton) // SETDEBUG("ISAM2 recalculate", true); // Pose and landmark key types from planarSLAM - typedef planarSLAM::PoseKey PoseKey; - typedef planarSLAM::PointKey PointKey; + using planarSLAM::PoseKey; + using planarSLAM::PointKey; // Set up parameters SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); - planarSLAM::Values fullinit; + GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + Values fullinit; planarSLAM::Graph fullgraph; // i keeps track of the time step @@ -223,7 +221,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); @@ -238,7 +236,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); @@ -253,7 +251,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); @@ -271,7 +269,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); @@ -286,7 +284,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); @@ -298,7 +296,7 @@ TEST(ISAM2, slamlike_solution_gaussnewton) CHECK(isam_check(fullgraph, fullinit, isam)); // Check gradient at each node - typedef GaussianISAM2::sharedClique sharedClique; + typedef GaussianISAM2<>::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient FactorGraph jfg; @@ -335,16 +333,16 @@ TEST(ISAM2, slamlike_solution_dogleg) // SETDEBUG("ISAM2 recalculate", true); // Pose and landmark key types from planarSLAM - typedef planarSLAM::PoseKey PoseKey; - typedef planarSLAM::PointKey PointKey; + using planarSLAM::PoseKey; + using planarSLAM::PointKey; // Set up parameters SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2 isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); - planarSLAM::Values fullinit; + GaussianISAM2<> isam(ISAM2Params(ISAM2DoglegParams(1.0), 0.0, 0, false)); + Values fullinit; planarSLAM::Graph fullgraph; // i keeps track of the time step @@ -356,7 +354,7 @@ TEST(ISAM2, slamlike_solution_dogleg) newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); @@ -371,7 +369,7 @@ TEST(ISAM2, slamlike_solution_dogleg) newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); @@ -386,7 +384,7 @@ TEST(ISAM2, slamlike_solution_dogleg) newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); @@ -404,7 +402,7 @@ TEST(ISAM2, slamlike_solution_dogleg) newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); @@ -419,7 +417,7 @@ TEST(ISAM2, slamlike_solution_dogleg) newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); @@ -431,7 +429,7 @@ TEST(ISAM2, slamlike_solution_dogleg) CHECK(isam_check(fullgraph, fullinit, isam)); // Check gradient at each node - typedef GaussianISAM2::sharedClique sharedClique; + typedef GaussianISAM2<>::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient FactorGraph jfg; @@ -463,16 +461,16 @@ TEST(ISAM2, slamlike_solution_dogleg) TEST(ISAM2, clone) { // Pose and landmark key types from planarSLAM - typedef planarSLAM::PoseKey PoseKey; - typedef planarSLAM::PointKey PointKey; + using planarSLAM::PoseKey; + using planarSLAM::PointKey; // Set up parameters SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)); - planarSLAM::Values fullinit; + GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false, true)); + Values fullinit; planarSLAM::Graph fullgraph; // i keeps track of the time step @@ -484,7 +482,7 @@ TEST(ISAM2, clone) { newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); @@ -499,7 +497,7 @@ TEST(ISAM2, clone) { newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); @@ -514,7 +512,7 @@ TEST(ISAM2, clone) { newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); @@ -532,7 +530,7 @@ TEST(ISAM2, clone) { newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); @@ -547,7 +545,7 @@ TEST(ISAM2, clone) { newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); @@ -556,8 +554,8 @@ TEST(ISAM2, clone) { } // CLONING... - boost::shared_ptr > isam2 - = boost::shared_ptr >(new GaussianISAM2()); + boost::shared_ptr > isam2 + = boost::shared_ptr >(new GaussianISAM2<>()); isam.cloneTo(isam2); CHECK(assert_equal(isam, *isam2)); @@ -636,16 +634,16 @@ TEST(ISAM2, removeFactors) // then removes the 2nd-to-last landmark measurement // Pose and landmark key types from planarSLAM - typedef planarSLAM::PoseKey PoseKey; - typedef planarSLAM::PointKey PointKey; + using planarSLAM::PoseKey; + using planarSLAM::PointKey; // Set up parameters SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); - planarSLAM::Values fullinit; + GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + Values fullinit; planarSLAM::Graph fullgraph; // i keeps track of the time step @@ -657,7 +655,7 @@ TEST(ISAM2, removeFactors) newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); @@ -672,7 +670,7 @@ TEST(ISAM2, removeFactors) newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); @@ -687,7 +685,7 @@ TEST(ISAM2, removeFactors) newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); @@ -705,7 +703,7 @@ TEST(ISAM2, removeFactors) newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); @@ -721,7 +719,7 @@ TEST(ISAM2, removeFactors) fullgraph.push_back(newfactors[0]); fullgraph.push_back(newfactors[2]); // Don't add measurement on landmark 0 - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); @@ -731,14 +729,14 @@ TEST(ISAM2, removeFactors) // Remove the measurement on landmark 0 FastVector toRemove; toRemove.push_back(result.newFactorsIndices[1]); - isam.update(planarSLAM::Graph(), planarSLAM::Values(), toRemove); + isam.update(planarSLAM::Graph(), Values(), toRemove); } // Compare solutions CHECK(isam_check(fullgraph, fullinit, isam)); // Check gradient at each node - typedef GaussianISAM2::sharedClique sharedClique; + typedef GaussianISAM2<>::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient FactorGraph jfg; @@ -775,20 +773,20 @@ TEST(ISAM2, constrained_ordering) // SETDEBUG("ISAM2 recalculate", true); // Pose and landmark key types from planarSLAM - typedef planarSLAM::PoseKey PoseKey; - typedef planarSLAM::PointKey PointKey; + using planarSLAM::PoseKey; + using planarSLAM::PointKey; // Set up parameters SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); SharedDiagonal brNoise = sharedSigmas(Vector_(2, M_PI/100.0, 0.1)); // These variables will be reused and accumulate factors and values - GaussianISAM2 isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); - planarSLAM::Values fullinit; + GaussianISAM2<> isam(ISAM2Params(ISAM2GaussNewtonParams(0.001), 0.0, 0, false)); + Values fullinit; planarSLAM::Graph fullgraph; // We will constrain x3 and x4 to the end - FastSet constrained; constrained.insert(planarSLAM::PoseKey(3)); constrained.insert(planarSLAM::PoseKey(4)); + FastSet constrained; constrained.insert(planarSLAM::PoseKey(3)); constrained.insert(planarSLAM::PoseKey(4)); // i keeps track of the time step size_t i = 0; @@ -799,7 +797,7 @@ TEST(ISAM2, constrained_ordering) newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); fullinit.insert(PoseKey(0), Pose2(0.01, 0.01, 0.01)); @@ -814,7 +812,7 @@ TEST(ISAM2, constrained_ordering) newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); @@ -832,7 +830,7 @@ TEST(ISAM2, constrained_ordering) newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0), 5.0, brNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(1.01, 0.01, 0.01)); init.insert(PointKey(0), Point2(5.0/sqrt(2.0), 5.0/sqrt(2.0))); init.insert(PointKey(1), Point2(5.0/sqrt(2.0), -5.0/sqrt(2.0))); @@ -850,7 +848,7 @@ TEST(ISAM2, constrained_ordering) newfactors.addOdometry(i, i+1, Pose2(1.0, 0.0, 0.0), odoNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(double(i+1)+0.1, -0.1, 0.01)); @@ -865,7 +863,7 @@ TEST(ISAM2, constrained_ordering) newfactors.addBearingRange(i, 1, Rot2::fromAngle(-M_PI/4.0 + M_PI/16.0), 4.5, brNoise); fullgraph.push_back(newfactors); - planarSLAM::Values init; + Values init; init.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); fullinit.insert(PoseKey(i+1), Pose2(6.9, 0.1, 0.01)); @@ -881,7 +879,7 @@ TEST(ISAM2, constrained_ordering) (isam.getOrdering()[planarSLAM::PoseKey(3)] == 13 && isam.getOrdering()[planarSLAM::PoseKey(4)] == 12)); // Check gradient at each node - typedef GaussianISAM2::sharedClique sharedClique; + typedef GaussianISAM2<>::sharedClique sharedClique; BOOST_FOREACH(const sharedClique& clique, isam.nodes()) { // Compute expected gradient FactorGraph jfg; diff --git a/tests/testGaussianJunctionTree.cpp b/tests/testGaussianJunctionTree.cpp index da9a8aede..b01aa2ba4 100644 --- a/tests/testGaussianJunctionTree.cpp +++ b/tests/testGaussianJunctionTree.cpp @@ -25,9 +25,6 @@ #include using namespace boost::assign; -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include #include #include @@ -43,6 +40,9 @@ using namespace std; using namespace gtsam; using namespace example; +Key kx(size_t i) { return Symbol('x',i); } +Key kl(size_t i) { return Symbol('l',i); } + /* ************************************************************************* * Bayes tree for smoother with "nested dissection" ordering: C1 x5 x6 x4 @@ -53,20 +53,20 @@ using namespace example; TEST( GaussianJunctionTree, constructor2 ) { // create a graph - Ordering ordering; ordering += "x1","x3","x5","x7","x2","x6","x4"; + Ordering ordering; ordering += kx(1),kx(3),kx(5),kx(7),kx(2),kx(6),kx(4); GaussianFactorGraph fg = createSmoother(7, ordering).first; // create an ordering GaussianJunctionTree actual(fg); - vector frontal1; frontal1 += ordering["x5"], ordering["x6"], ordering["x4"]; - vector frontal2; frontal2 += ordering["x3"], ordering["x2"]; - vector frontal3; frontal3 += ordering["x1"]; - vector frontal4; frontal4 += ordering["x7"]; + vector frontal1; frontal1 += ordering[kx(5)], ordering[kx(6)], ordering[kx(4)]; + vector frontal2; frontal2 += ordering[kx(3)], ordering[kx(2)]; + vector frontal3; frontal3 += ordering[kx(1)]; + vector frontal4; frontal4 += ordering[kx(7)]; vector sep1; - vector sep2; sep2 += ordering["x4"]; - vector sep3; sep3 += ordering["x2"]; - vector sep4; sep4 += ordering["x6"]; + vector sep2; sep2 += ordering[kx(4)]; + vector sep3; sep3 += ordering[kx(2)]; + vector sep4; sep4 += ordering[kx(6)]; EXPECT(assert_equal(frontal1, actual.root()->frontal)); EXPECT(assert_equal(sep1, actual.root()->separator)); LONGS_EQUAL(5, actual.root()->size()); @@ -110,8 +110,8 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal2) { // create a graph example::Graph nlfg = createNonlinearFactorGraph(); - example::Values noisy = createNoisyValues(); - Ordering ordering; ordering += "x1","x2","l1"; + Values noisy = createNoisyValues(); + Ordering ordering; ordering += kx(1),kx(2),kl(1); GaussianFactorGraph fg = *nlfg.linearize(noisy, ordering); // optimize the graph @@ -125,10 +125,10 @@ TEST( GaussianJunctionTree, optimizeMultiFrontal2) /* ************************************************************************* */ TEST(GaussianJunctionTree, slamlike) { - typedef planarSLAM::PoseKey PoseKey; - typedef planarSLAM::PointKey PointKey; + using planarSLAM::PoseKey; + using planarSLAM::PointKey; - planarSLAM::Values init; + Values init; planarSLAM::Graph newfactors; planarSLAM::Graph fullgraph; SharedDiagonal odoNoise = sharedSigmas(Vector_(3, 0.1, 0.1, M_PI/100.0)); @@ -179,11 +179,11 @@ TEST(GaussianJunctionTree, slamlike) { GaussianJunctionTree gjt(linearized); VectorValues deltaactual = gjt.optimize(&EliminateQR); - planarSLAM::Values actual = init.retract(deltaactual, ordering); + Values actual = init.retract(deltaactual, ordering); GaussianBayesNet gbn = *GaussianSequentialSolver(linearized).eliminate(); VectorValues delta = optimize(gbn); - planarSLAM::Values expected = init.retract(delta, ordering); + Values expected = init.retract(delta, ordering); EXPECT(assert_equal(expected, actual)); } @@ -195,15 +195,15 @@ TEST(GaussianJunctionTree, simpleMarginal) { // Create a simple graph pose2SLAM::Graph fg; - fg.addPrior(pose2SLAM::PoseKey(0), Pose2(), sharedSigma(3, 10.0)); + fg.addPrior(0, Pose2(), sharedSigma(3, 10.0)); fg.addOdometry(0, 1, Pose2(1.0, 0.0, 0.0), sharedSigmas(Vector_(3, 10.0, 1.0, 1.0))); - pose2SLAM::Values init; - init.insert(0, Pose2()); - init.insert(1, Pose2(1.0, 0.0, 0.0)); + Values init; + init.insert(pose2SLAM::PoseKey(0), Pose2()); + init.insert(pose2SLAM::PoseKey(1), Pose2(1.0, 0.0, 0.0)); Ordering ordering; - ordering += "x1", "x0"; + ordering += kx(1), kx(0); GaussianFactorGraph gfg = *fg.linearize(init, ordering); diff --git a/tests/testGraph.cpp b/tests/testGraph.cpp index db9874c89..f56efae82 100644 --- a/tests/testGraph.cpp +++ b/tests/testGraph.cpp @@ -32,21 +32,22 @@ using namespace boost::assign; using namespace std; using namespace gtsam; +Key kx(size_t i) { return Symbol('x',i); } + /* ************************************************************************* */ // x1 -> x2 // -> x3 -> x4 // -> x5 TEST ( Ordering, predecessorMap2Keys ) { - typedef TypedSymbol Key; PredecessorMap p_map; - p_map.insert(1,1); - p_map.insert(2,1); - p_map.insert(3,1); - p_map.insert(4,3); - p_map.insert(5,1); + p_map.insert(kx(1),kx(1)); + p_map.insert(kx(2),kx(1)); + p_map.insert(kx(3),kx(1)); + p_map.insert(kx(4),kx(3)); + p_map.insert(kx(5),kx(1)); list expected; - expected += 4,5,3,2,1;//Key(4), Key(5), Key(3), Key(2), Key(1); + expected += kx(4),kx(5),kx(3),kx(2),kx(1);//PoseKey(4), PoseKey(5), PoseKey(3), PoseKey(2), PoseKey(1); list actual = predecessorMap2Keys(p_map); LONGS_EQUAL(expected.size(), actual.size()); @@ -61,18 +62,18 @@ TEST ( Ordering, predecessorMap2Keys ) { TEST( Graph, predecessorMap2Graph ) { typedef SGraph::Vertex SVertex; - SGraph graph; + SGraph graph; SVertex root; - map key2vertex; + map key2vertex; - PredecessorMap p_map; - p_map.insert("x1", "x2"); - p_map.insert("x2", "x2"); - p_map.insert("x3", "x2"); - tie(graph, root, key2vertex) = predecessorMap2Graph, SVertex, string>(p_map); + PredecessorMap p_map; + p_map.insert(kx(1), kx(2)); + p_map.insert(kx(2), kx(2)); + p_map.insert(kx(3), kx(2)); + tie(graph, root, key2vertex) = predecessorMap2Graph, SVertex, Key>(p_map); LONGS_EQUAL(3, boost::num_vertices(graph)); - CHECK(root == key2vertex["x2"]); + CHECK(root == key2vertex[kx(2)]); } /* ************************************************************************* */ @@ -86,22 +87,22 @@ TEST( Graph, composePoses ) graph.addOdometry(2,3, p23, cov); graph.addOdometry(4,3, p43, cov); - PredecessorMap tree; - tree.insert(1,2); - tree.insert(2,2); - tree.insert(3,2); - tree.insert(4,3); + PredecessorMap tree; + tree.insert(pose2SLAM::PoseKey(1),pose2SLAM::PoseKey(2)); + tree.insert(pose2SLAM::PoseKey(2),pose2SLAM::PoseKey(2)); + tree.insert(pose2SLAM::PoseKey(3),pose2SLAM::PoseKey(2)); + tree.insert(pose2SLAM::PoseKey(4),pose2SLAM::PoseKey(3)); Pose2 rootPose = p2; - boost::shared_ptr actual = composePoses (graph, tree, rootPose); + boost::shared_ptr actual = composePoses (graph, tree, rootPose); - pose2SLAM::Values expected; - expected.insert(1, p1); - expected.insert(2, p2); - expected.insert(3, p3); - expected.insert(4, p4); + Values expected; + expected.insert(pose2SLAM::PoseKey(1), p1); + expected.insert(pose2SLAM::PoseKey(2), p2); + expected.insert(pose2SLAM::PoseKey(3), p3); + expected.insert(pose2SLAM::PoseKey(4), p4); LONGS_EQUAL(4, actual->size()); CHECK(assert_equal(expected, *actual)); diff --git a/tests/testInference.cpp b/tests/testInference.cpp index 555f8a061..df8843415 100644 --- a/tests/testInference.cpp +++ b/tests/testInference.cpp @@ -17,9 +17,6 @@ #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include #include #include @@ -54,14 +51,14 @@ TEST( Inference, marginals2) SharedDiagonal poseModel(sharedSigma(3, 0.1)); SharedDiagonal pointModel(sharedSigma(3, 0.1)); - fg.addPrior(planarSLAM::PoseKey(0), Pose2(), poseModel); - fg.addOdometry(planarSLAM::PoseKey(0), planarSLAM::PoseKey(1), Pose2(1.0,0.0,0.0), poseModel); - fg.addOdometry(planarSLAM::PoseKey(1), planarSLAM::PoseKey(2), Pose2(1.0,0.0,0.0), poseModel); - fg.addBearingRange(planarSLAM::PoseKey(0), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel); - fg.addBearingRange(planarSLAM::PoseKey(1), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel); - fg.addBearingRange(planarSLAM::PoseKey(2), planarSLAM::PointKey(0), Rot2(), 1.0, pointModel); + fg.addPrior(0, Pose2(), poseModel); + fg.addOdometry(0, 1, Pose2(1.0,0.0,0.0), poseModel); + fg.addOdometry(1, 2, Pose2(1.0,0.0,0.0), poseModel); + fg.addBearingRange(0, 0, Rot2(), 1.0, pointModel); + fg.addBearingRange(1, 0, Rot2(), 1.0, pointModel); + fg.addBearingRange(2, 0, Rot2(), 1.0, pointModel); - planarSLAM::Values init; + Values init; init.insert(planarSLAM::PoseKey(0), Pose2(0.0,0.0,0.0)); init.insert(planarSLAM::PoseKey(1), Pose2(1.0,0.0,0.0)); init.insert(planarSLAM::PoseKey(2), Pose2(2.0,0.0,0.0)); diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index fd1459a17..2ceb066e2 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -31,21 +31,19 @@ namespace eq2D = simulated2D::equality_constraints; static const double tol = 1e-5; -typedef TypedSymbol PoseKey; -typedef Values PoseValues; -typedef PriorFactor PosePrior; -typedef NonlinearEquality PoseNLE; +typedef PriorFactor PosePrior; +typedef NonlinearEquality PoseNLE; typedef boost::shared_ptr shared_poseNLE; -typedef NonlinearFactorGraph PoseGraph; -typedef NonlinearOptimizer PoseOptimizer; +typedef NonlinearFactorGraph PoseGraph; +typedef NonlinearOptimizer PoseOptimizer; -PoseKey key(1); +Symbol key('x',1); /* ************************************************************************* */ TEST ( NonlinearEquality, linearization ) { Pose2 value = Pose2(2.1, 1.0, 2.0); - PoseValues linearize; + Values linearize; linearize.insert(key, value); // create a nonlinear equality constraint @@ -61,9 +59,9 @@ TEST ( NonlinearEquality, linearization ) { /* ********************************************************************** */ TEST ( NonlinearEquality, linearization_pose ) { - PoseKey key(1); + Symbol key('x',1); Pose2 value; - PoseValues config; + Values config; config.insert(key, value); // create a nonlinear equality constraint @@ -77,7 +75,7 @@ TEST ( NonlinearEquality, linearization_pose ) { TEST ( NonlinearEquality, linearization_fail ) { Pose2 value = Pose2(2.1, 1.0, 2.0); Pose2 wrong = Pose2(2.1, 3.0, 4.0); - PoseValues bad_linearize; + Values bad_linearize; bad_linearize.insert(key, wrong); // create a nonlinear equality constraint @@ -90,10 +88,10 @@ TEST ( NonlinearEquality, linearization_fail ) { /* ********************************************************************** */ TEST ( NonlinearEquality, linearization_fail_pose ) { - PoseKey key(1); + Symbol key('x',1); Pose2 value(2.0, 1.0, 2.0), wrong(2.0, 3.0, 4.0); - PoseValues bad_linearize; + Values bad_linearize; bad_linearize.insert(key, wrong); // create a nonlinear equality constraint @@ -106,10 +104,10 @@ TEST ( NonlinearEquality, linearization_fail_pose ) { /* ********************************************************************** */ TEST ( NonlinearEquality, linearization_fail_pose_origin ) { - PoseKey key(1); + Symbol key('x',1); Pose2 value, wrong(2.0, 3.0, 4.0); - PoseValues bad_linearize; + Values bad_linearize; bad_linearize.insert(key, wrong); // create a nonlinear equality constraint @@ -123,7 +121,7 @@ TEST ( NonlinearEquality, linearization_fail_pose_origin ) { TEST ( NonlinearEquality, error ) { Pose2 value = Pose2(2.1, 1.0, 2.0); Pose2 wrong = Pose2(2.1, 3.0, 4.0); - PoseValues feasible, bad_linearize; + Values feasible, bad_linearize; feasible.insert(key, value); bad_linearize.insert(key, wrong); @@ -156,7 +154,7 @@ TEST ( NonlinearEquality, equals ) { /* ************************************************************************* */ TEST ( NonlinearEquality, allow_error_pose ) { - PoseKey key1(1); + Symbol key1('x',1); Pose2 feasible1(1.0, 2.0, 3.0); double error_gain = 500.0; PoseNLE nle(key1, feasible1, error_gain); @@ -168,7 +166,7 @@ TEST ( NonlinearEquality, allow_error_pose ) { EXPECT(assert_equal(expVec, actVec, 1e-5)); // the actual error should have a gain on it - PoseValues config; + Values config; config.insert(key1, badPoint1); double actError = nle.error(config); DOUBLES_EQUAL(500.0, actError, 1e-9); @@ -184,7 +182,7 @@ TEST ( NonlinearEquality, allow_error_pose ) { /* ************************************************************************* */ TEST ( NonlinearEquality, allow_error_optimize ) { - PoseKey key1(1); + Symbol key1('x',1); Pose2 feasible1(1.0, 2.0, 3.0); double error_gain = 500.0; PoseNLE nle(key1, feasible1, error_gain); @@ -195,18 +193,18 @@ TEST ( NonlinearEquality, allow_error_optimize ) { // initialize away from the ideal Pose2 initPose(0.0, 2.0, 3.0); - boost::shared_ptr init(new PoseValues()); + boost::shared_ptr init(new Values()); init->insert(key1, initPose); // optimize boost::shared_ptr ord(new Ordering()); ord->push_back(key1); - NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-5, 1e-5); + NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-5, 1e-5); PoseOptimizer optimizer(graph, init, ord, params); PoseOptimizer result = optimizer.levenbergMarquardt(); // verify - PoseValues expected; + Values expected; expected.insert(key1, feasible1); EXPECT(assert_equal(expected, *result.values())); } @@ -215,11 +213,11 @@ TEST ( NonlinearEquality, allow_error_optimize ) { TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { // create a hard constraint - PoseKey key1(1); + Symbol key1('x',1); Pose2 feasible1(1.0, 2.0, 3.0); // initialize away from the ideal - boost::shared_ptr init(new PoseValues()); + boost::shared_ptr init(new Values()); Pose2 initPose(0.0, 2.0, 3.0); init->insert(key1, initPose); @@ -237,12 +235,12 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { // optimize boost::shared_ptr ord(new Ordering()); ord->push_back(key1); - NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-5, 1e-5); + NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-5, 1e-5); PoseOptimizer optimizer(graph, init, ord, params); PoseOptimizer result = optimizer.levenbergMarquardt(); // verify - PoseValues expected; + Values expected; expected.insert(key1, feasible1); EXPECT(assert_equal(expected, *result.values())); } @@ -251,26 +249,26 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { SharedDiagonal hard_model = noiseModel::Constrained::All(2); SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0); -typedef NonlinearFactorGraph Graph; +typedef NonlinearFactorGraph Graph; typedef boost::shared_ptr shared_graph; -typedef boost::shared_ptr shared_values; -typedef NonlinearOptimizer Optimizer; +typedef boost::shared_ptr shared_values; +typedef NonlinearOptimizer Optimizer; /* ************************************************************************* */ TEST( testNonlinearEqualityConstraint, unary_basics ) { Point2 pt(1.0, 2.0); - simulated2D::PoseKey key(1); + Symbol key1('x',1); double mu = 1000.0; eq2D::UnaryEqualityConstraint constraint(pt, key, mu); - simulated2D::Values config1; + Values config1; config1.insert(key, pt); EXPECT(constraint.active(config1)); EXPECT(assert_equal(zero(2), constraint.evaluateError(pt), tol)); EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol)); EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol); - simulated2D::Values config2; + Values config2; Point2 ptBad1(2.0, 2.0); config2.insert(key, ptBad1); EXPECT(constraint.active(config2)); @@ -282,19 +280,19 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) { /* ************************************************************************* */ TEST( testNonlinearEqualityConstraint, unary_linearization ) { Point2 pt(1.0, 2.0); - simulated2D::PoseKey key(1); + Symbol key1('x',1); double mu = 1000.0; Ordering ordering; ordering += key; eq2D::UnaryEqualityConstraint constraint(pt, key, mu); - simulated2D::Values config1; + Values config1; config1.insert(key, pt); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1, ordering); GaussianFactor::shared_ptr expected1(new JacobianFactor(ordering[key], eye(2,2), zero(2), hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); - simulated2D::Values config2; + Values config2; Point2 ptBad(2.0, 2.0); config2.insert(key, ptBad); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2, ordering); @@ -307,7 +305,7 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) { // create a single-node graph with a soft and hard constraint to // ensure that the hard constraint overrides the soft constraint Point2 truth_pt(1.0, 2.0); - simulated2D::PoseKey key(1); + Symbol key('x',1); double mu = 10.0; eq2D::UnaryEqualityConstraint::shared_ptr constraint( new eq2D::UnaryEqualityConstraint(truth_pt, key, mu)); @@ -320,13 +318,13 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) { graph->push_back(constraint); graph->push_back(factor); - shared_values initValues(new simulated2D::Values()); + shared_values initValues(new Values()); initValues->insert(key, badPt); // verify error values EXPECT(constraint->active(*initValues)); - simulated2D::Values expected; + Values expected; expected.insert(key, truth_pt); EXPECT(constraint->active(expected)); EXPECT_DOUBLES_EQUAL(0.0, constraint->error(expected), tol); @@ -338,11 +336,11 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) { /* ************************************************************************* */ TEST( testNonlinearEqualityConstraint, odo_basics ) { Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); - simulated2D::PoseKey key1(1), key2(2); + Symbol key1('x',1), key2('x',2); double mu = 1000.0; eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); - simulated2D::Values config1; + Values config1; config1.insert(key1, x1); config1.insert(key2, x2); EXPECT(constraint.active(config1)); @@ -350,7 +348,7 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) { EXPECT(assert_equal(zero(2), constraint.unwhitenedError(config1), tol)); EXPECT_DOUBLES_EQUAL(0.0, constraint.error(config1), tol); - simulated2D::Values config2; + Values config2; Point2 x1bad(2.0, 2.0); Point2 x2bad(2.0, 2.0); config2.insert(key1, x1bad); @@ -364,13 +362,13 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) { /* ************************************************************************* */ TEST( testNonlinearEqualityConstraint, odo_linearization ) { Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); - simulated2D::PoseKey key1(1), key2(2); + Symbol key1('x',1), key2('x',2); double mu = 1000.0; Ordering ordering; ordering += key1, key2; eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); - simulated2D::Values config1; + Values config1; config1.insert(key1, x1); config1.insert(key2, x2); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1, ordering); @@ -379,7 +377,7 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { eye(2,2), zero(2), hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); - simulated2D::Values config2; + Values config2; Point2 x1bad(2.0, 2.0); Point2 x2bad(2.0, 2.0); config2.insert(key1, x1bad); @@ -397,7 +395,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { // a hard prior on one variable, and a conflicting soft prior // on the other variable - the constraints should override the soft constraint Point2 truth_pt1(1.0, 2.0), truth_pt2(3.0, 2.0); - simulated2D::PoseKey key1(1), key2(2); + Symbol key1('x',1), key2('x',2); // hard prior on x1 eq2D::UnaryEqualityConstraint::shared_ptr constraint1( @@ -418,12 +416,12 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { graph->push_back(constraint2); graph->push_back(factor); - shared_values initValues(new simulated2D::Values()); + shared_values initValues(new Values()); initValues->insert(key1, Point2()); initValues->insert(key2, badPt); Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues); - simulated2D::Values expected; + Values expected; expected.insert(key1, truth_pt1); expected.insert(key2, truth_pt2); CHECK(assert_equal(expected, *actual, tol)); @@ -439,8 +437,8 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { shared_graph graph(new Graph()); - simulated2D::PoseKey x1(1), x2(2); - simulated2D::PointKey l1(1), l2(2); + Symbol x1('x',1), x2('x',2); + Symbol l1('l',1), l2('l',2); Point2 pt_x1(1.0, 1.0), pt_x2(5.0, 6.0); graph->add(eq2D::UnaryEqualityConstraint(pt_x1, x1)); @@ -455,7 +453,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { graph->add(eq2D::PointEqualityConstraint(l1, l2)); - shared_values initialEstimate(new simulated2D::Values()); + shared_values initialEstimate(new Values()); initialEstimate->insert(x1, pt_x1); initialEstimate->insert(x2, Point2()); initialEstimate->insert(l1, Point2(1.0, 6.0)); // ground truth @@ -463,7 +461,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate); - simulated2D::Values expected; + Values expected; expected.insert(x1, pt_x1); expected.insert(l1, Point2(1.0, 6.0)); expected.insert(l2, Point2(1.0, 6.0)); @@ -477,8 +475,8 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { shared_graph graph(new Graph()); // keys - simulated2D::PoseKey x1(1), x2(2); - simulated2D::PointKey l1(1), l2(2); + Symbol x1('x',1), x2('x',2); + Symbol l1('l',1), l2('l',2); // constant constraint on x1 Point2 pose1(1.0, 1.0); @@ -498,7 +496,7 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { graph->add(eq2D::PointEqualityConstraint(l1, l2)); // create an initial estimate - shared_values initialEstimate(new simulated2D::Values()); + shared_values initialEstimate(new Values()); initialEstimate->insert(x1, Point2( 1.0, 1.0)); initialEstimate->insert(l1, Point2( 1.0, 6.0)); initialEstimate->insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame @@ -507,7 +505,7 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { // optimize Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initialEstimate); - simulated2D::Values expected; + Values expected; expected.insert(x1, Point2(1.0, 1.0)); expected.insert(l1, Point2(1.0, 6.0)); expected.insert(l2, Point2(1.0, 6.0)); @@ -522,13 +520,12 @@ Cal3_S2 K(fov,w,h); boost::shared_ptr shK(new Cal3_S2(K)); // typedefs for visual SLAM example -typedef visualSLAM::Values VValues; -typedef boost::shared_ptr shared_vconfig; +typedef boost::shared_ptr shared_vconfig; typedef visualSLAM::Graph VGraph; -typedef NonlinearOptimizer VOptimizer; +typedef NonlinearOptimizer VOptimizer; // factors for visual slam -typedef NonlinearEquality2 Point3Equality; +typedef NonlinearEquality2 Point3Equality; /* ********************************************************************* */ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { @@ -545,8 +542,8 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away // keys - visualSLAM::PoseKey x1(1), x2(2); - visualSLAM::PointKey l1(1), l2(2); + Symbol x1('x',1), x2('x',2); + Symbol l1('l',1), l2('l',2); // create graph VGraph::shared_graph graph(new VGraph()); @@ -567,7 +564,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { Point3 landmark1(0.5, 5.0, 0.0); Point3 landmark2(1.5, 5.0, 0.0); - shared_vconfig initValues(new VValues()); + shared_vconfig initValues(new Values()); initValues->insert(x1, pose1); initValues->insert(x2, pose2); initValues->insert(l1, landmark1); @@ -577,7 +574,7 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { VOptimizer::shared_values actual = VOptimizer::optimizeLM(graph, initValues); // create config - VValues truthValues; + Values truthValues; truthValues.insert(x1, camera1.pose()); truthValues.insert(x2, camera2.pose()); truthValues.insert(l1, landmark); diff --git a/tests/testNonlinearFactor.cpp b/tests/testNonlinearFactor.cpp index 095f562a4..d32a50fe5 100644 --- a/tests/testNonlinearFactor.cpp +++ b/tests/testNonlinearFactor.cpp @@ -25,9 +25,6 @@ // TODO: DANGEROUS, create shared pointers #define GTSAM_MAGIC_GAUSSIAN 2 -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include #include #include @@ -40,7 +37,10 @@ using namespace std; using namespace gtsam; using namespace example; -typedef boost::shared_ptr > shared_nlf; +using simulated2D::PoseKey; +using simulated2D::PointKey; + +typedef boost::shared_ptr shared_nlf; /* ************************************************************************* */ TEST( NonlinearFactor, equals ) @@ -49,11 +49,11 @@ TEST( NonlinearFactor, equals ) // create two nonlinear2 factors Point2 z3(0.,-1.); - simulated2D::Measurement f0(z3, sigma, 1,1); + simulated2D::Measurement f0(z3, sigma, PoseKey(1),PointKey(1)); // measurement between x2 and l1 Point2 z4(-1.5, -1.); - simulated2D::Measurement f1(z4, sigma, 2,1); + simulated2D::Measurement f1(z4, sigma, PoseKey(2),PointKey(1)); CHECK(assert_equal(f0,f0)); CHECK(f0.equals(f0)); @@ -82,18 +82,18 @@ TEST( NonlinearFactor, NonlinearFactor ) Graph fg = createNonlinearFactorGraph(); // create a values structure for the non linear factor graph - example::Values cfg = createNoisyValues(); + Values cfg = createNoisyValues(); // get the factor "f1" from the factor graph Graph::sharedFactor factor = fg[0]; // calculate the error_vector from the factor "f1" // error_vector = [0.1 0.1] - Vector actual_e = boost::dynamic_pointer_cast >(factor)->unwhitenedError(cfg); + Vector actual_e = boost::dynamic_pointer_cast(factor)->unwhitenedError(cfg); CHECK(assert_equal(0.1*ones(2),actual_e)); // error = 0.5 * [1 1] * [1;1] = 1 - double expected = 1.0; + double expected = 1.0; // calculate the error from the factor "f1" double actual = factor->error(cfg); @@ -103,7 +103,7 @@ TEST( NonlinearFactor, NonlinearFactor ) /* ************************************************************************* */ TEST( NonlinearFactor, linearize_f1 ) { - example::Values c = createNoisyValues(); + Values c = createNoisyValues(); // Grab a non-linear factor Graph nfg = createNonlinearFactorGraph(); @@ -125,7 +125,7 @@ TEST( NonlinearFactor, linearize_f1 ) /* ************************************************************************* */ TEST( NonlinearFactor, linearize_f2 ) { - example::Values c = createNoisyValues(); + Values c = createNoisyValues(); // Grab a non-linear factor Graph nfg = createNonlinearFactorGraph(); @@ -148,7 +148,7 @@ TEST( NonlinearFactor, linearize_f3 ) Graph::sharedFactor nlf = nfg[2]; // We linearize at noisy config from SmallExample - example::Values c = createNoisyValues(); + Values c = createNoisyValues(); GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary()); GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary()); @@ -165,7 +165,7 @@ TEST( NonlinearFactor, linearize_f4 ) Graph::sharedFactor nlf = nfg[3]; // We linearize at noisy config from SmallExample - example::Values c = createNoisyValues(); + Values c = createNoisyValues(); GaussianFactor::shared_ptr actual = nlf->linearize(c, *c.orderingArbitrary()); GaussianFactorGraph lfg = createGaussianFactorGraph(*c.orderingArbitrary()); @@ -179,14 +179,14 @@ TEST( NonlinearFactor, size ) { // create a non linear factor graph Graph fg = createNonlinearFactorGraph(); - + // create a values structure for the non linear factor graph - example::Values cfg = createNoisyValues(); - + Values cfg = createNoisyValues(); + // get some factors from the graph Graph::sharedFactor factor1 = fg[0], factor2 = fg[1], factor3 = fg[2]; - + CHECK(factor1->size() == 1); CHECK(factor2->size() == 2); CHECK(factor3->size() == 2); @@ -199,16 +199,16 @@ TEST( NonlinearFactor, linearize_constraint1 ) SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas); Point2 mu(1., -1.); - Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, 1)); + Graph::sharedFactor f0(new simulated2D::Prior(mu, constraint, PoseKey(1))); - example::Values config; + Values config; config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0)); GaussianFactor::shared_ptr actual = f0->linearize(config, *config.orderingArbitrary()); // create expected Ordering ord(*config.orderingArbitrary()); Vector b = Vector_(2, 0., -3.); - JacobianFactor expected(ord["x1"], Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b, constraint); + JacobianFactor expected(ord[PoseKey(1)], Matrix_(2,2, 5.0, 0.0, 0.0, 1.0), b, constraint); CHECK(assert_equal((const GaussianFactor&)expected, *actual)); } @@ -219,9 +219,9 @@ TEST( NonlinearFactor, linearize_constraint2 ) SharedDiagonal constraint = noiseModel::Constrained::MixedSigmas(sigmas); Point2 z3(1.,-1.); - simulated2D::Measurement f0(z3, constraint, 1,1); + simulated2D::Measurement f0(z3, constraint, PoseKey(1),PointKey(1)); - example::Values config; + Values config; config.insert(simulated2D::PoseKey(1), Point2(1.0, 2.0)); config.insert(simulated2D::PointKey(1), Point2(5.0, 4.0)); GaussianFactor::shared_ptr actual = f0.linearize(config, *config.orderingArbitrary()); @@ -230,19 +230,15 @@ TEST( NonlinearFactor, linearize_constraint2 ) Ordering ord(*config.orderingArbitrary()); Matrix A = Matrix_(2,2, 5.0, 0.0, 0.0, 1.0); Vector b = Vector_(2, -15., -3.); - JacobianFactor expected(ord["x1"], -1*A, ord["l1"], A, b, constraint); + JacobianFactor expected(ord[PoseKey(1)], -1*A, ord[PointKey(1)], A, b, constraint); CHECK(assert_equal((const GaussianFactor&)expected, *actual)); } /* ************************************************************************* */ -typedef TypedSymbol TestKey; -typedef gtsam::Values TestValues; - -/* ************************************************************************* */ -class TestFactor4 : public NonlinearFactor4 { +class TestFactor4 : public NoiseModelFactor4 { public: - typedef NonlinearFactor4 Base; - TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4) {} + typedef NoiseModelFactor4 Base; + TestFactor4() : Base(sharedSigmas(Vector_(1, 2.0)), PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4)) {} virtual Vector evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4, @@ -261,16 +257,16 @@ public: }; /* ************************************ */ -TEST(NonlinearFactor, NonlinearFactor4) { +TEST(NonlinearFactor, NoiseModelFactor4) { TestFactor4 tf; - TestValues tv; - tv.insert(1, LieVector(1, 1.0)); - tv.insert(2, LieVector(1, 2.0)); - tv.insert(3, LieVector(1, 3.0)); - tv.insert(4, LieVector(1, 4.0)); + Values tv; + tv.insert(PoseKey(1), LieVector(1, 1.0)); + tv.insert(PoseKey(2), LieVector(1, 2.0)); + tv.insert(PoseKey(3), LieVector(1, 3.0)); + tv.insert(PoseKey(4), LieVector(1, 4.0)); EXPECT(assert_equal(Vector_(1, 10.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); - Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4); + Ordering ordering; ordering += PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv, ordering))); LONGS_EQUAL(jf.keys()[0], 0); LONGS_EQUAL(jf.keys()[1], 1); @@ -284,10 +280,10 @@ TEST(NonlinearFactor, NonlinearFactor4) { } /* ************************************************************************* */ -class TestFactor5 : public NonlinearFactor5 { +class TestFactor5 : public NoiseModelFactor5 { public: - typedef NonlinearFactor5 Base; - TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5) {} + typedef NoiseModelFactor5 Base; + TestFactor5() : Base(sharedSigmas(Vector_(1, 2.0)), PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4), PoseKey(5)) {} virtual Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, @@ -308,17 +304,17 @@ public: }; /* ************************************ */ -TEST(NonlinearFactor, NonlinearFactor5) { +TEST(NonlinearFactor, NoiseModelFactor5) { TestFactor5 tf; - TestValues tv; - tv.insert(1, LieVector(1, 1.0)); - tv.insert(2, LieVector(1, 2.0)); - tv.insert(3, LieVector(1, 3.0)); - tv.insert(4, LieVector(1, 4.0)); - tv.insert(5, LieVector(1, 5.0)); + Values tv; + tv.insert(PoseKey(1), LieVector(1, 1.0)); + tv.insert(PoseKey(2), LieVector(1, 2.0)); + tv.insert(PoseKey(3), LieVector(1, 3.0)); + tv.insert(PoseKey(4), LieVector(1, 4.0)); + tv.insert(PoseKey(5), LieVector(1, 5.0)); EXPECT(assert_equal(Vector_(1, 15.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9); - Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5); + Ordering ordering; ordering += PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4), PoseKey(5); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv, ordering))); LONGS_EQUAL(jf.keys()[0], 0); LONGS_EQUAL(jf.keys()[1], 1); @@ -334,10 +330,10 @@ TEST(NonlinearFactor, NonlinearFactor5) { } /* ************************************************************************* */ -class TestFactor6 : public NonlinearFactor6 { +class TestFactor6 : public NoiseModelFactor6 { public: - typedef NonlinearFactor6 Base; - TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), 1, 2, 3, 4, 5, 6) {} + typedef NoiseModelFactor6 Base; + TestFactor6() : Base(sharedSigmas(Vector_(1, 2.0)), PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4), PoseKey(5), PoseKey(6)) {} virtual Vector evaluateError(const X1& x1, const X2& x2, const X3& x3, const X4& x4, const X5& x5, const X6& x6, @@ -360,18 +356,18 @@ public: }; /* ************************************ */ -TEST(NonlinearFactor, NonlinearFactor6) { +TEST(NonlinearFactor, NoiseModelFactor6) { TestFactor6 tf; - TestValues tv; - tv.insert(1, LieVector(1, 1.0)); - tv.insert(2, LieVector(1, 2.0)); - tv.insert(3, LieVector(1, 3.0)); - tv.insert(4, LieVector(1, 4.0)); - tv.insert(5, LieVector(1, 5.0)); - tv.insert(6, LieVector(1, 6.0)); + Values tv; + tv.insert(PoseKey(1), LieVector(1, 1.0)); + tv.insert(PoseKey(2), LieVector(1, 2.0)); + tv.insert(PoseKey(3), LieVector(1, 3.0)); + tv.insert(PoseKey(4), LieVector(1, 4.0)); + tv.insert(PoseKey(5), LieVector(1, 5.0)); + tv.insert(PoseKey(6), LieVector(1, 6.0)); EXPECT(assert_equal(Vector_(1, 21.0), tf.unwhitenedError(tv))); DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9); - Ordering ordering; ordering += TestKey(1), TestKey(2), TestKey(3), TestKey(4), TestKey(5), TestKey(6); + Ordering ordering; ordering += PoseKey(1), PoseKey(2), PoseKey(3), PoseKey(4), PoseKey(5), PoseKey(6); JacobianFactor jf(*boost::dynamic_pointer_cast(tf.linearize(tv, ordering))); LONGS_EQUAL(jf.keys()[0], 0); LONGS_EQUAL(jf.keys()[1], 1); diff --git a/tests/testNonlinearFactorGraph.cpp b/tests/testNonlinearFactorGraph.cpp index 6b926c6d7..92e569038 100644 --- a/tests/testNonlinearFactorGraph.cpp +++ b/tests/testNonlinearFactorGraph.cpp @@ -27,9 +27,6 @@ using namespace boost::assign; #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include #include #include @@ -39,6 +36,9 @@ using namespace boost::assign; using namespace gtsam; using namespace example; +Key kx(size_t i) { return Symbol('x',i); } +Key kl(size_t i) { return Symbol('l',i); } + /* ************************************************************************* */ TEST( Graph, equals ) { @@ -51,11 +51,11 @@ TEST( Graph, equals ) TEST( Graph, error ) { Graph fg = createNonlinearFactorGraph(); - example::Values c1 = createValues(); + Values c1 = createValues(); double actual1 = fg.error(c1); DOUBLES_EQUAL( 0.0, actual1, 1e-9 ); - example::Values c2 = createNoisyValues(); + Values c2 = createNoisyValues(); double actual2 = fg.error(c2); DOUBLES_EQUAL( 5.625, actual2, 1e-9 ); } @@ -64,19 +64,23 @@ TEST( Graph, error ) TEST( Graph, keys ) { Graph fg = createNonlinearFactorGraph(); - set actual = fg.keys(); + set actual = fg.keys(); LONGS_EQUAL(3, actual.size()); - set::const_iterator it = actual.begin(); - CHECK(assert_equal(Symbol('l', 1), *(it++))); - CHECK(assert_equal(Symbol('x', 1), *(it++))); - CHECK(assert_equal(Symbol('x', 2), *(it++))); + set::const_iterator it = actual.begin(); + LONGS_EQUAL(kl(1), *(it++)); + LONGS_EQUAL(kx(1), *(it++)); + LONGS_EQUAL(kx(2), *(it++)); } /* ************************************************************************* */ TEST( Graph, GET_ORDERING) { - Ordering expected; expected += "x1","l1","x2"; +// Ordering expected; expected += "x1","l1","x2"; // For starting with x1,x2,l1 + Ordering expected; expected += kl(1), kx(2), kx(1); // For starting with l1,x1,x2 Graph nlfg = createNonlinearFactorGraph(); + SymbolicFactorGraph::shared_ptr symbolic; + Ordering::shared_ptr ordering; + boost::tie(symbolic, ordering) = nlfg.symbolic(createNoisyValues()); Ordering actual = *nlfg.orderingCOLAMD(createNoisyValues()); CHECK(assert_equal(expected,actual)); } @@ -85,7 +89,7 @@ TEST( Graph, GET_ORDERING) TEST( Graph, probPrime ) { Graph fg = createNonlinearFactorGraph(); - example::Values cfg = createValues(); + Values cfg = createValues(); // evaluate the probability of the factor graph double actual = fg.probPrime(cfg); @@ -97,7 +101,7 @@ TEST( Graph, probPrime ) TEST( Graph, linearize ) { Graph fg = createNonlinearFactorGraph(); - example::Values initial = createNoisyValues(); + Values initial = createNoisyValues(); boost::shared_ptr > linearized = fg.linearize(initial, *initial.orderingArbitrary()); FactorGraph expected = createGaussianFactorGraph(*initial.orderingArbitrary()); CHECK(assert_equal(expected,*linearized)); // Needs correct linearizations diff --git a/tests/testNonlinearISAM.cpp b/tests/testNonlinearISAM.cpp index b90e98625..80ed7b6b6 100644 --- a/tests/testNonlinearISAM.cpp +++ b/tests/testNonlinearISAM.cpp @@ -12,7 +12,7 @@ using namespace gtsam; using namespace planarSLAM; -typedef NonlinearISAM PlanarISAM; +typedef NonlinearISAM<> PlanarISAM; const double tol=1e-5; @@ -25,15 +25,14 @@ TEST(testNonlinearISAM, markov_chain ) { Sampler sampler(model, 42u); // create initial graph - PoseKey key(0); Pose2 cur_pose; // start at origin Graph start_factors; - start_factors.addPoseConstraint(key, cur_pose); + start_factors.addPoseConstraint(0, cur_pose); planarSLAM::Values init; planarSLAM::Values expected; - init.insert(key, cur_pose); - expected.insert(key, cur_pose); + init.insertPose(0, cur_pose); + expected.insertPose(0, cur_pose); isam.update(start_factors, init); // loop for a period of time to verify memory usage @@ -41,8 +40,7 @@ TEST(testNonlinearISAM, markov_chain ) { Pose2 z(1.0, 2.0, 0.1); for (size_t i=1; i<=nrPoses; ++i) { Graph new_factors; - PoseKey key1(i-1), key2(i); - new_factors.addOdometry(key1, key2, z, model); + new_factors.addOdometry(i-1, i, z, model); planarSLAM::Values new_init; // perform a check on changing orderings @@ -50,8 +48,8 @@ TEST(testNonlinearISAM, markov_chain ) { Ordering ordering = isam.getOrdering(); // swap last two elements - Symbol last = ordering.pop_back().first; - Symbol secondLast = ordering.pop_back().first; + Key last = ordering.pop_back().first; + Key secondLast = ordering.pop_back().first; ordering.push_back(last); ordering.push_back(secondLast); isam.setOrdering(ordering); @@ -61,16 +59,15 @@ TEST(testNonlinearISAM, markov_chain ) { } cur_pose = cur_pose.compose(z); - new_init.insert(key2, cur_pose.retract(sampler.sample())); - expected.insert(key2, cur_pose); + new_init.insertPose(i, cur_pose.retract(sampler.sample())); + expected.insertPose(i, cur_pose); isam.update(new_factors, new_init); } // verify values - all but the last one should be very close planarSLAM::Values actual = isam.estimate(); for (size_t i=0; i using namespace boost; -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include #include #include @@ -44,7 +41,10 @@ using namespace gtsam; const double tol = 1e-5; -typedef NonlinearOptimizer Optimizer; +Key kx(size_t i) { return Symbol('x',i); } +Key kl(size_t i) { return Symbol('l',i); } + +typedef NonlinearOptimizer Optimizer; /* ************************************************************************* */ TEST( NonlinearOptimizer, linearizeAndOptimizeForDelta ) @@ -55,20 +55,20 @@ TEST( NonlinearOptimizer, linearizeAndOptimizeForDelta ) // Expected values structure is the difference between the noisy config // and the ground-truth config. One step only because it's linear ! - Ordering ord1; ord1 += "x2","l1","x1"; + Ordering ord1; ord1 += kx(2),kl(1),kx(1); VectorValues expected(initial->dims(ord1)); Vector dl1(2); dl1(0) = -0.1; dl1(1) = 0.1; - expected[ord1["l1"]] = dl1; + expected[ord1[kl(1)]] = dl1; Vector dx1(2); dx1(0) = -0.1; dx1(1) = -0.1; - expected[ord1["x1"]] = dx1; + expected[ord1[kx(1)]] = dx1; Vector dx2(2); dx2(0) = 0.1; dx2(1) = -0.2; - expected[ord1["x2"]] = dx2; + expected[ord1[kx(2)]] = dx2; // Check one ordering Optimizer optimizer1(fg, initial, Optimizer::shared_ordering(new Ordering(ord1))); @@ -78,7 +78,7 @@ TEST( NonlinearOptimizer, linearizeAndOptimizeForDelta ) // SL-FIX // Check another // shared_ptr ord2(new Ordering()); -// *ord2 += "x1","x2","l1"; +// *ord2 += kx(1),kx(2),kl(1); // solver = Optimizer::shared_solver(new Optimizer::solver(ord2)); // Optimizer optimizer2(fg, initial, solver); // @@ -87,7 +87,7 @@ TEST( NonlinearOptimizer, linearizeAndOptimizeForDelta ) // // // And yet another... // shared_ptr ord3(new Ordering()); -// *ord3 += "l1","x1","x2"; +// *ord3 += kl(1),kx(1),kx(2); // solver = Optimizer::shared_solver(new Optimizer::solver(ord3)); // Optimizer optimizer3(fg, initial, solver); // @@ -96,7 +96,7 @@ TEST( NonlinearOptimizer, linearizeAndOptimizeForDelta ) // // // More... // shared_ptr ord4(new Ordering()); -// *ord4 += "x1","x2", "l1"; +// *ord4 += kx(1),kx(2), kl(1); // solver = Optimizer::shared_solver(new Optimizer::solver(ord4)); // Optimizer optimizer4(fg, initial, solver); // @@ -113,12 +113,12 @@ TEST( NonlinearOptimizer, iterateLM ) // config far from minimum Point2 x0(3,0); - boost::shared_ptr config(new example::Values); + boost::shared_ptr config(new Values); config->insert(simulated2D::PoseKey(1), x0); // ordering shared_ptr ord(new Ordering()); - ord->push_back("x1"); + ord->push_back(kx(1)); // create initial optimization state, with lambda=0 Optimizer optimizer(fg, config, ord, NonlinearOptimizationParameters::newLambda(0.)); @@ -149,19 +149,19 @@ TEST( NonlinearOptimizer, optimize ) // test error at minimum Point2 xstar(0,0); - example::Values cstar; + Values cstar; cstar.insert(simulated2D::PoseKey(1), xstar); DOUBLES_EQUAL(0.0,fg->error(cstar),0.0); // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = Point2 x0(3,3); - boost::shared_ptr c0(new example::Values); + boost::shared_ptr c0(new Values); c0->insert(simulated2D::PoseKey(1), x0); DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3); // optimize parameters shared_ptr ord(new Ordering()); - ord->push_back("x1"); + ord->push_back(kx(1)); // initial optimization state is the same in both cases tested boost::shared_ptr params = boost::make_shared(); @@ -185,7 +185,7 @@ TEST( NonlinearOptimizer, SimpleLMOptimizer ) example::createReallyNonlinearFactorGraph())); Point2 x0(3,3); - boost::shared_ptr c0(new example::Values); + boost::shared_ptr c0(new Values); c0->insert(simulated2D::PoseKey(1), x0); Optimizer::shared_values actual = Optimizer::optimizeLM(fg, c0); @@ -198,7 +198,7 @@ TEST( NonlinearOptimizer, SimpleLMOptimizer_noshared ) example::Graph fg = example::createReallyNonlinearFactorGraph(); Point2 x0(3,3); - example::Values c0; + Values c0; c0.insert(simulated2D::PoseKey(1), x0); Optimizer::shared_values actual = Optimizer::optimizeLM(fg, c0); @@ -212,7 +212,7 @@ TEST( NonlinearOptimizer, SimpleGNOptimizer ) example::createReallyNonlinearFactorGraph())); Point2 x0(3,3); - boost::shared_ptr c0(new example::Values); + boost::shared_ptr c0(new Values); c0->insert(simulated2D::PoseKey(1), x0); Optimizer::shared_values actual = Optimizer::optimizeGN(fg, c0); @@ -225,7 +225,7 @@ TEST( NonlinearOptimizer, SimpleGNOptimizer_noshared ) example::Graph fg = example::createReallyNonlinearFactorGraph(); Point2 x0(3,3); - example::Values c0; + Values c0; c0.insert(simulated2D::PoseKey(1), x0); Optimizer::shared_values actual = Optimizer::optimizeGN(fg, c0); @@ -238,14 +238,14 @@ TEST( NonlinearOptimizer, optimization_method ) example::Graph fg = example::createReallyNonlinearFactorGraph(); Point2 x0(3,3); - example::Values c0; + Values c0; c0.insert(simulated2D::PoseKey(1), x0); - example::Values actualMFQR = optimize( + Values actualMFQR = optimize( fg, c0, *NonlinearOptimizationParameters().newFactorization(true), MULTIFRONTAL, LM); DOUBLES_EQUAL(0,fg.error(actualMFQR),tol); - example::Values actualMFLDL = optimize( + Values actualMFLDL = optimize( fg, c0, *NonlinearOptimizationParameters().newFactorization(false), MULTIFRONTAL, LM); DOUBLES_EQUAL(0,fg.error(actualMFLDL),tol); } @@ -253,11 +253,11 @@ TEST( NonlinearOptimizer, optimization_method ) /* ************************************************************************* */ TEST( NonlinearOptimizer, Factorization ) { - typedef NonlinearOptimizer Optimizer; + typedef NonlinearOptimizer Optimizer; - boost::shared_ptr config(new pose2SLAM::Values); - config->insert(1, Pose2(0.,0.,0.)); - config->insert(2, Pose2(1.5,0.,0.)); + boost::shared_ptr config(new Values); + config->insert(pose2SLAM::PoseKey(1), Pose2(0.,0.,0.)); + config->insert(pose2SLAM::PoseKey(2), Pose2(1.5,0.,0.)); boost::shared_ptr graph(new pose2SLAM::Graph); graph->addPrior(1, Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)); @@ -270,9 +270,9 @@ TEST( NonlinearOptimizer, Factorization ) Optimizer optimizer(graph, config, ordering); Optimizer optimized = optimizer.iterateLM(); - pose2SLAM::Values expected; - expected.insert(1, Pose2(0.,0.,0.)); - expected.insert(2, Pose2(1.,0.,0.)); + Values expected; + expected.insert(pose2SLAM::PoseKey(1), Pose2(0.,0.,0.)); + expected.insert(pose2SLAM::PoseKey(2), Pose2(1.,0.,0.)); CHECK(assert_equal(expected, *optimized.values(), 1e-5)); } @@ -287,19 +287,19 @@ TEST_UNSAFE(NonlinearOptimizer, NullFactor) { // test error at minimum Point2 xstar(0,0); - example::Values cstar; + Values cstar; cstar.insert(simulated2D::PoseKey(1), xstar); DOUBLES_EQUAL(0.0,fg->error(cstar),0.0); // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 = Point2 x0(3,3); - boost::shared_ptr c0(new example::Values); + boost::shared_ptr c0(new Values); c0->insert(simulated2D::PoseKey(1), x0); DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3); // optimize parameters shared_ptr ord(new Ordering()); - ord->push_back("x1"); + ord->push_back(kx(1)); // initial optimization state is the same in both cases tested boost::shared_ptr params = boost::make_shared(); @@ -367,7 +367,7 @@ TEST_UNSAFE(NonlinearOptimizer, NullFactor) { // // // Check one ordering // shared_ptr ord1(new Ordering()); -// *ord1 += "x2","l1","x1"; +// *ord1 += kx(2),kl(1),kx(1); // solver = Optimizer::shared_solver(new Optimizer::solver(ord1)); // Optimizer optimizer1(fg, initial, solver); // diff --git a/tests/testPose2SLAMwSPCG.cpp b/tests/testPose2SLAMwSPCG.cpp index c345b37b2..439228c79 100644 --- a/tests/testPose2SLAMwSPCG.cpp +++ b/tests/testPose2SLAMwSPCG.cpp @@ -14,7 +14,6 @@ using namespace pose2SLAM; const double tol = 1e-5; -#if ENABLE_SPCG /* ************************************************************************* */ TEST(testPose2SLAMwSPCG, example1) { @@ -41,7 +40,7 @@ TEST(testPose2SLAMwSPCG, example1) { graph.addConstraint(x6,x9,Pose2(2,0,0),sigma) ; graph.addPrior(x1, Pose2(0,0,0), sigma) ; - pose2SLAM::Values initial; + Values initial; initial.insert(x1, Pose2( 0, 0, 0)); initial.insert(x2, Pose2( 0, 2.1, 0.01)); initial.insert(x3, Pose2( 0, 3.9,-0.01)); @@ -52,7 +51,7 @@ TEST(testPose2SLAMwSPCG, example1) { initial.insert(x8, Pose2(3.9, 2.1, 0.01)); initial.insert(x9, Pose2(4.1, 3.9,-0.01)); - pose2SLAM::Values expected; + Values expected; expected.insert(x1, Pose2(0.0, 0.0, 0.0)); expected.insert(x2, Pose2(0.0, 2.0, 0.0)); expected.insert(x3, Pose2(0.0, 4.0, 0.0)); @@ -63,11 +62,10 @@ TEST(testPose2SLAMwSPCG, example1) { expected.insert(x8, Pose2(4.0, 2.0, 0.0)); expected.insert(x9, Pose2(4.0, 4.0, 0.0)); - pose2SLAM::Values actual = optimizeSPCG(graph, initial); + Values actual = optimizeSPCG(graph, initial); EXPECT(assert_equal(expected, actual, tol)); } -#endif /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } diff --git a/tests/testRot3Optimization.cpp b/tests/testRot3Optimization.cpp index a35165273..105bc8331 100644 --- a/tests/testRot3Optimization.cpp +++ b/tests/testRot3Optimization.cpp @@ -23,35 +23,32 @@ #include #include #include -#include #include #include #include using namespace gtsam; -typedef TypedSymbol Key; -typedef Values Rot3Values; -typedef PriorFactor Prior; -typedef BetweenFactor Between; -typedef NonlinearFactorGraph Graph; +typedef PriorFactor Prior; +typedef BetweenFactor Between; +typedef NonlinearFactorGraph Graph; /* ************************************************************************* */ TEST(Rot3, optimize) { // Optimize a circle - Rot3Values truth; - Rot3Values initial; + Values truth; + Values initial; Graph fg; - fg.add(Prior(0, Rot3(), sharedSigma(3, 0.01))); + fg.add(Prior(Symbol('r',0), Rot3(), sharedSigma(3, 0.01))); for(int j=0; j<6; ++j) { - truth.insert(j, Rot3::Rz(M_PI/3.0 * double(j))); - initial.insert(j, Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2))); - fg.add(Between(j, (j+1)%6, Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01))); + truth.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j))); + initial.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2))); + fg.add(Between(Symbol('r',j), Symbol('r',(j+1)%6), Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01))); } NonlinearOptimizationParameters params; - Rot3Values final = optimize(fg, initial, params); + Values final = optimize(fg, initial, params); EXPECT(assert_equal(truth, final, 1e-5)); } diff --git a/tests/testSerialization.cpp b/tests/testSerialization.cpp deleted file mode 100644 index 72132deb6..000000000 --- a/tests/testSerialization.cpp +++ /dev/null @@ -1,553 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @brief Unit tests for serialization of library classes - * @author Frank Dellaert - * @author Alex Cunningham - */ - -/* ************************************************************************* */ -// Serialization testing code. -/* ************************************************************************* */ - -#include -#include - -// includes for standard serialization types -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -// whether to print the serialized text to stdout -const bool verbose = false; - -template -std::string serialize(const T& input) { - std::ostringstream out_archive_stream; - boost::archive::text_oarchive out_archive(out_archive_stream); - out_archive << input; - return out_archive_stream.str(); -} - -template -void deserialize(const std::string& serialized, T& output) { - std::istringstream in_archive_stream(serialized); - boost::archive::text_iarchive in_archive(in_archive_stream); - in_archive >> output; -} - -// Templated round-trip serialization -template -void roundtrip(const T& input, T& output) { - // Serialize - std::string serialized = serialize(input); - if (verbose) std::cout << serialized << std::endl << std::endl; - - deserialize(serialized, output); -} - -// This version requires equality operator -template -bool equality(const T& input = T()) { - T output; - roundtrip(input,output); - return input==output; -} - -// This version requires equals -template -bool equalsObj(const T& input = T()) { - T output; - roundtrip(input,output); - return input.equals(output); -} - -// De-referenced version for pointers -template -bool equalsDereferenced(const T& input) { - T output; - roundtrip(input,output); - return input->equals(*output); -} - -/* ************************************************************************* */ -template -std::string serializeXML(const T& input) { - std::ostringstream out_archive_stream; - boost::archive::xml_oarchive out_archive(out_archive_stream); - out_archive << boost::serialization::make_nvp("data", input); - return out_archive_stream.str(); -} - -template -void deserializeXML(const std::string& serialized, T& output) { - std::istringstream in_archive_stream(serialized); - boost::archive::xml_iarchive in_archive(in_archive_stream); - in_archive >> boost::serialization::make_nvp("data", output); -} - -// Templated round-trip serialization using XML -template -void roundtripXML(const T& input, T& output) { - // Serialize - std::string serialized = serializeXML(input); - if (verbose) std::cout << serialized << std::endl << std::endl; - - // De-serialize - deserializeXML(serialized, output); -} - -// This version requires equality operator -template -bool equalityXML(const T& input = T()) { - T output; - roundtripXML(input,output); - return input==output; -} - -// This version requires equals -template -bool equalsXML(const T& input = T()) { - T output; - roundtripXML(input,output); - return input.equals(output); -} - -// This version is for pointers -template -bool equalsDereferencedXML(const T& input = T()) { - T output; - roundtripXML(input,output); - return input->equals(*output); -} - -/* ************************************************************************* */ -// Actual Tests -/* ************************************************************************* */ - -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -using namespace std; -using namespace gtsam; - -/* ************************************************************************* */ -TEST (Serialization, matrix_vector) { - EXPECT(equality(Vector_(4, 1.0, 2.0, 3.0, 4.0))); - EXPECT(equality(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0))); - - EXPECT(equalityXML(Vector_(4, 1.0, 2.0, 3.0, 4.0))); - EXPECT(equalityXML(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0))); -} - -Point3 pt3(1.0, 2.0, 3.0); -Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); -Pose3 pose3(rt3, pt3); - -Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5); -Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0); -Cal3Bundler cal3(1.0, 2.0, 3.0); -Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); -Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4)); -CalibratedCamera cal5(Pose3(rt3, pt3)); - -PinholeCamera cam1(pose3, cal1); -StereoCamera cam2(pose3, cal4ptr); -StereoPoint2 spt(1.0, 2.0, 3.0); - - -/* ************************************************************************* */ -TEST (Serialization, text_geometry) { - EXPECT(equalsObj(Point2(1.0, 2.0))); - EXPECT(equalsObj(Pose2(1.0, 2.0, 0.3))); - EXPECT(equalsObj(Rot2::fromDegrees(30.0))); - - EXPECT(equalsObj(pt3)); - EXPECT(equalsObj(rt3)); - EXPECT(equalsObj(Pose3(rt3, pt3))); - - EXPECT(equalsObj(cal1)); - EXPECT(equalsObj(cal2)); - EXPECT(equalsObj(cal3)); - EXPECT(equalsObj(cal4)); - EXPECT(equalsObj(cal5)); - - EXPECT(equalsObj(cam1)); - EXPECT(equalsObj(cam2)); - EXPECT(equalsObj(spt)); -} - -/* ************************************************************************* */ -TEST (Serialization, xml_geometry) { - EXPECT(equalsXML(Point2(1.0, 2.0))); - EXPECT(equalsXML(Pose2(1.0, 2.0, 0.3))); - EXPECT(equalsXML(Rot2::fromDegrees(30.0))); - - EXPECT(equalsXML(pt3)); - EXPECT(equalsXML(rt3)); - EXPECT(equalsXML(Pose3(rt3, pt3))); - - EXPECT(equalsXML(cal1)); - EXPECT(equalsXML(cal2)); - EXPECT(equalsXML(cal3)); - EXPECT(equalsXML(cal4)); - EXPECT(equalsXML(cal5)); - - EXPECT(equalsXML(cam1)); - EXPECT(equalsXML(cam2)); - EXPECT(equalsXML(spt)); -} - -/* ************************************************************************* */ -// Export Noisemodels -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); - -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); - -/* ************************************************************************* */ -// example noise models -noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3)); -noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3)); -noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2); -noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1)); -noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3); - -/* ************************************************************************* */ -TEST (Serialization, noiseModels) { - // tests using pointers to the derived class - EXPECT( equalsDereferenced(diag3)); - EXPECT(equalsDereferencedXML(diag3)); - - EXPECT( equalsDereferenced(gaussian3)); - EXPECT(equalsDereferencedXML(gaussian3)); - - EXPECT( equalsDereferenced(iso3)); - EXPECT(equalsDereferencedXML(iso3)); - - EXPECT( equalsDereferenced(constrained3)); - EXPECT(equalsDereferencedXML(constrained3)); - - EXPECT( equalsDereferenced(unit3)); - EXPECT(equalsDereferencedXML(unit3)); -} - -/* ************************************************************************* */ -TEST (Serialization, SharedNoiseModel_noiseModels) { - SharedNoiseModel diag3_sg = diag3; - EXPECT(equalsDereferenced(diag3_sg)); - EXPECT(equalsDereferencedXML(diag3_sg)); - - EXPECT(equalsDereferenced(diag3)); - EXPECT(equalsDereferencedXML(diag3)); - - EXPECT(equalsDereferenced(iso3)); - EXPECT(equalsDereferencedXML(iso3)); - - EXPECT(equalsDereferenced(gaussian3)); - EXPECT(equalsDereferencedXML(gaussian3)); - - EXPECT(equalsDereferenced(unit3)); - EXPECT(equalsDereferencedXML(unit3)); - - EXPECT(equalsDereferenced(constrained3)); - EXPECT(equalsDereferencedXML(constrained3)); -} - -/* ************************************************************************* */ -TEST (Serialization, SharedDiagonal_noiseModels) { - EXPECT(equalsDereferenced(diag3)); - EXPECT(equalsDereferencedXML(diag3)); - - EXPECT(equalsDereferenced(iso3)); - EXPECT(equalsDereferencedXML(iso3)); - - EXPECT(equalsDereferenced(unit3)); - EXPECT(equalsDereferencedXML(unit3)); - - EXPECT(equalsDereferenced(constrained3)); - EXPECT(equalsDereferencedXML(constrained3)); -} - -/* ************************************************************************* */ -// Linear components -/* ************************************************************************* */ - -/* ************************************************************************* */ -TEST (Serialization, linear_factors) { - VectorValues values; - values.insert(0, Vector_(1, 1.0)); - values.insert(1, Vector_(2, 2.0,3.0)); - values.insert(2, Vector_(2, 4.0,5.0)); - EXPECT(equalsObj(values)); - EXPECT(equalsXML(values)); - - Index i1 = 4, i2 = 7; - Matrix A1 = eye(3), A2 = -1.0 * eye(3); - Vector b = ones(3); - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1.0, 2.0, 3.0)); - JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model); - EXPECT(equalsObj(jacobianfactor)); - EXPECT(equalsXML(jacobianfactor)); - - HessianFactor hessianfactor(jacobianfactor); - EXPECT(equalsObj(hessianfactor)); - EXPECT(equalsXML(hessianfactor)); -} - -/* ************************************************************************* */ -TEST (Serialization, gaussian_conditional) { - Matrix A1 = Matrix_(2,2, 1., 2., 3., 4.); - Matrix A2 = Matrix_(2,2, 6., 0.2, 8., 0.4); - Matrix R = Matrix_(2,2, 0.1, 0.3, 0.0, 0.34); - Vector d(2); d << 0.2, 0.5; - GaussianConditional cg(0, d, R, 1, A1, 2, A2, ones(2)); - - EXPECT(equalsObj(cg)); - EXPECT(equalsXML(cg)); -} - -/* Create GUIDs for factors */ -/* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); -/* ************************************************************************* */ -TEST (Serialization, smallExample_linear) { - using namespace example; - - Ordering ordering; ordering += "x1","x2","l1"; - GaussianFactorGraph fg = createGaussianFactorGraph(ordering); - EXPECT(equalsObj(ordering)); - EXPECT(equalsXML(ordering)); - - EXPECT(equalsObj(fg)); - EXPECT(equalsXML(fg)); - - GaussianBayesNet cbn = createSmallGaussianBayesNet(); - EXPECT(equalsObj(cbn)); - EXPECT(equalsXML(cbn)); -} - -/* ************************************************************************* */ -TEST (Serialization, symbolic_graph) { - Ordering o; o += "x1","l1","x2"; - // construct expected symbolic graph - SymbolicFactorGraph sfg; - sfg.push_factor(o["x1"]); - sfg.push_factor(o["x1"],o["x2"]); - sfg.push_factor(o["x1"],o["l1"]); - sfg.push_factor(o["l1"],o["x2"]); - - EXPECT(equalsObj(sfg)); - EXPECT(equalsXML(sfg)); -} - -/* ************************************************************************* */ -TEST (Serialization, symbolic_bn) { - Ordering o; o += "x2","l1","x1"; - - IndexConditional::shared_ptr x2(new IndexConditional(o["x2"], o["l1"], o["x1"])); - IndexConditional::shared_ptr l1(new IndexConditional(o["l1"], o["x1"])); - IndexConditional::shared_ptr x1(new IndexConditional(o["x1"])); - - SymbolicBayesNet sbn; - sbn.push_back(x2); - sbn.push_back(l1); - sbn.push_back(x1); - - EXPECT(equalsObj(sbn)); - EXPECT(equalsXML(sbn)); -} - -/* ************************************************************************* */ -TEST (Serialization, symbolic_bayes_tree ) { - typedef BayesTree SymbolicBayesTree; - static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5; - IndexConditional::shared_ptr - B(new IndexConditional(_B_)), - L(new IndexConditional(_L_, _B_)), - E(new IndexConditional(_E_, _L_, _B_)), - S(new IndexConditional(_S_, _L_, _B_)), - T(new IndexConditional(_T_, _E_, _L_)), - X(new IndexConditional(_X_, _E_)); - - // Bayes Tree for Asia example - SymbolicBayesTree bayesTree; - SymbolicBayesTree::insert(bayesTree, B); - SymbolicBayesTree::insert(bayesTree, L); - SymbolicBayesTree::insert(bayesTree, E); - SymbolicBayesTree::insert(bayesTree, S); - SymbolicBayesTree::insert(bayesTree, T); - SymbolicBayesTree::insert(bayesTree, X); - - EXPECT(equalsObj(bayesTree)); - EXPECT(equalsXML(bayesTree)); -} - -/* ************************************************************************* */ -TEST (Serialization, gaussianISAM) { - using namespace example; - Ordering ordering; - GaussianFactorGraph smoother; - boost::tie(smoother, ordering) = createSmoother(7); - BayesTree bayesTree = *GaussianMultifrontalSolver(smoother).eliminate(); - GaussianISAM isam(bayesTree); - - EXPECT(equalsObj(isam)); - EXPECT(equalsXML(isam)); -} - -/* ************************************************************************* */ -/* Create GUIDs for factors in simulated2D */ -BOOST_CLASS_EXPORT_GUID(simulated2D::Prior, "gtsam::simulated2D::Prior" ); -BOOST_CLASS_EXPORT_GUID(simulated2D::Odometry, "gtsam::simulated2D::Odometry" ); -BOOST_CLASS_EXPORT_GUID(simulated2D::Measurement, "gtsam::simulated2D::Measurement"); -/* ************************************************************************* */ -TEST (Serialization, smallExample) { - using namespace example; - Graph nfg = createNonlinearFactorGraph(); - example::Values c1 = createValues(); - EXPECT(equalsObj(nfg)); - EXPECT(equalsXML(nfg)); - - EXPECT(equalsObj(c1)); - EXPECT(equalsXML(c1)); -} - -/* ************************************************************************* */ -/* Create GUIDs for factors */ -BOOST_CLASS_EXPORT_GUID(planarSLAM::Prior, "gtsam::planarSLAM::Prior"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Bearing, "gtsam::planarSLAM::Bearing"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Range, "gtsam::planarSLAM::Range"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::BearingRange,"gtsam::planarSLAM::BearingRange"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Odometry, "gtsam::planarSLAM::Odometry"); -BOOST_CLASS_EXPORT_GUID(planarSLAM::Constraint, "gtsam::planarSLAM::Constraint"); -/* ************************************************************************* */ -TEST (Serialization, planar_system) { - using namespace planarSLAM; - planarSLAM::Values values; - values.insert(PointKey(3), Point2(1.0, 2.0)); - values.insert(PoseKey(4), Pose2(1.0, 2.0, 0.3)); - - SharedNoiseModel model1 = noiseModel::Isotropic::Sigma(1, 0.3); - SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3); - SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3); - - Prior prior(PoseKey(3), Pose2(0.1,-0.3, 0.2), model1); - Bearing bearing(PoseKey(3), PointKey(5), Rot2::fromDegrees(0.5), model1); - Range range(PoseKey(2), PointKey(9), 7.0, model1); - BearingRange bearingRange(PoseKey(2), PointKey(3), Rot2::fromDegrees(0.6), 2.0, model2); - Odometry odometry(PoseKey(2), PoseKey(3), Pose2(1.0, 2.0, 0.3), model3); - Constraint constraint(PoseKey(9), Pose2(2.0,-1.0, 0.2)); - - Graph graph; - graph.add(prior); - graph.add(bearing); - graph.add(range); - graph.add(bearingRange); - graph.add(odometry); - graph.add(constraint); - - // text - EXPECT(equalsObj(PoseKey(2))); - EXPECT(equalsObj(PointKey(3))); - EXPECT(equalsObj(values)); - EXPECT(equalsObj(prior)); - EXPECT(equalsObj(bearing)); - EXPECT(equalsObj(bearingRange)); - EXPECT(equalsObj(range)); - EXPECT(equalsObj(odometry)); - EXPECT(equalsObj(constraint)); - EXPECT(equalsObj(graph)); - - // xml - EXPECT(equalsXML(PoseKey(2))); - EXPECT(equalsXML(PointKey(3))); - EXPECT(equalsXML(values)); - EXPECT(equalsXML(prior)); - EXPECT(equalsXML(bearing)); - EXPECT(equalsXML(bearingRange)); - EXPECT(equalsXML(range)); - EXPECT(equalsXML(odometry)); - EXPECT(equalsXML(constraint)); - EXPECT(equalsXML(graph)); -} - -/* ************************************************************************* */ -/* Create GUIDs for factors */ -BOOST_CLASS_EXPORT_GUID(visualSLAM::PoseConstraint, "gtsam::visualSLAM::PoseConstraint"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::PointConstraint, "gtsam::visualSLAM::PointConstraint"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::PosePrior, "gtsam::visualSLAM::PosePrior"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::PointPrior, "gtsam::visualSLAM::PointPrior"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::ProjectionFactor,"gtsam::visualSLAM::ProjectionFactor"); -BOOST_CLASS_EXPORT_GUID(visualSLAM::StereoFactor, "gtsam::visualSLAM::StereoFactor"); - -/* ************************************************************************* */ -TEST (Serialization, visual_system) { - using namespace visualSLAM; - visualSLAM::Values values; - PoseKey x1(1), x2(2); - PointKey l1(1), l2(2); - Pose3 pose1 = pose3, pose2 = pose3.inverse(); - Point3 pt1(1.0, 2.0, 3.0), pt2(4.0, 5.0, 6.0); - values.insert(x1, pose1); - values.insert(l1, pt1); - SharedNoiseModel model2 = noiseModel::Isotropic::Sigma(2, 0.3); - SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.3); - SharedNoiseModel model6 = noiseModel::Isotropic::Sigma(6, 0.3); - boost::shared_ptr K(new Cal3_S2(cal1)); - - Graph graph; - graph.addMeasurement(Point2(1.0, 2.0), model2, x1, l1, K); - graph.addPointConstraint(1, pt1); - graph.addPointPrior(1, pt2, model3); - graph.addPoseConstraint(1, pose1); - graph.addPosePrior(1, pose2, model6); - - EXPECT(equalsObj(values)); - EXPECT(equalsObj(graph)); - - EXPECT(equalsXML(values)); - EXPECT(equalsXML(graph)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/tests/testSymbolicBayesNet.cpp b/tests/testSymbolicBayesNet.cpp index de51cb8dc..200530e6d 100644 --- a/tests/testSymbolicBayesNet.cpp +++ b/tests/testSymbolicBayesNet.cpp @@ -21,9 +21,6 @@ using namespace boost::assign; #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include #include #include @@ -34,6 +31,9 @@ using namespace std; using namespace gtsam; using namespace example; +Key kx(size_t i) { return Symbol('x',i); } +Key kl(size_t i) { return Symbol('l',i); } + //Symbol _B_('B', 0), _L_('L', 0); //IndexConditional::shared_ptr // B(new IndexConditional(_B_)), @@ -42,12 +42,12 @@ using namespace example; /* ************************************************************************* */ TEST( SymbolicBayesNet, constructor ) { - Ordering o; o += "x2","l1","x1"; + Ordering o; o += kx(2),kl(1),kx(1); // Create manually IndexConditional::shared_ptr - x2(new IndexConditional(o["x2"],o["l1"], o["x1"])), - l1(new IndexConditional(o["l1"],o["x1"])), - x1(new IndexConditional(o["x1"])); + x2(new IndexConditional(o[kx(2)],o[kl(1)], o[kx(1)])), + l1(new IndexConditional(o[kl(1)],o[kx(1)])), + x1(new IndexConditional(o[kx(1)])); BayesNet expected; expected.push_back(x2); expected.push_back(l1); diff --git a/tests/testSymbolicFactorGraph.cpp b/tests/testSymbolicFactorGraph.cpp index 01515f079..f8fa777d1 100644 --- a/tests/testSymbolicFactorGraph.cpp +++ b/tests/testSymbolicFactorGraph.cpp @@ -20,9 +20,6 @@ using namespace boost::assign; #include -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include #include #include @@ -33,16 +30,19 @@ using namespace std; using namespace gtsam; using namespace example; +Key kx(size_t i) { return Symbol('x',i); } +Key kl(size_t i) { return Symbol('l',i); } + /* ************************************************************************* */ TEST( SymbolicFactorGraph, symbolicFactorGraph ) { - Ordering o; o += "x1","l1","x2"; + Ordering o; o += kx(1),kl(1),kx(2); // construct expected symbolic graph SymbolicFactorGraph expected; - expected.push_factor(o["x1"]); - expected.push_factor(o["x1"],o["x2"]); - expected.push_factor(o["x1"],o["l1"]); - expected.push_factor(o["x2"],o["l1"]); + expected.push_factor(o[kx(1)]); + expected.push_factor(o[kx(1)],o[kx(2)]); + expected.push_factor(o[kx(1)],o[kl(1)]); + expected.push_factor(o[kx(2)],o[kl(1)]); // construct it from the factor graph GaussianFactorGraph factorGraph = createGaussianFactorGraph(o); @@ -59,7 +59,7 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph ) // SymbolicFactorGraph actual(factorGraph); // SymbolicFactor::shared_ptr f1 = actual[0]; // SymbolicFactor::shared_ptr f3 = actual[2]; -// actual.findAndRemoveFactors("x2"); +// actual.findAndRemoveFactors(kx(2)); // // // construct expected graph after find_factors_and_remove // SymbolicFactorGraph expected; @@ -79,13 +79,13 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph ) // SymbolicFactorGraph fg(factorGraph); // // // ask for all factor indices connected to x1 -// list x1_factors = fg.factors("x1"); +// list x1_factors = fg.factors(kx(1)); // int x1_indices[] = { 0, 1, 2 }; // list x1_expected(x1_indices, x1_indices + 3); // CHECK(x1_factors==x1_expected); // // // ask for all factor indices connected to x2 -// list x2_factors = fg.factors("x2"); +// list x2_factors = fg.factors(kx(2)); // int x2_indices[] = { 1, 3 }; // list x2_expected(x2_indices, x2_indices + 2); // CHECK(x2_factors==x2_expected); @@ -99,26 +99,26 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph ) // SymbolicFactorGraph fg(factorGraph); // // // combine all factors connected to x1 -// SymbolicFactor::shared_ptr actual = removeAndCombineFactors(fg,"x1"); +// SymbolicFactor::shared_ptr actual = removeAndCombineFactors(fg,kx(1)); // // // check result -// SymbolicFactor expected("l1","x1","x2"); +// SymbolicFactor expected(kl(1),kx(1),kx(2)); // CHECK(assert_equal(expected,*actual)); //} ///* ************************************************************************* */ //TEST( SymbolicFactorGraph, eliminateOne ) //{ -// Ordering o; o += "x1","l1","x2"; +// Ordering o; o += kx(1),kl(1),kx(2); // // create a test graph // GaussianFactorGraph factorGraph = createGaussianFactorGraph(o); // SymbolicFactorGraph fg(factorGraph); // // // eliminate -// IndexConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, o["x1"]+1); +// IndexConditional::shared_ptr actual = GaussianSequentialSolver::EliminateUntil(fg, o[kx(1)]+1); // // // create expected symbolic IndexConditional -// IndexConditional expected(o["x1"],o["l1"],o["x2"]); +// IndexConditional expected(o[kx(1)],o[kl(1)],o[kx(2)]); // // CHECK(assert_equal(expected,*actual)); //} @@ -126,12 +126,12 @@ TEST( SymbolicFactorGraph, symbolicFactorGraph ) /* ************************************************************************* */ TEST( SymbolicFactorGraph, eliminate ) { - Ordering o; o += "x2","l1","x1"; + Ordering o; o += kx(2),kl(1),kx(1); // create expected Chordal bayes Net - IndexConditional::shared_ptr x2(new IndexConditional(o["x2"], o["l1"], o["x1"])); - IndexConditional::shared_ptr l1(new IndexConditional(o["l1"], o["x1"])); - IndexConditional::shared_ptr x1(new IndexConditional(o["x1"])); + IndexConditional::shared_ptr x2(new IndexConditional(o[kx(2)], o[kl(1)], o[kx(1)])); + IndexConditional::shared_ptr l1(new IndexConditional(o[kl(1)], o[kx(1)])); + IndexConditional::shared_ptr x1(new IndexConditional(o[kx(1)])); SymbolicBayesNet expected; expected.push_back(x2); diff --git a/tests/testTupleValues.cpp b/tests/testTupleValues.cpp deleted file mode 100644 index 3df49061c..000000000 --- a/tests/testTupleValues.cpp +++ /dev/null @@ -1,627 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testTupleValues.cpp - * @author Richard Roberts - * @author Alex Cunningham - */ - -#include -#include - -#include - -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - -#include -#include -#include -#include - -#include -#include -#include - -using namespace gtsam; -using namespace std; - -static const double tol = 1e-5; - -typedef TypedSymbol PoseKey; -typedef TypedSymbol PointKey; -typedef Values PoseValues; -typedef Values PointValues; -typedef TupleValues2 TestValues; - -/* ************************************************************************* */ -TEST( TupleValues, constructors ) -{ - Pose2 x1(1,2,3), x2(6,7,8); - Point2 l1(4,5), l2(9,10); - - TestValues::Values1 cfg1; - cfg1.insert(PoseKey(1), x1); - cfg1.insert(PoseKey(2), x2); - TestValues::Values2 cfg2; - cfg2.insert(PointKey(1), l1); - cfg2.insert(PointKey(2), l2); - - TestValues actual(cfg1, cfg2), expected; - expected.insert(PoseKey(1), x1); - expected.insert(PoseKey(2), x2); - expected.insert(PointKey(1), l1); - expected.insert(PointKey(2), l2); - - CHECK(assert_equal(expected, actual)); -} - -/* ************************************************************************* */ -TEST( TupleValues, insert_equals1 ) -{ - Pose2 x1(1,2,3), x2(6,7,8); - Point2 l1(4,5), l2(9,10); - - TestValues expected; - expected.insert(PoseKey(1), x1); - expected.insert(PoseKey(2), x2); - expected.insert(PointKey(1), l1); - expected.insert(PointKey(2), l2); - - TestValues actual; - actual.insert(PoseKey(1), x1); - actual.insert(PoseKey(2), x2); - actual.insert(PointKey(1), l1); - actual.insert(PointKey(2), l2); - - CHECK(assert_equal(expected,actual)); -} - -/* ************************************************************************* */ -TEST( TupleValues, insert_equals2 ) -{ - Pose2 x1(1,2,3), x2(6,7,8); - Point2 l1(4,5), l2(9,10); - - TestValues values1; - values1.insert(PoseKey(1), x1); - values1.insert(PoseKey(2), x2); - values1.insert(PointKey(1), l1); - values1.insert(PointKey(2), l2); - - TestValues values2; - values2.insert(PoseKey(1), x1); - values2.insert(PoseKey(2), x2); - values2.insert(PointKey(1), l1); - - CHECK(!values1.equals(values2)); - - values2.insert(2, Point2(9,11)); - - CHECK(!values1.equals(values2)); -} - -/* ************************************************************************* */ -TEST( TupleValues, insert_duplicate ) -{ - Pose2 x1(1,2,3), x2(6,7,8); - Point2 l1(4,5), l2(9,10); - - TestValues values1; - values1.insert(1, x1); // 3 - values1.insert(2, x2); // 6 - values1.insert(1, l1); // 8 - values1.insert(2, l2); // 10 - CHECK_EXCEPTION(values1.insert(2, l1), KeyAlreadyExists); // still 10 !!!! -} - -/* ************************************************************************* */ -TEST( TupleValues, size_dim ) -{ - Pose2 x1(1,2,3), x2(6,7,8); - Point2 l1(4,5), l2(9,10); - - TestValues values1; - values1.insert(PoseKey(1), x1); - values1.insert(PoseKey(2), x2); - values1.insert(PointKey(1), l1); - values1.insert(PointKey(2), l2); - - EXPECT(values1.size() == 4); - EXPECT(values1.dim() == 10); -} - -/* ************************************************************************* */ -TEST(TupleValues, at) -{ - Pose2 x1(1,2,3), x2(6,7,8); - Point2 l1(4,5), l2(9,10); - - TestValues values1; - values1.insert(PoseKey(1), x1); - values1.insert(PoseKey(2), x2); - values1.insert(PointKey(1), l1); - values1.insert(PointKey(2), l2); - - EXPECT(assert_equal(x1, values1[PoseKey(1)])); - EXPECT(assert_equal(x2, values1[PoseKey(2)])); - EXPECT(assert_equal(l1, values1[PointKey(1)])); - EXPECT(assert_equal(l2, values1[PointKey(2)])); - - CHECK_EXCEPTION(values1[PoseKey(3)], KeyDoesNotExist); - CHECK_EXCEPTION(values1[PointKey(3)], KeyDoesNotExist); -} - -/* ************************************************************************* */ -TEST(TupleValues, zero_expmap_logmap) -{ - Pose2 x1(1,2,3), x2(6,7,8); - Point2 l1(4,5), l2(9,10); - - TestValues values1; - values1.insert(PoseKey(1), x1); - values1.insert(PoseKey(2), x2); - values1.insert(PointKey(1), l1); - values1.insert(PointKey(2), l2); - - Ordering o; o += "x1", "x2", "l1", "l2"; - VectorValues expected_zero(values1.dims(o)); - expected_zero[o["x1"]] = zero(3); - expected_zero[o["x2"]] = zero(3); - expected_zero[o["l1"]] = zero(2); - expected_zero[o["l2"]] = zero(2); - - CHECK(assert_equal(expected_zero, values1.zero(o))); - - VectorValues delta(values1.dims(o)); - delta[o["x1"]] = Vector_(3, 1.0, 1.1, 1.2); - delta[o["x2"]] = Vector_(3, 1.3, 1.4, 1.5); - delta[o["l1"]] = Vector_(2, 1.0, 1.1); - delta[o["l2"]] = Vector_(2, 1.3, 1.4); - - TestValues expected; - expected.insert(PoseKey(1), x1.retract(Vector_(3, 1.0, 1.1, 1.2))); - expected.insert(PoseKey(2), x2.retract(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)); - - TestValues actual = values1.retract(delta, o); - CHECK(assert_equal(expected, actual)); - - // Check log - VectorValues expected_log = delta; - VectorValues actual_log = values1.localCoordinates(actual, o); - CHECK(assert_equal(expected_log, actual_log)); -} - -/* ************************************************************************* */ - -// some key types -typedef TypedSymbol LamKey; -typedef TypedSymbol Pose3Key; -typedef TypedSymbol Point3Key; -typedef TypedSymbol Point3Key2; - -// some values types -typedef Values PoseValues; -typedef Values PointValues; -typedef Values LamValues; -typedef Values Pose3Values; -typedef Values Point3Values; -typedef Values Point3Values2; - -// some TupleValues types -typedef TupleValues > ValuesA; -typedef TupleValues > > ValuesB; - -typedef TupleValues1 TuplePoseValues; -typedef TupleValues1 TuplePointValues; -typedef TupleValues2 SimpleValues; - -/* ************************************************************************* */ -TEST(TupleValues, slicing) { - PointKey l1(1), l2(2); - Point2 l1_val(1.0, 2.0), l2_val(3.0, 4.0); - PoseKey x1(1), x2(2); - Pose2 x1_val(1.0, 2.0, 0.3), x2_val(3.0, 4.0, 0.4); - - PoseValues liePoseValues; - liePoseValues.insert(x1, x1_val); - liePoseValues.insert(x2, x2_val); - - PointValues liePointValues; - liePointValues.insert(l1, l1_val); - liePointValues.insert(l2, l2_val); - - // construct TupleValues1 from the base values - TuplePoseValues tupPoseValues1(liePoseValues); - EXPECT(assert_equal(liePoseValues, tupPoseValues1.first(), tol)); - - TuplePointValues tupPointValues1(liePointValues); - EXPECT(assert_equal(liePointValues, tupPointValues1.first(), tol)); - -// // construct a TupleValues2 from a TupleValues1 -// SimpleValues pairValues1(tupPoseValues1); -// EXPECT(assert_equal(liePoseValues, pairValues1.first(), tol)); -// EXPECT(pairValues1.second().empty()); -// -// SimpleValues pairValues2(tupPointValues1); -// EXPECT(assert_equal(liePointValues, pairValues2.second(), tol)); -// EXPECT(pairValues1.first().empty()); - -} - -/* ************************************************************************* */ -TEST(TupleValues, basic_functions) { - // create some tuple values - ValuesA valuesA; - ValuesB valuesB; - - PoseKey x1(1); - PointKey l1(1); - LamKey L1(1); - Pose2 pose1(1.0, 2.0, 0.3); - Point2 point1(2.0, 3.0); - LieVector lam1 = LieVector(2.3); - - // Insert - valuesA.insert(x1, pose1); - valuesA.insert(l1, point1); - - valuesB.insert(x1, pose1); - valuesB.insert(l1, point1); - valuesB.insert(L1, lam1); - - // bracket operator - EXPECT(assert_equal(valuesA[x1], pose1)); - EXPECT(assert_equal(valuesA[l1], point1)); - EXPECT(assert_equal(valuesB[x1], pose1)); - EXPECT(assert_equal(valuesB[l1], point1)); - EXPECT(assert_equal(valuesB[L1], lam1)); - - // exists - EXPECT(valuesA.exists(x1)); - EXPECT(valuesA.exists(l1)); - EXPECT(valuesB.exists(x1)); - EXPECT(valuesB.exists(l1)); - EXPECT(valuesB.exists(L1)); - - // at - EXPECT(assert_equal(valuesA.at(x1), pose1)); - EXPECT(assert_equal(valuesA.at(l1), point1)); - EXPECT(assert_equal(valuesB.at(x1), pose1)); - EXPECT(assert_equal(valuesB.at(l1), point1)); - EXPECT(assert_equal(valuesB.at(L1), lam1)); - - // size - EXPECT(valuesA.size() == 2); - EXPECT(valuesB.size() == 3); - - // dim - EXPECT(valuesA.dim() == 5); - EXPECT(valuesB.dim() == 6); - - // erase - valuesA.erase(x1); - CHECK(!valuesA.exists(x1)); - CHECK(valuesA.size() == 1); - valuesA.erase(l1); - CHECK(!valuesA.exists(l1)); - CHECK(valuesA.size() == 0); - valuesB.erase(L1); - CHECK(!valuesB.exists(L1)); - CHECK(valuesB.size() == 2); - - // clear - valuesA.clear(); - EXPECT(valuesA.size() == 0); - valuesB.clear(); - EXPECT(valuesB.size() == 0); - - // empty - EXPECT(valuesA.empty()); - EXPECT(valuesB.empty()); -} - -/* ************************************************************************* */ -TEST(TupleValues, insert_values) { - ValuesB values1, values2, expected; - - PoseKey x1(1), x2(2); - PointKey l1(1), l2(2); - LamKey L1(1), L2(2); - 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); - LieVector lam1 = LieVector(2.3), lam2 = LieVector(4.5); - - values1.insert(x1, pose1); - values1.insert(l1, point1); - values1.insert(L1, lam1); - - values2.insert(x2, pose2); - values2.insert(l2, point2); - values2.insert(L2, lam2); - - values1.insert(values2); - - expected.insert(x1, pose1); - expected.insert(l1, point1); - expected.insert(L1, lam1); - expected.insert(x2, pose2); - expected.insert(l2, point2); - expected.insert(L2, lam2); - - CHECK(assert_equal(expected, values1)); -} - -/* ************************************************************************* */ -TEST( TupleValues, update_element ) -{ - TupleValues2 cfg; - Pose2 x1(2.0, 1.0, 2.0), x2(3.0, 4.0, 5.0); - Point2 l1(1.0, 2.0), l2(3.0, 4.0); - PoseKey xk(1); - PointKey lk(1); - - cfg.insert(xk, x1); - CHECK(cfg.size() == 1); - CHECK(assert_equal(x1, cfg.at(xk))); - - cfg.update(xk, x2); - CHECK(cfg.size() == 1); - CHECK(assert_equal(x2, cfg.at(xk))); - - cfg.insert(lk, l1); - CHECK(cfg.size() == 2); - CHECK(assert_equal(l1, cfg.at(lk))); - - cfg.update(lk, l2); - CHECK(cfg.size() == 2); - CHECK(assert_equal(l2, cfg.at(lk))); -} - -/* ************************************************************************* */ -TEST( TupleValues, equals ) -{ - Pose2 x1(1,2,3), x2(6,7,8), x2_alt(5,6,7); - PoseKey x1k(1), x2k(2); - Point2 l1(4,5), l2(9,10); - PointKey l1k(1), l2k(2); - - ValuesA values1, values2, values3, values4, values5; - - values1.insert(x1k, x1); - values1.insert(x2k, x2); - values1.insert(l1k, l1); - values1.insert(l2k, l2); - - values2.insert(x1k, x1); - values2.insert(x2k, x2); - values2.insert(l1k, l1); - values2.insert(l2k, l2); - - values3.insert(x2k, x2); - values3.insert(l1k, l1); - - values4.insert(x1k, x1); - values4.insert(x2k, x2_alt); - values4.insert(l1k, l1); - values4.insert(l2k, l2); - - ValuesA values6(values1); - - EXPECT(assert_equal(values1,values2)); - EXPECT(assert_equal(values1,values1)); - EXPECT(assert_inequal(values1,values3)); - EXPECT(assert_inequal(values1,values4)); - EXPECT(assert_inequal(values1,values5)); - EXPECT(assert_equal(values1, values6)); -} - -/* ************************************************************************* */ -TEST(TupleValues, expmap) -{ - Pose2 x1(1,2,3), x2(6,7,8); - PoseKey x1k(1), x2k(2); - Point2 l1(4,5), l2(9,10); - PointKey l1k(1), l2k(2); - - Ordering o; o += "x1", "x2", "l1", "l2"; - - ValuesA values1; - values1.insert(x1k, x1); - values1.insert(x2k, x2); - values1.insert(l1k, l1); - values1.insert(l2k, l2); - - VectorValues delta(values1.dims(o)); - delta[o["x1"]] = Vector_(3, 1.0, 1.1, 1.2); - delta[o["x2"]] = Vector_(3, 1.3, 1.4, 1.5); - delta[o["l1"]] = Vector_(2, 1.0, 1.1); - delta[o["l2"]] = Vector_(2, 1.3, 1.4); - - ValuesA expected; - expected.insert(x1k, x1.retract(Vector_(3, 1.0, 1.1, 1.2))); - expected.insert(x2k, x2.retract(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, values1.retract(delta, o))); - CHECK(assert_equal(delta, values1.localCoordinates(expected, o))); -} - -/* ************************************************************************* */ -TEST(TupleValues, expmap_typedefs) -{ - Pose2 x1(1,2,3), x2(6,7,8); - PoseKey x1k(1), x2k(2); - Point2 l1(4,5), l2(9,10); - PointKey l1k(1), l2k(2); - - Ordering o; o += "x1", "x2", "l1", "l2"; - - TupleValues2 values1, expected, actual; - values1.insert(x1k, x1); - values1.insert(x2k, x2); - values1.insert(l1k, l1); - values1.insert(l2k, l2); - - VectorValues delta(values1.dims(o)); - delta[o["x1"]] = Vector_(3, 1.0, 1.1, 1.2); - delta[o["x2"]] = Vector_(3, 1.3, 1.4, 1.5); - delta[o["l1"]] = Vector_(2, 1.0, 1.1); - delta[o["l2"]] = Vector_(2, 1.3, 1.4); - - expected.insert(x1k, x1.retract(Vector_(3, 1.0, 1.1, 1.2))); - expected.insert(x2k, x2.retract(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, TupleValues2(values1.retract(delta, o)))); - //CHECK(assert_equal(delta, values1.localCoordinates(expected))); -} - -/* ************************************************************************* */ -TEST(TupleValues, typedefs) -{ - TupleValues2 values1; - TupleValues3 values2; - TupleValues4 values3; - TupleValues5 values4; - TupleValues6 values5; -} - -/* ************************************************************************* */ -TEST( TupleValues, pairvalues_style ) -{ - PoseKey x1(1); - PointKey l1(1); - LamKey L1(1); - Pose2 pose1(1.0, 2.0, 0.3); - Point2 point1(2.0, 3.0); - LieVector lam1 = LieVector(2.3); - - PoseValues values1; values1.insert(x1, pose1); - PointValues values2; values2.insert(l1, point1); - LamValues values3; values3.insert(L1, lam1); - - // Constructor - TupleValues3 values(values1, values2, values3); - - // access - CHECK(assert_equal(values1, values.first())); - CHECK(assert_equal(values2, values.second())); - CHECK(assert_equal(values3, values.third())); -} - -/* ************************************************************************* */ -TEST(TupleValues, insert_values_typedef) { - - TupleValues4 values1, values2, expected; - - PoseKey x1(1), x2(2); - PointKey l1(1), l2(2); - LamKey L1(1), L2(2); - 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); - LieVector lam1 = LieVector(2.3), lam2 = LieVector(4.5); - - values1.insert(x1, pose1); - values1.insert(l1, point1); - values1.insert(L1, lam1); - - values2.insert(x2, pose2); - values2.insert(l2, point2); - values2.insert(L2, lam2); - - values1.insert(values2); - - expected.insert(x1, pose1); - expected.insert(l1, point1); - expected.insert(L1, lam1); - expected.insert(x2, pose2); - expected.insert(l2, point2); - expected.insert(L2, lam2); - - CHECK(assert_equal(expected, values1)); -} - -/* ************************************************************************* */ -TEST(TupleValues, partial_insert) { - TupleValues3 init, expected; - - PoseKey x1(1), x2(2); - PointKey l1(1), l2(2); - LamKey L1(1), L2(2); - 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); - LieVector lam1 = LieVector(2.3), lam2 = LieVector(4.5); - - init.insert(x1, pose1); - init.insert(l1, point1); - init.insert(L1, lam1); - - PoseValues cfg1; - cfg1.insert(x2, pose2); - - init.insertSub(cfg1); - - expected.insert(x1, pose1); - expected.insert(l1, point1); - expected.insert(L1, lam1); - expected.insert(x2, pose2); - - CHECK(assert_equal(expected, init)); -} - -/* ************************************************************************* */ -TEST(TupleValues, update) { - TupleValues3 init, superset, expected; - - PoseKey x1(1), x2(2); - PointKey l1(1), l2(2); - 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(TupleValues, arbitrary_ordering ) { - TupleValues1 values; - PoseKey x1(1), x2(2); - Pose2 pose1(1.0, 2.0, 0.3), pose2(3.0, 4.0, 5.0); - values.insert(x1, pose1); - values.insert(x2, pose2); - Ordering::shared_ptr actual = values.orderingArbitrary(); - Ordering expected; expected += x1, x2; - EXPECT(assert_equal(expected, *actual)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ diff --git a/tests/timeGaussianFactorGraph.cpp b/tests/timeGaussianFactorGraph.cpp index e6b028e6d..3c9d83b3d 100644 --- a/tests/timeGaussianFactorGraph.cpp +++ b/tests/timeGaussianFactorGraph.cpp @@ -15,9 +15,6 @@ * @author Frank Dellaert */ -// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h -#define GTSAM_MAGIC_KEY - #include #include #include // for operator += in Ordering diff --git a/tests/timeMultifrontalOnDataset.cpp b/tests/timeMultifrontalOnDataset.cpp index 8fe127989..874ccf8bb 100644 --- a/tests/timeMultifrontalOnDataset.cpp +++ b/tests/timeMultifrontalOnDataset.cpp @@ -44,7 +44,7 @@ int main(int argc, char *argv[]) { else if (argc == 4) nrTrials = strtoul(argv[3], NULL, 10); - pair, shared_ptr > data = load2D(dataset(datasetname)); + pair, shared_ptr > data = load2D(dataset(datasetname)); // Add a prior on the first pose if (soft_prior) diff --git a/tests/timeSequentialOnDataset.cpp b/tests/timeSequentialOnDataset.cpp index d9b71576f..9e1b986f9 100644 --- a/tests/timeSequentialOnDataset.cpp +++ b/tests/timeSequentialOnDataset.cpp @@ -44,7 +44,7 @@ int main(int argc, char *argv[]) { else if (argc == 4) nrTrials = strtoul(argv[3], NULL, 10); - pair, shared_ptr > data = load2D(dataset(datasetname)); + pair, shared_ptr > data = load2D(dataset(datasetname)); // Add a prior on the first pose if (soft_prior) diff --git a/wrap/Makefile.am b/wrap/Makefile.am deleted file mode 100644 index 2cb426bae..000000000 --- a/wrap/Makefile.am +++ /dev/null @@ -1,146 +0,0 @@ -#---------------------------------------------------------------------------------------------------- -# GTSAM Matlab wrap toolset -#---------------------------------------------------------------------------------------------------- - -# use nostdinc to turn off -I. and -I.., we do not need them because -# header files are qualified so they can be included in external projects. -AUTOMAKE_OPTIONS = nostdinc -AM_DEFAULT_SOURCE_EXT = .cpp - -headers = -sources = -check_PROGRAMS = -noinst_PROGRAMS = -wrap_PROGRAMS = -wrapdir = $(includedir)/wrap - -# disable all of matlab toolbox build by default -if ENABLE_BUILD_TOOLBOX - -# Build a library from the core sources -sources += utilities.cpp Argument.cpp ReturnValue.cpp Constructor.cpp -sources += Method.cpp StaticMethod.cpp Class.cpp Module.cpp FileWriter.cpp -check_PROGRAMS += tests/testSpirit tests/testWrap - -# Manually install wrap later -noinst_PROGRAMS += wrap - - -#---------------------------------------------------------------------------------------------------- -# Create a libtool library that is not installed -# The headers are installed in $(includedir)/wrap: -#---------------------------------------------------------------------------------------------------- -# Only install the header necessary for wrap interfaces to build with mex -headers += matlab.h -wrap_HEADERS = $(headers) -noinst_LTLIBRARIES = libwrap.la -libwrap_la_SOURCES = $(sources) -AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir) -DTOPSRCDIR="\"$(top_srcdir)\"" -AM_LDFLAGS = $(BOOST_LDFLAGS) - -#---------------------------------------------------------------------------------------------------- -# rules to build local programs -#---------------------------------------------------------------------------------------------------- -TESTS = $(check_PROGRAMS) -AM_LDFLAGS += $(boost_serialization) -LDADD = libwrap.la ../CppUnitLite/libCppUnitLite.a - -# rule to run an executable -%.run: % $(LDADD) - ./$^ - -# rule to run executable with valgrind -%.valgrind: % $(LDADD) - valgrind ./$^ - -# generate local toolbox dir -interfacePath = $(top_srcdir) -moduleName = gtsam -toolboxpath = ../toolbox - -# Set flags to pass to mex -mexFlags = -if ENABLE_UNSAFE_WRAP -mexFlags += "${BOOST_CPPFLAGS} -DUNSAFE_WRAP -I${prefix}/include -I${prefix}/include/gtsam -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam" -else -mexFlags += "${BOOST_CPPFLAGS} -I${prefix}/include -I${prefix}/include/gtsam -I${prefix}/include/gtsam/base -I${prefix}/include/gtsam/geometry -I${prefix}/include/gtsam/linear -I${prefix}/include/gtsam/nonlinear -I${prefix}/include/gtsam/slam -L${exec_prefix}/lib -lgtsam" -endif - -# Find the extension for mex binaries -# this should be done with mexext with matlab -mexextension = -if LINUX -if IS_64BIT -mexextension += mexa64 -else -mexextension += mexglx -endif -else # Linux -if DARWIN -mexextension += mexmaci64 -else -mexextension += mex_bin -endif -endif # Linux - -# Choose correct cp command by OS -# In linux, this will only copy the file if it is an update -# Macs use an older version of cp that doesn't support the -u flag -cp_install = -if LINUX -cp_install += cp -ru -endif -if DARWIN -cp_install += cp -Rf -endif - -all: generate_toolbox - -generate_toolbox: $(top_srcdir)/gtsam.h wrap - ./wrap ${mexextension} ${interfacePath} ${moduleName} ${toolboxpath} ${mexFlags} - -source_mode = -m 644 - -wrap-install-matlab-toolbox: generate_toolbox - install -d ${toolbox}/gtsam - ${cp_install} ../toolbox/*.m ${toolbox}/gtsam - ${cp_install} ../toolbox/*.cpp ${toolbox}/gtsam - ${cp_install} ../toolbox/Makefile ${toolbox}/gtsam - ${cp_install} ../toolbox/@* ${toolbox}/gtsam - -wrap-install-bin: wrap - install -d ${wrap} - install -c ./wrap ${wrap} - -wrap-install-matlab-tests: - install -d ${toolbox}/gtsam/tests - install ${source_mode} -c ../../tests/matlab/*.m ${toolbox}/gtsam/tests - -wrap-install-matlab-examples: - install -d ${toolbox}/gtsam/examples - install ${source_mode} -c ../../examples/matlab/*.m ${toolbox}/gtsam/examples - -wrap_install_targets = -wrap_install_targets += wrap-install-matlab-toolbox - -if ENABLE_INSTALL_WRAP -wrap_install_targets += wrap-install-bin -endif - -if ENABLE_INSTALL_MATLAB_TESTS -wrap_install_targets += wrap-install-matlab-tests -endif - -if ENABLE_INSTALL_MATLAB_EXAMPLES -wrap_install_targets += wrap-install-matlab-examples -endif - -install-exec-hook: ${wrap_install_targets} - -# clean local toolbox dir -clean: - @test -z "wrap" || rm -f wrap - @test -z "../toolbox" || rm -rf ../toolbox - -endif -#---------------------------------------------------------------------------------------------------- diff --git a/wrap/matlab.h b/wrap/matlab.h index 2c256a55d..631abb659 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -40,8 +40,6 @@ using namespace std; using namespace boost; // not usual, but for conciseness of generated code // start GTSAM Specifics ///////////////////////////////////////////////// -// to make keys be constructed from strings: -#define GTSAM_MAGIC_KEY // to enable Matrix and Vector constructor for SharedGaussian: #define GTSAM_MAGIC_GAUSSIAN // end GTSAM Specifics /////////////////////////////////////////////////