12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added an implementation of the planarSLAM to gtsam.h using manual includes and namespaces, removed old duplicate header files

release/4.3a0
Alex Cunningham 2011-12-09 15:44:37 +00:00
parent 9dff4c35bd
commit c302a50146
9 changed files with 112 additions and 217 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
configure.ac
View File

@ -47,21 +47,6 @@ AC_ARG_ENABLE([debug],
AM_CONDITIONAL([DEBUG], [test x$debug = xtrue]) 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 # enable profiling
AC_ARG_ENABLE([profiling], AC_ARG_ENABLE([profiling],
[ --enable-profiling 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]) 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 # enable matlab toolbox generation
AC_ARG_ENABLE([build_toolbox], AC_ARG_ENABLE([build_toolbox],
[ --enable-build-toolbox Enable building of the Matlab 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]) 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 # Matlab toolbox: optional flag to change location of toolbox, defaults to install prefix
AC_ARG_WITH([toolbox], AC_ARG_WITH([toolbox],
[AS_HELP_STRING([--with-toolbox], [AS_HELP_STRING([--with-toolbox],
@ -179,6 +151,30 @@ AC_ARG_WITH([wrap],
[wrap=$prefix/bin]) [wrap=$prefix/bin])
AC_SUBST([wrap]) 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 \ AC_CONFIG_FILES([CppUnitLite/Makefile \
wrap/Makefile \ wrap/Makefile \
gtsam/3rdparty/Makefile \ gtsam/3rdparty/Makefile \

41
gtsam.h
View File

@ -256,14 +256,6 @@ class KalmanFilter {
void update(Matrix H, Vector z, const SharedDiagonal& model); 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 { class Ordering {
Ordering(); Ordering();
void print(string s) const; void print(string s) const;
@ -276,22 +268,28 @@ class SharedNoiseModel {
// FIXME: this needs actual constructors // FIXME: this needs actual constructors
}; };
class PlanarSLAMValues { // Planar SLAM example domain
PlanarSLAMValues(); namespace planarSLAM {
class Values {
#include <gtsam/slam/planarSLAM.h>
Values();
void print(string s) const; 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 insertPose(int key, const Pose2& pose);
void insertPoint(int key, const Point2& point); void insertPoint(int key, const Point2& point);
}; };
class PlanarSLAMGraph { class Graph {
PlanarSLAMGraph(); #include <gtsam/slam/planarSLAM.h>
Graph();
void print(string s) const; void print(string s) const;
double error(const PlanarSLAMValues& values) const; double error(const planarSLAM::Values& values) const;
Ordering* orderingCOLAMD(const PlanarSLAMValues& values) const; Ordering* orderingCOLAMD(const planarSLAM::Values& values) const;
GaussianFactorGraph* linearize(const PlanarSLAMValues& values, GaussianFactorGraph* linearize(const planarSLAM::Values& values,
const Ordering& ordering) const; const Ordering& ordering) const;
void addPrior(int key, const Pose2& pose, const SharedNoiseModel& noiseModel); 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 addRange(int poseKey, int pointKey, double range, const SharedNoiseModel& noiseModel);
void addBearingRange(int poseKey, int pointKey, const Rot2& bearing, double range, void addBearingRange(int poseKey, int pointKey, const Rot2& bearing, double range,
const SharedNoiseModel& noiseModel); const SharedNoiseModel& noiseModel);
PlanarSLAMValues* optimize_(const PlanarSLAMValues& initialEstimate); planarSLAM::Values optimize(const planarSLAM::Values& initialEstimate);
}; };
class PlanarSLAMOdometry { class Odometry {
PlanarSLAMOdometry(int key1, int key2, const Pose2& measured, #include <gtsam/slam/planarSLAM.h>
Odometry(int key1, int key2, const Pose2& measured,
const SharedNoiseModel& model); const SharedNoiseModel& model);
void print(string s) const; 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 { class GaussianSequentialSolver {
GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR); GaussianSequentialSolver(const GaussianFactorGraph& graph, bool useQR);
GaussianBayesNet* eliminate() const; GaussianBayesNet* eliminate() const;

27
gtsam/slam/Landmark2.h
View File

@ -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 <gtsam/geometry/Point2.h>
namespace gtsam {
typedef gtsam::Point2 Landmark2;
}

8
gtsam/slam/Makefile.am
View File

@ -51,10 +51,10 @@ sources += planarSLAM.cpp
check_PROGRAMS += tests/testPlanarSLAM check_PROGRAMS += tests/testPlanarSLAM
# MATLAB Wrap headers for planarSLAM # MATLAB Wrap headers for planarSLAM
headers += Landmark2.h # headers += Landmark2.h
headers += PlanarSLAMGraph.h # headers += PlanarSLAMGraph.h
headers += PlanarSLAMValues.h # headers += PlanarSLAMValues.h
headers += PlanarSLAMOdometry.h # headers += PlanarSLAMOdometry.h
# 3D Pose constraints # 3D Pose constraints
sources += pose3SLAM.cpp sources += pose3SLAM.cpp

28
gtsam/slam/PlanarSLAMGraph.h
View File

@ -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 <gtsam/slam/planarSLAM.h>
#include <gtsam/slam/PlanarSLAMValues.h>
namespace gtsam {
typedef gtsam::planarSLAM::Graph PlanarSLAMGraph;
}

28
gtsam/slam/PlanarSLAMOdometry.h
View File

@ -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 <gtsam/slam/planarSLAM.h>
#include <gtsam/slam/PlanarSLAMValues.h>
namespace gtsam {
typedef gtsam::planarSLAM::Odometry PlanarSLAMOdometry;
}

27
gtsam/slam/PlanarSLAMValues.h
View File

@ -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 <gtsam/slam/planarSLAM.h>
namespace gtsam {
typedef gtsam::planarSLAM::Values PlanarSLAMValues;
}

19
gtsam/slam/planarSLAM.h
View File

@ -71,10 +71,10 @@ namespace gtsam {
// Convenience for MATLAB wrapper, which does not allow for identically named methods // Convenience for MATLAB wrapper, which does not allow for identically named methods
/// get a pose /// get a pose
boost::shared_ptr<Pose2> pose(int key) { Pose2 pose(int key) const { return (*this)[PoseKey(key)]; }
Pose2 pose = (*this)[PoseKey(key)];
return boost::shared_ptr<Pose2>(new Pose2(pose)); /// get a point
} Point2 point(int key) const { return (*this)[PointKey(key)]; }
/// insert a pose /// insert a pose
void insertPose(int key, const Pose2& pose) {insert(PoseKey(key), 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, void addBearingRange(const PoseKey& poseKey, const PointKey& pointKey,
const Rot2& bearing, double range, const SharedNoiseModel& model); const Rot2& bearing, double range, const SharedNoiseModel& model);
/// Optimize_ for MATLAB /// Optimize
boost::shared_ptr<Values> optimize_(const Values& initialEstimate) { Values optimize(const Values& initialEstimate) {
typedef NonlinearOptimizer<Graph, Values> Optimizer; typedef NonlinearOptimizer<Graph, Values> Optimizer;
boost::shared_ptr<Values> result( return *Optimizer::optimizeLM(*this, initialEstimate,
new Values( NonlinearOptimizationParameters::LAMBDA);
*Optimizer::optimizeLM(*this, initialEstimate,
NonlinearOptimizationParameters::LAMBDA)));
return result;
} }
}; };

77
wrap/Makefile.am
View File

@ -58,64 +58,75 @@ interfacePath = $(top_srcdir)
moduleName = gtsam moduleName = gtsam
toolboxpath = ../toolbox toolboxpath = ../toolbox
nameSpace = "gtsam" nameSpace = "gtsam"
# Set flags to pass to mex
mexFlags = 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 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 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 endif
# Find the extension for mex binaries - FIXME: this should be done with mexext with matlab # Find the extension for mex binaries
mexextension = mexa64 # this should be done with mexext with matlab
mexextension =
if LINUX if LINUX
if IS_64BIT if IS_64BIT
mexextension += mexa64 mexextension += mexa64
else else
mexextension += mexglx mexextension += mexglx
endif endif
else # Linux else # Linux
if DARWIN if DARWIN
mexextension += mexmaci64 mexextension += mexmaci64
else else
mexextension += mex_bin mexextension += mex_bin
endif endif
endif # Linux endif # Linux
all: all: generate_toolbox
generate_toolbox: $(top_srcdir)/gtsam.h
./wrap ${mexextension} ${interfacePath} ${moduleName} ${toolboxpath} ${nameSpace} ${mexFlags} ./wrap ${mexextension} ${interfacePath} ${moduleName} ${toolboxpath} ${nameSpace} ${mexFlags}
wrap-install-bin: all source_mode = -m 644
install -d ${wrap} && \
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} install ./wrap ${wrap}
wrap-install-matlab-tests: all wrap-install-matlab-tests:
install -d ${toolbox}/gtsam/tests && \ install -d ${toolbox}/gtsam/tests
cp -rf ../../tests/matlab/*.m ${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 if ENABLE_INSTALL_WRAP
wrap_install_targets += wrap-install-bin
endif
if ENABLE_INSTALL_MATLAB_TESTS if ENABLE_INSTALL_MATLAB_TESTS
install-exec-hook: wrap-install-bin wrap-install-matlab-tests wrap_install_targets += 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
endif endif
if ENABLE_INSTALL_MATLAB_EXAMPLES
wrap_install_targets += wrap-install-matlab-examples
endif endif
install-exec-hook: ${wrap_install_targets}
# clean local toolbox dir # clean local toolbox dir
clean: clean:
@test -z "wrap" || rm -f wrap @test -z "wrap" || rm -f wrap