diff --git a/configure.ac b/configure.ac index ee6950438..5929b22ee 100644 --- a/configure.ac +++ b/configure.ac @@ -47,21 +47,6 @@ AC_ARG_ENABLE([debug], AM_CONDITIONAL([DEBUG], [test x$debug = xtrue]) - -# AGC: isn't this redundant? -#AC_CANONICAL_HOST # why was this called twice? -# We need to determine what os we are on to determine if we need to do -# special things because we are on a mac -# case $host_os in -# darwin* ) -# # Do something specific for mac -# ISMAC=true -# ;; -# *) -# ISMAC=false -# ;; -# esac - # enable profiling AC_ARG_ENABLE([profiling], [ --enable-profiling Enable profiling], @@ -95,30 +80,6 @@ AC_ARG_ENABLE([install_cppunitlite], AM_CONDITIONAL([ENABLE_INSTALL_CPPUNITLITE], [test x$install_cppunitlite = xtrue]) -# 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]) - # enable matlab toolbox generation AC_ARG_ENABLE([build_toolbox], [ --enable-build-toolbox Enable building of the Matlab toolbox], @@ -141,6 +102,17 @@ AC_ARG_ENABLE([install_matlab_tests], 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], @@ -179,6 +151,30 @@ AC_ARG_WITH([wrap], [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 \ diff --git a/gtsam.h b/gtsam.h index c0ecea575..4f9dd5e46 100644 --- a/gtsam.h +++ b/gtsam.h @@ -256,14 +256,6 @@ class KalmanFilter { void update(Matrix H, Vector z, const SharedDiagonal& model); }; -class Landmark2 { - Landmark2(); - Landmark2(double x, double y); - void print(string s) const; - double x(); - double y(); -}; - class Ordering { Ordering(); void print(string s) const; @@ -276,22 +268,28 @@ class SharedNoiseModel { // FIXME: this needs actual constructors }; -class PlanarSLAMValues { - PlanarSLAMValues(); +// Planar SLAM example domain +namespace planarSLAM { + +class Values { +#include + Values(); void print(string s) const; - Pose2* pose(int key); + Pose2 pose(int key) const; + Point2 point(int key) const; void insertPose(int key, const Pose2& pose); void insertPoint(int key, const Point2& point); }; -class PlanarSLAMGraph { - PlanarSLAMGraph(); +class Graph { +#include + Graph(); void print(string s) const; - double error(const PlanarSLAMValues& values) const; - Ordering* orderingCOLAMD(const PlanarSLAMValues& values) const; - GaussianFactorGraph* linearize(const PlanarSLAMValues& values, + double error(const planarSLAM::Values& values) const; + Ordering* orderingCOLAMD(const planarSLAM::Values& values) const; + GaussianFactorGraph* linearize(const planarSLAM::Values& values, const Ordering& ordering) const; void addPrior(int key, const Pose2& pose, const SharedNoiseModel& noiseModel); @@ -301,16 +299,19 @@ class PlanarSLAMGraph { void addRange(int poseKey, int pointKey, double range, const SharedNoiseModel& noiseModel); void addBearingRange(int poseKey, int pointKey, const Rot2& bearing, double range, const SharedNoiseModel& noiseModel); - PlanarSLAMValues* optimize_(const PlanarSLAMValues& initialEstimate); + planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate); }; -class PlanarSLAMOdometry { - PlanarSLAMOdometry(int key1, int key2, const Pose2& measured, +class Odometry { +#include + Odometry(int key1, int key2, const Pose2& measured, const SharedNoiseModel& model); void print(string s) const; - GaussianFactor* linearize(const PlanarSLAMValues& center, const Ordering& ordering) const; + GaussianFactor* linearize(const planarSLAM::Values& center, const Ordering& ordering) const; }; +}///\namespace planarSLAM + class GaussianSequentialSolver { GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR); GaussianBayesNet* eliminate() const; diff --git a/gtsam/slam/Landmark2.h b/gtsam/slam/Landmark2.h deleted file mode 100644 index f67cfed56..000000000 --- a/gtsam/slam/Landmark2.h +++ /dev/null @@ -1,27 +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 Landmark2.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include - -namespace gtsam { - - typedef gtsam::Point2 Landmark2; - -} - diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am index 406431edd..d55d40666 100644 --- a/gtsam/slam/Makefile.am +++ b/gtsam/slam/Makefile.am @@ -51,10 +51,10 @@ sources += planarSLAM.cpp check_PROGRAMS += tests/testPlanarSLAM # MATLAB Wrap headers for planarSLAM -headers += Landmark2.h -headers += PlanarSLAMGraph.h -headers += PlanarSLAMValues.h -headers += PlanarSLAMOdometry.h +# headers += Landmark2.h +# headers += PlanarSLAMGraph.h +# headers += PlanarSLAMValues.h +# headers += PlanarSLAMOdometry.h # 3D Pose constraints sources += pose3SLAM.cpp diff --git a/gtsam/slam/PlanarSLAMGraph.h b/gtsam/slam/PlanarSLAMGraph.h deleted file mode 100644 index 98eb9aaef..000000000 --- a/gtsam/slam/PlanarSLAMGraph.h +++ /dev/null @@ -1,28 +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 PlanarSLAMGraph.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - typedef gtsam::planarSLAM::Graph PlanarSLAMGraph; - -} - diff --git a/gtsam/slam/PlanarSLAMOdometry.h b/gtsam/slam/PlanarSLAMOdometry.h deleted file mode 100644 index 941eda285..000000000 --- a/gtsam/slam/PlanarSLAMOdometry.h +++ /dev/null @@ -1,28 +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 PlanarSLAMOdometry.h - * @brief Re-created on Feb 22, 2010 for compatibility with MATLAB - * @author Frank Dellaert - */ - -#pragma once - -#include -#include - -namespace gtsam { - - typedef gtsam::planarSLAM::Odometry PlanarSLAMOdometry; - -} - diff --git a/gtsam/slam/PlanarSLAMValues.h b/gtsam/slam/PlanarSLAMValues.h deleted file mode 100644 index d174229fd..000000000 --- a/gtsam/slam/PlanarSLAMValues.h +++ /dev/null @@ -1,27 +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 PlanarSLAMValues.h - * @brief Convenience for MATLAB wrapper, which does not allow for identically named methods - * @author Frank Dellaert - */ - -#pragma once - -#include - -namespace gtsam { - - typedef gtsam::planarSLAM::Values PlanarSLAMValues; - -} - diff --git a/gtsam/slam/planarSLAM.h b/gtsam/slam/planarSLAM.h index 228ced08c..5776e989c 100644 --- a/gtsam/slam/planarSLAM.h +++ b/gtsam/slam/planarSLAM.h @@ -71,10 +71,10 @@ namespace gtsam { // Convenience for MATLAB wrapper, which does not allow for identically named methods /// get a pose - boost::shared_ptr pose(int key) { - Pose2 pose = (*this)[PoseKey(key)]; - return boost::shared_ptr(new Pose2(pose)); - } + Pose2 pose(int key) const { return (*this)[PoseKey(key)]; } + + /// get a point + Point2 point(int key) const { return (*this)[PointKey(key)]; } /// insert a pose void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), pose); } @@ -131,14 +131,11 @@ namespace gtsam { void addBearingRange(const PoseKey& poseKey, const PointKey& pointKey, const Rot2& bearing, double range, const SharedNoiseModel& model); - /// Optimize_ for MATLAB - boost::shared_ptr optimize_(const Values& initialEstimate) { + /// Optimize + Values optimize(const Values& initialEstimate) { typedef NonlinearOptimizer Optimizer; - boost::shared_ptr result( - new Values( - *Optimizer::optimizeLM(*this, initialEstimate, - NonlinearOptimizationParameters::LAMBDA))); - return result; + return *Optimizer::optimizeLM(*this, initialEstimate, + NonlinearOptimizationParameters::LAMBDA); } }; diff --git a/wrap/Makefile.am b/wrap/Makefile.am index d572b88e4..7d0c980e1 100644 --- a/wrap/Makefile.am +++ b/wrap/Makefile.am @@ -58,64 +58,75 @@ interfacePath = $(top_srcdir) moduleName = gtsam toolboxpath = ../toolbox nameSpace = "gtsam" + +# Set flags to pass to mex mexFlags = -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" 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" +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" +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 - FIXME: this should be done with mexext with matlab -mexextension = mexa64 +# Find the extension for mex binaries +# this should be done with mexext with matlab +mexextension = if LINUX if IS_64BIT - mexextension += mexa64 +mexextension += mexa64 else - mexextension += mexglx +mexextension += mexglx endif else # Linux if DARWIN - mexextension += mexmaci64 +mexextension += mexmaci64 else - mexextension += mex_bin +mexextension += mex_bin endif endif # Linux -all: +all: generate_toolbox + +generate_toolbox: $(top_srcdir)/gtsam.h ./wrap ${mexextension} ${interfacePath} ${moduleName} ${toolboxpath} ${nameSpace} ${mexFlags} -wrap-install-bin: all - install -d ${wrap} && \ +source_mode = -m 644 + +wrap-install-matlab-toolbox: + install -d ${toolbox}/gtsam + install ${source_mode} -c ../toolbox/*.m ${toolbox}/gtsam + install ${source_mode} -c ../toolbox/*.cpp ${toolbox}/gtsam + install ${source_mode} -c ../toolbox/Makefile ${toolbox}/gtsam + cp -ru ../toolbox/@* ${toolbox}/gtsam + +wrap-install-bin: + install -d ${wrap} install ./wrap ${wrap} -wrap-install-matlab-tests: all - install -d ${toolbox}/gtsam/tests && \ - cp -rf ../../tests/matlab/*.m ${toolbox}/gtsam/tests +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 -# install the headers and matlab toolbox if ENABLE_INSTALL_WRAP +wrap_install_targets += wrap-install-bin +endif + if ENABLE_INSTALL_MATLAB_TESTS -install-exec-hook: wrap-install-bin wrap-install-matlab-tests - install -d ${toolbox}/gtsam && \ - cp -rf ../toolbox/* ${toolbox}/gtsam -else -install-exec-hook: wrap-install-bin - install -d ${toolbox}/gtsam && \ - cp -rf ../toolbox/* ${toolbox}/gtsam/tests -endif -else -if ENABLE_INSTALL_MATLAB_TESTS -install-exec-hook: wrap-install-matlab-tests - install -d ${toolbox}/gtsam && \ - cp -rf ../toolbox/* ${toolbox}/gtsam -else -install-exec-hook: all - install -d ${toolbox}/gtsam && \ - cp -rf ../toolbox/* ${toolbox}/gtsam/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