Added an implementation of the planarSLAM to gtsam.h using manual includes and namespaces, removed old duplicate header files
Alex Cunningham
parent
9dff4c35bd
commit
c302a50146
74
configure.ac
74
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 \
|
||||
|
|
|
|||
41
gtsam.h
41
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 <gtsam/slam/planarSLAM.h>
|
||||
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 <gtsam/slam/planarSLAM.h>
|
||||
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 <gtsam/slam/planarSLAM.h>
|
||||
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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -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;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -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;
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -71,10 +71,10 @@ namespace gtsam {
|
|||
// Convenience for MATLAB wrapper, which does not allow for identically named methods
|
||||
|
||||
/// get a pose
|
||||
boost::shared_ptr<Pose2> pose(int key) {
|
||||
Pose2 pose = (*this)[PoseKey(key)];
|
||||
return boost::shared_ptr<Pose2>(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<Values> optimize_(const Values& initialEstimate) {
|
||||
/// Optimize
|
||||
Values optimize(const Values& initialEstimate) {
|
||||
typedef NonlinearOptimizer<Graph, Values> Optimizer;
|
||||
boost::shared_ptr<Values> result(
|
||||
new Values(
|
||||
*Optimizer::optimizeLM(*this, initialEstimate,
|
||||
NonlinearOptimizationParameters::LAMBDA)));
|
||||
return result;
|
||||
return *Optimizer::optimizeLM(*this, initialEstimate,
|
||||
NonlinearOptimizationParameters::LAMBDA);
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -58,16 +58,18 @@ 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"
|
||||
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 - 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
|
||||
|
|
@ -82,40 +84,49 @@ else
|
|||
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
|
||||
|
|
|
|||
Loading…
Reference in New Issue